diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index cde987b69c7b41a95af481940cf7b7561e204637..65be5602a8e17b49dca77603899216467aabeae8 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,71 +1,185 @@
-image: segaleran/ceres
+stages:
+ - license
+ - build_and_test
-before_script:
- - ls
+############ YAML ANCHORS ############
+.preliminaries_template: &preliminaries_definition
+ ## Install ssh-agent if not already installed, it is required by Docker.
+ ## (change apt-get to yum if you use an RPM-based image)
+ - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
+
+ ## Run ssh-agent (inside the build environment)
+ - eval $(ssh-agent -s)
+
+ ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
+ ## We're using tr to fix line endings which makes ed25519 keys work
+ ## without extra base64 encoding.
+ ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
+ - mkdir -p ~/.ssh
+ - chmod 700 ~/.ssh
+ - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
+ # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
+ - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
+
+ # update apt
- apt-get update
- - 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
+
+ # push changes (if any)
+ - if git commit -a -m "[skip ci] license headers added or modified" ; then
+ - git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+ - git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
+ - else
+ - echo "No changes, nothing to commit!"
+ - fi
+
+.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
- - 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 ..
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
+ - make -j$(nproc)
- 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
+
+.install_wolfimu_template: &install_wolfimu_definition
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - if [ -d imu ]; then
+ - echo "directory imu exists"
+ - cd imu
+ - git pull
- else
- - echo "directory inexistent"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
+ - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/imu.git
+ - cd imu
- 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_TESTS=OFF ..
+ - make -j$(nproc)
+ - make install
+
+.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 ..
+ - 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:
+ image: labrobotica/wolf_deps:16.04
+ stage: build_and_test
+ cache:
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ - key: imu-xenial
+ paths:
+ - ci_deps/imu/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_wolfimu_definition
+ - ldconfig # update links (shared libraries)
+ script:
+ - *build_and_test_definition
+
+############ UBUNTU 18.04 TESTS ############
+build_and_test:bionic:
+ image: labrobotica/wolf_deps:18.04
+ stage: build_and_test
+ cache:
+ - key: wolf-bionic
+ paths:
+ - ci_deps/wolf/
+ - key: imu-bionic
+ paths:
+ - ci_deps/imu/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_wolfimu_definition
+ - ldconfig # update links (shared libraries)
+ script:
+ - *build_and_test_definition
+
+############ UBUNTU 20.04 TESTS ############
+build_and_test:focal:
+ image: labrobotica/wolf_deps:20.04
+ stage: build_and_test
+ cache:
+ - key: wolf-focal
+ paths:
+ - ci_deps/wolf/
+ - key: imu-focal
+ paths:
+ - ci_deps/imu/
except:
- master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_wolfimu_definition
+ - ldconfig # update links (shared libraries)
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 9fb769bb08cee91fc6f3a70c105eb722e808544a..b2d37d37685bef61e1d3478e941593df4a675be8 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,12 @@ endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
-
# The project name
PROJECT(bodydynamics)
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)
SET(CMAKE_INSTALL_PREFIX /usr/local)
@@ -26,18 +27,14 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
-#Set compiler according C++11 support
+#Set compiler according C++14 support
include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+if(COMPILER_SUPPORTS_CXX14)
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
else()
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
if(UNIX)
@@ -46,9 +43,18 @@ if(UNIX)
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
+IF(NOT BUILD_TESTS)
+ OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(NOT BUILD_DEMOS)
+ OPTION(BUILD_DEMOS "Build demos in demos folder, requires robotpkg-multicontact-api and robotpkg-pinocchio packages" OFF)
+ENDIF(NOT BUILD_DEMOS)
+
+IF(NOT BUILD_DOC)
+ OPTION(BUILD_DOC "Build Documentation" OFF)
+ENDIF(NOT BUILD_DOC)
-#OPTION(BUILD_DOC "Build Documentation" OFF)
-OPTION(BUILD_TESTS "Build Unit tests" ON)
#############
## Testing ##
#############
@@ -61,42 +67,24 @@ if(BUILD_TESTS)
enable_testing()
endif()
-#+START_SRC --------------------------------------------------------------------------------------------------------------------------------
-#Start WOLF build
-MESSAGE("Starting WOLF 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)
-# option(BUILD_EXAMPLES "Build examples" OFF)
-set(BUILD_TESTS true)
-set(BUILD_EXAMPLES false)
-
-# 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)
-
-message("UPPER_NAME ${UPPER_NAME}")
-
+# ============ DEPENDENCIES ============
+FIND_PACKAGE(wolfcore REQUIRED)
+FIND_PACKAGE(wolfimu REQUIRED)
-#find dependencies.
-FIND_PACKAGE(wolf REQUIRED)
-FIND_PACKAGE(wolfIMU 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)
@@ -112,124 +100,93 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
-include_directories("include")
-include_directories(${wolf_INCLUDE_DIRS})
+# ============ INCLUDES ============
+INCLUDE_DIRECTORIES(${wolfimu_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(BEFORE "include")
-#HEADERS
-
-SET(HDRS_COMMON
- )
+# ============ HEADERS ============
SET(HDRS_MATH
include/bodydynamics/math/force_torque_delta_tools.h
)
-SET(HDRS_UTILS
- )
-SET(HDRS_PROBLEM
- )
-SET(HDRS_HARDWARE
- )
-SET(HDRS_TRAJECTORY
- )
-SET(HDRS_MAP
- )
-SET(HDRS_FRAME
- )
-SET(HDRS_STATE_BLOCK
- )
SET(HDRS_CAPTURE
+include/bodydynamics/capture/capture_force_torque_preint.h
+include/bodydynamics/capture/capture_inertial_kinematics.h
+include/bodydynamics/capture/capture_leg_odom.h
+include/bodydynamics/capture/capture_point_feet_nomove.h
)
SET(HDRS_FACTOR
-include/bodydynamics/factor/factor_autodiff_inertial_kinematics.h
include/bodydynamics/factor/factor_force_torque.h
+include/bodydynamics/factor/factor_force_torque_preint.h
+include/bodydynamics/factor/factor_inertial_kinematics.h
+include/bodydynamics/factor/factor_point_feet_nomove.h
)
SET(HDRS_FEATURE
-include/bodydynamics/feature/feature_inertial_kinematics.h
include/bodydynamics/feature/feature_force_torque.h
- )
-SET(HDRS_LANDMARK
+include/bodydynamics/feature/feature_force_torque_preint.h
+include/bodydynamics/feature/feature_inertial_kinematics.h
)
SET(HDRS_PROCESSOR
+include/bodydynamics/processor/processor_force_torque_preint.h
+include/bodydynamics/processor/processor_inertial_kinematics.h
+include/bodydynamics/processor/processor_point_feet_nomove.h
)
SET(HDRS_SENSOR
- )
-SET(HDRS_SOLVER
- )
-SET(HDRS_DTASSC
- )
-SET(HDRS_YAML
+include/bodydynamics/sensor/sensor_force_torque.h
+include/bodydynamics/sensor/sensor_inertial_kinematics.h
+include/bodydynamics/sensor/sensor_point_feet_nomove.h
)
-#SOURCES
-SET(SRCS_PROBLEM
- )
-SET(SRCS_HARDWARE
- )
-SET(SRCS_TRAJECTORY
- )
-SET(SRCS_MAP
- )
-SET(SRCS_FRAME
- )
-SET(SRCS_STATE_BLOCK
- )
-SET(SRCS_COMMON
- )
-SET(SRCS_MATH
- )
-SET(SRCS_UTILS
- )
+# ============ SOURCES ============
SET(SRCS_CAPTURE
- )
-SET(SRCS_FACTOR
- )
+src/capture/capture_force_torque_preint.cpp
+src/capture/capture_inertial_kinematics.cpp
+src/capture/capture_leg_odom.cpp
+src/capture/capture_point_feet_nomove.cpp
+)
SET(SRCS_FEATURE
-src/feature/feature_inertial_kinematics.cpp
src/feature/feature_force_torque.cpp
- )
-SET(SRCS_LANDMARK
+src/feature/feature_force_torque_preint.cpp
+src/feature/feature_inertial_kinematics.cpp
)
SET(SRCS_PROCESSOR
- )
+src/processor/processor_force_torque_preint.cpp
+src/processor/processor_inertial_kinematics.cpp
+src/processor/processor_point_feet_nomove.cpp
+)
SET(SRCS_SENSOR
- )
-SET(SRCS_DTASSC
- )
-SET(SRCS_SOLVER
- )
-SET(SRCS_YAML
+src/sensor/sensor_force_torque.cpp
+src/sensor/sensor_inertial_kinematics.cpp
+src/sensor/sensor_point_feet_nomove.cpp
)
# create the shared library
ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS_CAPTURE}
- ${SRCS_COMMON}
- ${SRCS_DTASSC}
- ${SRCS_FACTOR}
${SRCS_FEATURE}
- ${SRCS_FRAME}
- ${SRCS_HARDWARE}
- ${SRCS_LANDMARK}
- ${SRCS_MAP}
- ${SRCS_MATH}
- ${SRCS_PROBLEM}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
- ${SRCS_SOLVER}
- ${SRCS_STATE_BLOCK}
- ${SRCS_TRAJECTORY}
- ${SRCS_UTILS}
- ${SRCS_WRAPPER}
- ${SRCS_YAML}
)
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARIES})
-# Link the library with the required dependencies
-# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} )
+# Set compiler options
+# ====================
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ message(STATUS "Using C++ compiler clang")
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
+ # using Clang
+elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+ message(STATUS "Using C++ compiler gnu")
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
+ # using GCC
+endif()
+
+#Link the created libraries
+#===============EXAMPLE=========================
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfimu_LIBRARIES})
+
+# Build demos
IF(BUILD_DEMOS)
#Build examples
MESSAGE("Building demos.")
@@ -237,68 +194,36 @@ IF(BUILD_DEMOS)
ENDIF(BUILD_DEMOS)
#Build tests
-#===============EXAMPLE=========================
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_MATH}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
-INSTALL(FILES ${HDRS_UTILS}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/utils)
-INSTALL(FILES ${HDRS_PROBLEM}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/problem)
-INSTALL(FILES ${HDRS_HARDWARE}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/hardware)
-INSTALL(FILES ${HDRS_TRAJECTORY}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/trajectory)
-INSTALL(FILES ${HDRS_MAP}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/map)
-INSTALL(FILES ${HDRS_FRAME}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/frame)
-INSTALL(FILES ${HDRS_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_COMMON}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/common)
-INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association)
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_YAML}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
+INSTALL(FILES ${HDRS_SENSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
FILE(WRITE ${PROJECT_NAME}.found "")
INSTALL(FILES ${PROJECT_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
-
-
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
@@ -306,7 +231,6 @@ INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DES
export(PACKAGE ${PLUGIN_NAME})
-#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
FIND_PACKAGE(Doxygen)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..281d399f13dfd23087acc56303dd38d68162587c
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,619 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+ The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works. By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users. We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors. You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+ To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights. Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received. You must make sure that they, too, receive
+or can get the source code. And you must show them these terms so they
+know their rights.
+
+ Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+ For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software. For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+ Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so. This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software. The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable. Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products. If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+ Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary. To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ TERMS AND CONDITIONS
+
+ 0. Definitions.
+
+ "This License" refers to version 3 of the GNU General Public License.
+
+ "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+ "The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+ To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy. The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+ A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+ To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+ To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+ An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+ 1. Source Code.
+
+ The "source code" for a work means the preferred form of the work
+for making modifications to it. "Object code" means any non-source
+form of a work.
+
+ A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+ The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+ The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+ The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+ The Corresponding Source for a work in source code form is that
+same work.
+
+ 2. Basic Permissions.
+
+ All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+ You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force. You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+ Conveying under any other circumstances is permitted solely under
+the conditions stated below. Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+ No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+ When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+ 4. Conveying Verbatim Copies.
+
+ You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+ You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+ 5. Conveying Modified Source Versions.
+
+ You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified
+ it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is
+ released under this License and any conditions added under section
+ 7. This requirement modifies the requirement in section 4 to
+ "keep intact all notices".
+
+ c) You must license the entire work, as a whole, under this
+ License to anyone who comes into possession of a copy. This
+ License will therefore apply, along with any applicable section 7
+ additional terms, to the whole of the work, and all its parts,
+ regardless of how they are packaged. This License gives no
+ permission to license the work in any other way, but it does not
+ invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display
+ Appropriate Legal Notices; however, if the Program has interactive
+ interfaces that do not display Appropriate Legal Notices, your
+ work need not make them do so.
+
+ A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit. Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+ 6. Conveying Non-Source Forms.
+
+ You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by the
+ Corresponding Source fixed on a durable physical medium
+ customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by a
+ written offer, valid for at least three years and valid for as
+ long as you offer spare parts or customer support for that product
+ model, to give anyone who possesses the object code either (1) a
+ copy of the Corresponding Source for all the software in the
+ product that is covered by this License, on a durable physical
+ medium customarily used for software interchange, for a price no
+ more than your reasonable cost of physically performing this
+ conveying of source, or (2) access to copy the
+ Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the
+ written offer to provide the Corresponding Source. This
+ alternative is allowed only occasionally and noncommercially, and
+ only if you received the object code with such an offer, in accord
+ with subsection 6b.
+
+ d) Convey the object code by offering access from a designated
+ place (gratis or for a charge), and offer equivalent access to the
+ Corresponding Source in the same way through the same place at no
+ further charge. You need not require recipients to copy the
+ Corresponding Source along with the object code. If the place to
+ copy the object code is a network server, the Corresponding Source
+ may be on a different server (operated by you or a third party)
+ that supports equivalent copying facilities, provided you maintain
+ clear directions next to the object code saying where to find the
+ Corresponding Source. Regardless of what server hosts the
+ Corresponding Source, you remain obligated to ensure that it is
+ available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided
+ you inform other peers where the object code and Corresponding
+ Source of the work are being offered to the general public at no
+ charge under subsection 6d.
+
+ A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+ A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling. In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage. For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product. A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+ "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source. The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+ If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+ The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed. Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+ Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+ 7. Additional Terms.
+
+ "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law. If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+ When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+ Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the
+ terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or
+ author attributions in that material or in the Appropriate Legal
+ Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or
+ requiring that modified versions of such material be marked in
+ reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or
+ authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some
+ trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that
+ material by anyone who conveys the material (or modified versions of
+ it) with contractual assumptions of liability to the recipient, for
+ any liability that these contractual assumptions directly impose on
+ those licensors and authors.
+
+ All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10. If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term. If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+ If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+ Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+ 8. Termination.
+
+ You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+ However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+ Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+ Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+ 9. Acceptance Not Required for Having Copies.
+
+ You are not required to accept this License in order to receive or
+run a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+ 10. Automatic Licensing of Downstream Recipients.
+
+ Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+ An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+ You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+ 11. Patents.
+
+ A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+ A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+ Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+ In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+ If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+ If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+ A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License. You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+ Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+ 12. No Surrender of Others' Freedom.
+
+ If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all. For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+ 13. Use with the GNU Affero General Public License.
+
+ Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work. The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+ 14. Revised Versions of this License.
+
+ The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation. If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+ If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+ Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+ 15. Disclaimer of Warranty.
+
+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. Limitation of Liability.
+
+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+ 17. Interpretation of Sections 15 and 16.
+
+ If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
diff --git a/README.md b/README.md
index 7a6bdb0a88395a96d4a97deecf464c691dab1c5d..a57f7feee2fcd5c8e128af029ced7c7fcf31dec4 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
WOLF - Windowed Localization Frames | bodydynamics 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/).
+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/).
-TODO
\ No newline at end of file
+TODO
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
deleted file mode 100644
index 4ff944f2ffccea98faba230f382774a82f6ff2cc..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfConfig.cmake
+++ /dev/null
@@ -1,89 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolfbodydynamics_INCLUDE_DIRS
- NAMES bodydynamics.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
-IF(wolfbodydynamics_INCLUDE_DIRS)
- MESSAGE("Found bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
-ELSE(wolfbodydynamics_INCLUDE_DIRS)
- MESSAGE("Couldn't find bodydynamics include dirs")
-ENDIF(wolfbodydynamics_INCLUDE_DIRS)
-
-FIND_LIBRARY(
- wolfbodydynamics_LIBRARIES
- NAMES libwolfbodydynamics.so libwolfbodydynamics.dylib
- PATHS /usr/local/lib/iri-algorithms)
-IF(wolfbodydynamics_LIBRARIES)
- MESSAGE("Found bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
-ELSE(wolfbodydynamics_LIBRARIES)
- MESSAGE("Couldn't find wolf bodydynamics lib")
-ENDIF(wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
- SET(wolfbodydynamics_FOUND TRUE)
- ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
- set(wolfbodydynamics_FOUND FALSE)
-ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_FOUND)
- IF (NOT wolfbodydynamics_FIND_QUIETLY)
- MESSAGE(STATUS "Found bodydynamics: ${wolfbodydynamics_LIBRARIES}")
- ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
-ELSE (wolfbodydynamics_FOUND)
- IF (wolfbodydynamics_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
- ENDIF (wolfbodydynamics_FIND_REQUIRED)
-ENDIF (wolfbodydynamics_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
- set(wolfbodydynamics_FOUND FALSE)
- unset(wolfbodydynamics_INCLUDE_DIRS)
- unset(wolfbodydynamics_LIBRARIES)
-
- # Reset the CMake module path to its state when this script was called.
- set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
- # FindPackage() use the camelcase library name, not uppercase.
- if (wolfbodydynamics_FIND_QUIETLY)
- message(STATUS "Failed to find wolfbodydynamics- " ${REASON_MSG} ${ARGN})
- else (wolfbodydynamics_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolfbodydynamics - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
- # that prevents generation, but continues configuration.
- message(SEND_ERROR "Failed to find wolfbodydynamics - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfbodydynamics_FOUND)
- wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
-endif(NOT wolfbodydynamics_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfbodydynamics_FOUND TRUE)
-
-# Suppose that our plugin requires openCV & vision_utils
-
-FIND_PACKAGE(vision_utils REQUIRED)
-list(APPEND wolfbodydynamics_INCLUDE_DIRS ${vision_utils_INCLUDE_DIR})
-list(APPEND wolfbodydynamics_LIBRARIES ${vision_utils_LIBRARY})
-
-FIND_PACKAGE(OpenCV REQUIRED)
-list(APPEND wolfbodydynamics_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
-list(APPEND wolfbodydynamics_LIBRARIES ${OpenCV_LIBS})
-
-#Making sure wolf is looked for
-if(NOT wolf_FOUND)
- FIND_PACKAGE(wolf REQUIRED)
-
- #We reverse in order to insert at the start
- list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
- list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
- list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-
- list(REVERSE wolfbodydynamics_LIBRARIES)
- list(APPEND wolfbodydynamics_LIBRARIES ${wolf_LIBRARIES})
- list(REVERSE wolfbodydynamics_LIBRARIES)
-endif()
\ No newline at end of file
diff --git a/cmake_modules/wolfbodydynamicsConfig.cmake b/cmake_modules/wolfbodydynamicsConfig.cmake
index 050a2367341c643d938f88c0ef7438ef7e34a0a9..c1eabae603cf15bca171950f3e8f5937506ec80f 100644
--- a/cmake_modules/wolfbodydynamicsConfig.cmake
+++ b/cmake_modules/wolfbodydynamicsConfig.cmake
@@ -12,7 +12,7 @@ ENDIF(wolfbodydynamics_INCLUDE_DIRS)
FIND_LIBRARY(
wolfbodydynamics_LIBRARIES
NAMES libwolfbodydynamics.so
- PATHS /usr/local/lib/iri-algorithms)
+ PATHS /usr/local/lib)
IF(wolfbodydynamics_LIBRARIES)
MESSAGE("Found wolf bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
ELSE(wolfbodydynamics_LIBRARIES)
@@ -73,14 +73,19 @@ set(wolfbodydynamics_FOUND TRUE)
# Making sure that wolf is always looked for
if(NOT wolf_FOUND)
- FIND_PACKAGE(wolf REQUIRED)
+ FIND_PACKAGE(wolfcore REQUIRED)
#We reverse in order to insert at the start
list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
- list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
+ list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
list(REVERSE wolfbodydynamics_LIBRARIES)
- list(APPEND wolfbodydynamics_LIBRARIES ${wolf_LIBRARIES})
+ list(APPEND wolfbodydynamics_LIBRARIES ${wolfcore_LIBRARIES})
list(REVERSE wolfbodydynamics_LIBRARIES)
-endif()
\ No newline at end of file
+endif()
+
+# provide both INCLUDE_DIR and INCLUDE_DIRS
+SET(wolfbodydynamics_INCLUDE_DIR ${wolfbodydynamics_INCLUDE_DIRS})
+# provide both LIBRARY and LIBRARIES
+SET(wolfbodydynamics_LIBRARY ${wolfbodydynamics_LIBRARIES})
\ No newline at end of file
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e8896eccf78f6f4aef3138b1c6914646c788617c
--- /dev/null
+++ b/demos/CMakeLists.txt
@@ -0,0 +1,73 @@
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals") # necessary for files using boost
+
+FIND_PACKAGE(pinocchio REQUIRED)
+FIND_PACKAGE(multicontact-api REQUIRED)
+
+# SYSTEM disables warnings from library headers
+include_directories(
+ SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
+)
+
+add_library(mcapi_utils mcapi_utils.cpp)
+
+# add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
+# target_link_libraries(mcapi_povcdl_estimation
+# mcapi_utils
+# ${wolfcore_LIBRARIES}
+# ${wolfimu_LIBRARIES}
+# ${PLUGIN_NAME}
+# ${multicontact-api_LIBRARIES}
+# ${pinocchio_LIBRARIES}
+# )
+# target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
+
+
+# add_executable(mcapi_pov_estimation mcapi_pov_estimation.cpp)
+# target_link_libraries(mcapi_pov_estimation
+# mcapi_utils
+# ${wolfcore_LIBRARIES}
+# ${wolfimu_LIBRARIES}
+# ${PLUGIN_NAME}
+# ${multicontact-api_LIBRARIES}
+# ${pinocchio_LIBRARIES}
+# )
+# target_compile_definitions(mcapi_pov_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
+
+add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
+target_link_libraries(solo_real_povcdl_estimation
+ mcapi_utils
+ ${wolfcore_LIBRARIES}
+ ${wolfimu_LIBRARIES}
+ ${PLUGIN_NAME}
+ ${pinocchio_LIBRARIES}
+ /usr/local/lib/libcnpy.so
+ z
+)
+
+add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
+target_link_libraries(solo_real_pov_estimation
+ mcapi_utils
+ ${wolfcore_LIBRARIES}
+ ${wolfimu_LIBRARIES}
+ ${PLUGIN_NAME}
+ ${pinocchio_LIBRARIES}
+ /usr/local/lib/libcnpy.so
+ z
+)
+
+add_executable(solo_mocap_imu solo_mocap_imu.cpp)
+target_link_libraries(solo_mocap_imu
+ ${wolfcore_LIBRARIES}
+ ${wolfimu_LIBRARIES}
+ ${PLUGIN_NAME}
+ ${pinocchio_LIBRARIES}
+ /usr/local/lib/libcnpy.so
+ z
+)
+
+# add_executable(test_cnpy test_cnpy.cpp)
+# target_link_libraries(test_cnpy
+# /usr/local/lib/libcnpy.so
+# )
+
diff --git a/demos/eigenmvn.h b/demos/eigenmvn.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca871ae838a86541b5242d2b2437569650163e53
--- /dev/null
+++ b/demos/eigenmvn.h
@@ -0,0 +1,156 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * Multivariate Normal distribution sampling using C++11 and Eigen matrices.
+ *
+ * This is taken from http://stackoverflow.com/questions/16361226/error-while-creating-object-from-templated-class
+ * (also see http://lost-found-wandering.blogspot.fr/2011/05/sampling-from-multivariate-normal-in-c.html)
+ *
+ * I have been unable to contact the original author, and I've performed
+ * the following modifications to the original code:
+ * - removal of the dependency to Boost, in favor of straight C++11;
+ * - ability to choose from Solver or Cholesky decomposition (supposedly faster);
+ * - fixed Cholesky by using LLT decomposition instead of LDLT that was not yielding
+ * a correctly rotated variance
+ * (see this http://stats.stackexchange.com/questions/48749/how-to-sample-from-a-multivariate-normal-given-the-pt-ldlt-p-decomposition-o )
+ */
+
+/**
+ * Copyright (c) 2014 by Emmanuel Benazera beniz@droidnik.fr, All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3.0 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library.
+ */
+
+#ifndef __EIGENMULTIVARIATENORMAL_HPP
+#define __EIGENMULTIVARIATENORMAL_HPP
+
+#include <Eigen/Dense>
+#include <random>
+
+/*
+ We need a functor that can pretend it's const,
+ but to be a good random number generator
+ it needs mutable state. The standard Eigen function
+ Random() just calls rand(), which changes a global
+ variable.
+*/
+namespace Eigen {
+ namespace internal {
+ template<typename Scalar>
+ struct scalar_normal_dist_op
+ {
+ static std::mt19937 rng; // The uniform pseudo-random algorithm
+ mutable std::normal_distribution<Scalar> norm; // gaussian combinator
+
+ EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)
+
+ template<typename Index>
+ inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
+ inline void seed(const uint64_t &s) { rng.seed(s); }
+ };
+
+ template<typename Scalar>
+ std::mt19937 scalar_normal_dist_op<Scalar>::rng;
+
+ template<typename Scalar>
+ struct functor_traits<scalar_normal_dist_op<Scalar> >
+ { enum { Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
+
+ } // end namespace internal
+
+ /**
+ Find the eigen-decomposition of the covariance matrix
+ and then store it for sampling from a multi-variate normal
+ */
+ template<typename Scalar>
+ class EigenMultivariateNormal
+ {
+ Matrix<Scalar,Dynamic,Dynamic> _covar;
+ Matrix<Scalar,Dynamic,Dynamic> _transform;
+ Matrix< Scalar, Dynamic, 1> _mean;
+ internal::scalar_normal_dist_op<Scalar> randN; // Gaussian functor
+ bool _use_cholesky;
+ SelfAdjointEigenSolver<Matrix<Scalar,Dynamic,Dynamic> > _eigenSolver; // drawback: this creates a useless eigenSolver when using Cholesky decomposition, but it yields access to eigenvalues and vectors
+
+ public:
+ EigenMultivariateNormal(const Matrix<Scalar,Dynamic,1>& mean,const Matrix<Scalar,Dynamic,Dynamic>& covar,
+ const bool use_cholesky=false,const uint64_t &seed=std::mt19937::default_seed)
+ :_use_cholesky(use_cholesky)
+ {
+ randN.seed(seed);
+ setMean(mean);
+ setCovar(covar);
+ }
+
+ void setMean(const Matrix<Scalar,Dynamic,1>& mean) { _mean = mean; }
+ void setCovar(const Matrix<Scalar,Dynamic,Dynamic>& covar)
+ {
+ _covar = covar;
+
+ // Assuming that we'll be using this repeatedly,
+ // compute the transformation matrix that will
+ // be applied to unit-variance independent normals
+
+ if (_use_cholesky)
+ {
+ Eigen::LLT<Eigen::Matrix<Scalar,Dynamic,Dynamic> > cholSolver(_covar);
+ // We can only use the cholesky decomposition if
+ // the covariance matrix is symmetric, pos-definite.
+ // But a covariance matrix might be pos-semi-definite.
+ // In that case, we'll go to an EigenSolver
+ if (cholSolver.info()==Eigen::Success)
+ {
+ // Use cholesky solver
+ _transform = cholSolver.matrixL();
+ }
+ else
+ {
+ throw std::runtime_error("Failed computing the Cholesky decomposition. Use solver instead");
+ }
+ }
+ else
+ {
+ _eigenSolver = SelfAdjointEigenSolver<Matrix<Scalar,Dynamic,Dynamic> >(_covar);
+ _transform = _eigenSolver.eigenvectors()*_eigenSolver.eigenvalues().cwiseMax(0).cwiseSqrt().asDiagonal();
+ }
+ }
+
+ /// Draw nn samples from the gaussian and return them
+ /// as columns in a Dynamic by nn matrix
+ Matrix<Scalar,Dynamic,-1> samples(int nn)
+ {
+ return (_transform * Matrix<Scalar,Dynamic,-1>::NullaryExpr(_covar.rows(),nn,randN)).colwise() + _mean;
+ }
+ }; // end class EigenMultivariateNormal
+} // end namespace Eigen
+#endif
diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fdc2feebbea2a407bd5268d0be86388804e5de3
--- /dev/null
+++ b/demos/mcapi_pov_estimation.cpp
@@ -0,0 +1,751 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 <iostream>
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// MCAPI
+#include <curves/fwd.h>
+#include <curves/so3_linear.h>
+#include <curves/se3_curve.h>
+#include <curves/polynomial.h>
+#include <curves/bezier_curve.h>
+#include <curves/piecewise_curve.h>
+#include <curves/exact_cubic.h>
+#include <curves/cubic_hermite_spline.h>
+#include "multicontact-api/scenario/contact-sequence.hpp"
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/processor/processor_odom_3d.h>
+#include <core/sensor/sensor_odom_3d.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_leg_odom.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_inertial_kinematics.h"
+#include "bodydynamics/factor/factor_force_torque_preint.h"
+
+
+// UTILS
+#include "mcapi_utils.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+using namespace multicontact_api::scenario;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+
+int main (int argc, char **argv) {
+ // retrieve parameters from yaml file
+ YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/mcapi_povcdl_estimation.yaml");
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ double dt = config["dt"].as<double>();
+ double min_t = config["min_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ double solve_every_sec = config["solve_every_sec"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+ bool noisy_measurements = config["noisy_measurements"].as<bool>();
+
+ // estimator sensor noises
+ double std_pbc_est = config["std_pbc_est"].as<double>();
+ double std_vbc_est = config["std_vbc_est"].as<double>();
+ double std_f_est = config["std_f_est"].as<double>();
+ double std_tau_est = config["std_tau_est"].as<double>();
+ double std_odom3d_est = config["std_odom3d_est"].as<double>();
+
+ // simulated sensor noises
+ double std_acc_sim = config["std_acc_sim"].as<double>();
+ double std_gyr_sim = config["std_gyr_sim"].as<double>();
+ double std_pbc_sim = config["std_pbc_sim"].as<double>();
+ double std_vbc_sim = config["std_vbc_sim"].as<double>();
+ double std_f_sim = config["std_f_sim"].as<double>();
+ double std_tau_sim = config["std_tau_sim"].as<double>();
+ // double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
+
+ // priors
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+
+ double fz_thresh = config["fz_thresh"].as<double>();
+ double scale_dist = config["scale_dist"].as<double>();
+ bool base_dist_only = config["base_dist_only"].as<bool>();
+ bool mass_dist = config["mass_dist"].as<bool>();
+ bool vbc_is_dist = config["vbc_is_dist"].as<bool>();
+ bool Iw_is_dist = config["Iw_is_dist"].as<bool>();
+ bool Lgest_is_dist = config["Lgest_is_dist"].as<bool>();
+
+ std::string contact_sequence_file = config["contact_sequence_file"].as<std::string>();
+
+ ContactSequence cs;
+ std::cout << "Loading file " << contact_sequence_file << std::endl;
+ cs.loadFromBinary(contact_sequence_file);
+
+ auto q_traj = cs.concatenateQtrajectories();
+ auto dq_traj = cs.concatenateDQtrajectories();
+ auto ddq_traj = cs.concatenateDDQtrajectories();
+ auto c_traj = cs.concatenateCtrajectories();
+ auto dc_traj = cs.concatenateDCtrajectories();
+ auto L_traj = cs.concatenateLtrajectories();
+ auto tau_traj = cs.concatenateTauTrajectories();
+ std::vector<std::string> ee_names = cs.getAllEffectorsInContact();
+ unsigned int nbc = ee_names.size();
+ std::vector<curves::piecewise_t> cforce_traj_lst;
+ for (auto ee_name: ee_names){
+ std::cout << ee_name << std::endl;
+ auto cforce_traj = cs.concatenateContactForceTrajectories(ee_name);
+ cforce_traj_lst.push_back(cforce_traj);
+ }
+
+ if (min_t < 0){
+ min_t = q_traj.min();
+ }
+ if (max_t < 0){
+ max_t = q_traj.max();
+ }
+
+ // initialize some vectors and fill them with dicretized data
+ std::vector<double> t_arr; // timestamps
+ std::vector<VectorXd> q_traj_arr; // [p_ob, q_ob, qA]
+ std::vector<VectorXd> dq_traj_arr; // [spvel_b, qA_dot]
+ std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
+ std::vector<VectorXd> c_traj_arr; // w_p_wc
+ std::vector<VectorXd> dc_traj_arr; // w_v_wc
+ std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
+ std::vector<VectorXd> tau_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
+ std::vector<VectorXd> bp_traj_arr; // bias on b_p_bc: bias on the CoM position expressed in base frame
+ std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
+ int N = (max_t - min_t)/dt;
+ int i_t = 0;
+ for (double t=min_t; t <= max_t; t += dt){
+ t_arr.push_back(t);
+ q_traj_arr.push_back(q_traj(t));
+ dq_traj_arr.push_back(dq_traj(t));
+ ddq_traj_arr.push_back(ddq_traj(t));
+ c_traj_arr.push_back(c_traj(t));
+ dc_traj_arr.push_back(dc_traj(t));
+ L_traj_arr.push_back(L_traj(t));
+ tau_traj_arr.push_back(tau_traj(t));
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ cforce_traj_arr_lst[i_ee].push_back(cforce_traj_lst[i_ee](t));
+ }
+ i_t++;
+ }
+ std::cout << "Trajectory extracted, proceed to create sensor data" << std::endl;
+
+ // Creating measurements from simulated trajectory
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ Data data(model);
+
+ // distorted model (modified inertias)
+ Model model_dist;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model_dist);
+
+ Eigen::EigenMultivariateNormal<double> noise_mass = Eigen::EigenMultivariateNormal<double>(Vector1d::Zero(), Matrix1d::Identity(), false);
+ Eigen::EigenMultivariateNormal<double> noise_lever = Eigen::EigenMultivariateNormal<double>(Vector3d::Zero(), Matrix3d::Identity(), false);
+
+ // distortion
+ if (base_dist_only){
+ if (!mass_dist){
+ int root_joint_id = model.getJointId("root_joint");
+ Vector3d lever_dist = model.inertias[root_joint_id].lever() + scale_dist * noise_lever.samples(1);
+ model_dist.inertias[root_joint_id] = pinocchio::Inertia(model.inertias[root_joint_id].mass(), lever_dist, model.inertias[root_joint_id].inertia());
+ }
+ }
+ else{
+ for (int i=0; i < model_dist.inertias.size(); i++){
+ if (mass_dist){
+ double mass_dist = model.inertias[i].mass() + scale_dist * model.inertias[i].mass() * noise_mass.samples(1)(0,0);
+ model_dist.inertias[i] = pinocchio::Inertia(mass_dist, model.inertias[i].lever(), model.inertias[i].inertia());
+ std::cout << "mass model " << model.inertias[i].mass() << std::endl;
+ std::cout << "mass model_dist " << model_dist.inertias[i].mass() << std::endl;
+ }
+ else {
+ Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_lever.samples(1);
+ model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
+ }
+ }
+ }
+ Data data_dist(model_dist);
+
+ std::vector<int> ee_ids;
+ std::cout << "Frame ids" << std::endl;
+ for (auto ee_name: ee_names){
+ ee_ids.push_back(model.getFrameId(ee_name));
+ std::cout << ee_name << std::endl;
+ }
+ std::vector<Vector3d> contacts = contacts_from_footrect_center();
+
+ ///////////////////////////////////////////////
+ // Create some vectors to aggregate the groundtruth and measurements
+ // Groundtruth
+ std::vector<Vector3d> p_ob_gtr_v;
+ std::vector<Vector4d> q_ob_gtr_v;
+ std::vector<Vector3d> v_ob_gtr_v;
+ // std::vector<VectorXd>& c_gtr_v = c_traj_arr; // already have it directly
+ // std::vector<VectorXd>& cd_gtr_v = dc_traj_arr; // already have it directly
+ // std::vector<Vector3d> L_traj_arr; // already have it directly
+
+
+ // Measurements
+ std::vector<Vector3d> acc_b_meas_v;
+ std::vector<Vector3d> w_b_meas_v;
+ std::vector<std::vector<Vector3d>> p_bl_meas_v(nbc);
+ std::vector<std::vector<Vector4d>> q_bl_meas_v(nbc);
+ std::vector<std::vector<VectorXd>> wrench_meas_v(nbc);
+ std::vector<Vector3d> p_bc_meas_v;
+ std::vector<Vector3d> v_bc_meas_v;
+ std::vector<Vector3d> Lq_meas_v;
+ std::vector<Matrix3d> Iq_meas_v;
+ // define vectors to aggregate ground truth and simulated data
+ for (unsigned int i = 0; i < q_traj_arr.size(); i++)
+ {
+ /**
+ * Groundtruth to recover (in world frame, vels/inertial frame):
+ * b position --> OK
+ * b orientation (quat) --> OK
+ * b velocity: linear classical one --> OK
+ * CoM posi --> OK
+ * CoM vel --> OK
+ * Angular momentum --> OK
+ *
+ * Measures to recover (in local frames):
+ * IMU readings --> OK
+ * Kinematics readings --> OK
+ * Wrench readings --> OK
+ * CoM relative position/vel --> OK
+ * Iq, Lq --> OK
+ *
+ **/
+
+ // retrieve traj data
+ VectorXd q = q_traj_arr[i];
+ VectorXd dq = dq_traj_arr[i];
+ VectorXd ddq = ddq_traj_arr[i];
+ // Vector3d c = c_traj_arr[i]; // gtr
+ // VectorXd dc = dc_traj_arr[i]; // gtr
+ // VectorXd L = L_traj_arr[i]; // gtr
+ VectorXd tau = tau_traj_arr[i];
+
+ // std::cout << "q " << q.transpose() << std::endl;
+ // std::cout << "dq " << dq.transpose() << std::endl;
+ // std::cout << "ddq " << ddq.transpose() << std::endl;
+
+ //////////////////////////////////
+ //////////////////////////////////
+ // Set ground truth variables
+ Vector3d p_wb = q.head<3>(); // gtr
+ Vector4d quat_wb = q.segment<4>(3); // gtr
+ SE3 oTb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
+ // body vel is expressed in body frame
+ Vector3d b_v_b = dq.head<3>(); // vsp: spatial linear velocity
+ Vector3d b_w_b = dq.segment<3>(3); // meas
+ Vector3d b_asp_b = ddq.head<3>(); // body spatial acceleration in body plucker frame
+
+ Vector3d w_v_wb = oTb.rotation() * b_v_b; // gtr
+
+ ////////////////////////////////////
+ ////////////////////////////////////
+ // Get measurements
+
+ // IMU readings
+ // We use every quantity (spatial acc, body angvel, dr=lin spatial linvel) in the same body frame -> we get acc in body frame
+ Vector3d b_acc = b_asp_b + b_w_b.cross(b_v_b);
+ // universe o frame in mcapi/pinocchio is supposed to have same reference point and orientation as WOLF world frame
+ Vector3d b_proper_acc = b_acc - oTb.rotation().transpose() * gravity(); // meas
+
+ // update and retrieve kinematics
+ forwardKinematics(model, data, q);
+ updateFramePlacements(model, data);
+
+ // compute all linear jacobians (only for quadruped)
+ MatrixXd Jlinvel_all(12, model.nv); Jlinvel_all.setZero();
+ for (int i_ee=0; i_ee < nbc; i_ee++){
+ MatrixXd Jee(6, model.nv); Jee.setZero();
+ computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
+ Jlinvel_all.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
+ }
+ // remove free flyer (not needed for force reconstruction)
+ MatrixXd Jlinvel_noff = Jlinvel_all.rightCols(model.nv-6);
+
+ // estimate forces from torques
+ VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
+ VectorXd tau_joints = tau_cf.tail(model.nv-6) - tau; // remove measured joint torque (not on the free flyer)
+ VectorXd forces = Jlinvel_noff.transpose().lu().solve(tau_joints);
+
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ auto oTl = data.oMf[ee_ids[i_ee]];
+ auto bTl = oTb.inverse() * oTl;
+ Vector3d b_p_l = bTl.translation(); // meas
+ Vector4d b_qvec_l = Quaterniond(bTl.rotation()).coeffs(); // meas
+ p_bl_meas_v[i_ee].push_back(b_p_l);
+ q_bl_meas_v[i_ee].push_back(b_qvec_l);
+ // TODO: Only for 6D wrench or 3D force
+
+ // Compute either from torques or from given force and compare
+ // std::cout << forces.segment(3*i_ee, 3).transpose() - cforce_traj_arr_lst[i_ee][i].transpose() << std::endl;
+ // wrench_meas_v[i_ee].push_back(cforce_traj_arr_lst[i_ee][i]);
+ wrench_meas_v[i_ee].push_back(forces.segment(3*i_ee, 3));
+ }
+
+
+ //////////////////////////////////////////////
+ // COM relative position and velocity measures
+ VectorXd q_static = q;
+ VectorXd dq_static = dq;
+ q_static.head<7>() << 0,0,0, 0,0,0,1;
+ dq_static.head<6>() << 0,0,0, 0,0,0;
+ // compute for both perfect and distorted models to get the bias
+ centerOfMass(model, data, q_static, dq_static);
+ centerOfMass(model_dist, data_dist, q_static, dq_static);
+
+ // c =def w_p_wc
+ // Vector3d b_p_bc = oTb.inverse().act(c); // meas
+ Vector3d b_p_bc = data.com[0]; // meas
+ Vector3d b_p_bc_dist = data_dist.com[0]; // meas
+ Vector3d bias_pbc = b_p_bc_dist - b_p_bc;
+
+ // velocity due to to gesticulation only in base frame
+ // -> set world frame = base frame and set body frame spatial vel to zero
+ Vector3d b_v_bc = data.vcom[0]; // meas
+ Vector3d b_v_bc_dist = data_dist.vcom[0]; // meas
+ Vector3d bias_vbc = b_v_bc_dist - b_v_bc;
+
+ /////////////////////////
+ // Centroidal inertia and gesticulation kinetic momentum
+ Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
+ Matrix3d b_Iw_dist = compute_Iw(model_dist, data_dist, q_static, b_p_bc_dist);
+ Matrix3d bias_Iw = b_Iw_dist - b_Iw;
+
+ // gesticulation in base frame: just compute centroidal momentum with static q and vq
+ Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
+ Vector3d b_Lc_gesti_dist = computeCentroidalMomentum(model_dist, data_dist, q_static, dq_static).angular();
+ Vector3d bias_Lc = b_Lc_gesti_dist - b_Lc_gesti;
+
+
+ // std::cout << "bias_pbc: " << bias_pbc.transpose() << std::endl;
+ // std::cout << "bias_vbc: " << bias_vbc.transpose() << std::endl;
+ // std::cout << "bias_Iw: \n" << bias_Iw << std::endl;
+ // std::cout << "bias_Lc: " << bias_Lc.transpose() << std::endl;
+
+ /////////////////////////
+ // Agreggate everything in vectors for easier WOLF consumption
+ p_ob_gtr_v.push_back(p_wb);
+ q_ob_gtr_v.push_back(quat_wb);
+ v_ob_gtr_v.push_back(w_v_wb);
+ bp_traj_arr.push_back(bias_pbc);
+ // Lc_gtr_v;
+
+ // Measurements
+ acc_b_meas_v.push_back(b_proper_acc);
+ w_b_meas_v.push_back(b_w_b);
+ p_bc_meas_v.push_back(b_p_bc_dist);
+ if (vbc_is_dist){
+ v_bc_meas_v.push_back(b_v_bc_dist);
+ }
+ else {
+ v_bc_meas_v.push_back(b_v_bc);
+ }
+ if (Lgest_is_dist){
+ Lq_meas_v.push_back(b_Lc_gesti_dist);
+ }
+ else{
+ Lq_meas_v.push_back(b_Lc_gesti);
+ }
+ if (Iw_is_dist){
+ Iq_meas_v.push_back(b_Iw_dist);
+ }
+ else {
+ Iq_meas_v.push_back(b_Iw);
+ }
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("POV", 3);
+
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_mcapi_demo.yaml");
+ ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
+
+ // SENSOR + PROCESSOR ODOM3D FOR LEG ODOMETRY
+ Vector7d extr_senlegodom; extr_senlegodom << 0,0,0, 0,0,0,1; // not used in practice
+ SensorBasePtr sen_odom3d_base = problem->installSensor("SensorOdom3d", "SenLegOdom", extr_senlegodom, bodydynamics_root_dir + "/demos/sensor_odom_3d.yaml");
+ SensorOdom3dPtr sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen_odom3d_base);
+ ParamsProcessorOdom3dPtr params_sen_odom3d = std::make_shared<ParamsProcessorOdom3d>();
+ // ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", sen_odom3d, params_sen_odom3d);
+ ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", "SenLegOdom", bodydynamics_root_dir + "/demos/processor_odom_3d.yaml");
+ ProcessorOdom3dPtr proc_legodom = std::static_pointer_cast<ProcessorOdom3d>(proc_legodom_base);
+ //////////////////////////////
+
+ TimeStamp t0(t_arr[0]);
+ VectorComposite x_origin("POV", {p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0]});
+ VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+ FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
+
+ proc_imu->setOrigin(KF1);
+ proc_legodom->setOrigin(KF1);
+
+
+ ///////////////////////////////////////////
+ // process measurements in sequential order
+ double ts_since_last_kf = 0;
+
+ // Add gaussian noises
+ std::time_t epoch = std::time(nullptr);
+ int64_t seed = (int64_t)epoch;
+ Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc_sim, 2)*Matrix3d::Identity(), false, seed);
+ Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr_sim, 2)*Matrix3d::Identity(), false, seed);
+
+ //////////////////////////////////////////
+ // Vectors to store estimates at the time of data processing
+ // fbk -> feedback: to check if the estimation is stable enough for feedback control
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
+ //////////////////////////////////////////
+
+ unsigned int nb_kf = problem->getTrajectory()->getFrameMap().size();
+ for (unsigned int i=0; i < t_arr.size(); i++){
+ // TimeStamp ts(t_arr[i]+dt); // works best with tsid trajectories
+ TimeStamp ts(t_arr[i]); // works best with pyb trajectories
+
+ // IMU
+ Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
+
+ if (noisy_measurements){
+ acc_gyr_meas.head<3>() += noise_acc.samples(1);
+ acc_gyr_meas.tail<3>() += noise_gyr.samples(1);
+ }
+
+ Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
+
+ ////////////////////
+ /////////////////
+
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CImu->process();
+
+
+ if (i > 0){
+ // Leg odometry
+ // 1) detect feet in contact
+ std::vector<int> feet_int_contact;
+ for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // basic contact detection based on normal foot vel, things break if no foot is in contact
+ // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
+ if (wrench_meas_v[i_ee][i].norm() > fz_thresh){
+ feet_int_contact.push_back(i_ee);
+ }
+ }
+ // 2) fill the leg odometry data matrix, depending on the contact dimension
+ Eigen::MatrixXd data_legodom;
+ if (dimc == 6){
+ // cols organized as:
+ // [pbl1_km; qbl1_k], [pbl2_km; qbl2_k], ...
+ data_legodom.resize(7,feet_int_contact.size()*2);
+ data_legodom.setZero();
+ int j = 0;
+ for (int i_ee_c: feet_int_contact){
+ data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1], q_bl_meas_v[i_ee_c][i-1];
+ data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ], q_bl_meas_v[i_ee_c][i];
+ j++;
+ }
+ }
+ else if (dimc == 3){
+ // cols organized as:
+ // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
+ data_legodom.resize(3,feet_int_contact.size()*2 + 1);
+ data_legodom.setZero();
+ int j = 0;
+ for (int i_ee_c: feet_int_contact){
+ data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1];
+ data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ];
+ j++;
+ }
+ data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
+ }
+ // std::cout << "data_legodom\n" << data_legodom << std::endl;
+ // 3) process the data and its cov
+ Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
+ CLO->process();
+ }
+
+ // if new KF add
+ // unsigned int new_kf_nb = problem->getTrajectory()->getFrameMap().size();
+ // if (new_kf_nb > nb_kf){
+ // auto last_kf = problem->getTrajectory()->getFrameMap().rbegin()->second;
+ // nb_kf = new_kf_nb;
+ // // ADD ABSOLUTE FACTOR (GPS LIKE)
+ // }
+
+
+ ts_since_last_kf += dt;
+ if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
+ // problem->print(4,true,true,true);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+ ts_since_last_kf = 0;
+ }
+
+ // Store current state estimation
+ VectorComposite curr_state = problem->getState(ts);
+ p_ob_fbk_v.push_back(curr_state['P']);
+ q_ob_fbk_v.push_back(curr_state['O']);
+ v_ob_fbk_v.push_back(curr_state['V']);
+
+ }
+
+ ////////////////////////////////////////////
+ // BIAS FACTORS
+ // Add absolute factor on Imu biases to simulate a fix()
+ Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+
+
+ ///////////////////////////////////////////
+ //////////////// SOLVING //////////////////
+ ///////////////////////////////////////////
+ problem->print(4,true,true,true);
+ if (solve_end){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
+ std::cout << report << std::endl;
+ }
+
+ //////////////////////////////////////
+ //////////////////////////////////////
+ // STORE DATA //
+ //////////////////////////////////////
+ //////////////////////////////////////
+ std::fstream file_gtr;
+ std::fstream file_est;
+ std::fstream file_fbk;
+ std::fstream file_kfs;
+ std::fstream file_cov;
+ file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
+ file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
+ file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
+ file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
+ file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
+ std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
+ file_est << header_est;
+ file_fbk << header_est;
+ std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz\n";
+ file_kfs << header_kfs;
+ file_gtr << header_kfs;
+ std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz\n";
+ file_cov << header_cov;
+
+ VectorComposite state_est;
+ for (unsigned int i=0; i < t_arr.size(); i++) {
+ file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << p_ob_gtr_v[i](0) << ","
+ << p_ob_gtr_v[i](1) << ","
+ << p_ob_gtr_v[i](2) << ","
+ << q_ob_gtr_v[i](0) << ","
+ << q_ob_gtr_v[i](1) << ","
+ << q_ob_gtr_v[i](2) << ","
+ << q_ob_gtr_v[i](3) << ","
+ << v_ob_gtr_v[i](0) << ","
+ << v_ob_gtr_v[i](1) << ","
+ << v_ob_gtr_v[i](2)
+ << "\n";
+ }
+
+ for (unsigned int i=0; i < t_arr.size(); i++) {
+ state_est = problem->getState(t_arr[i]);
+ file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << state_est['P'](0) << ","
+ << state_est['P'](1) << ","
+ << state_est['P'](2) << ","
+ << state_est['O'](0) << ","
+ << state_est['O'](1) << ","
+ << state_est['O'](2) << ","
+ << state_est['O'](3) << ","
+ << state_est['V'](0) << ","
+ << state_est['V'](1) << ","
+ << state_est['V'](2)
+ << "\n";
+ }
+
+ for (unsigned int i=0; i < t_arr.size(); i++) {
+ file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << p_ob_fbk_v[i](0) << ","
+ << p_ob_fbk_v[i](1) << ","
+ << p_ob_fbk_v[i](2) << ","
+ << q_ob_fbk_v[i](0) << ","
+ << q_ob_fbk_v[i](1) << ","
+ << q_ob_fbk_v[i](2) << ","
+ << q_ob_fbk_v[i](3) << ","
+ << v_ob_fbk_v[i](0) << ","
+ << v_ob_fbk_v[i](1) << ","
+ << v_ob_fbk_v[i](2)
+ << "\n";
+ }
+
+ VectorComposite kf_state;
+ CaptureBasePtr cap_imu;
+ VectorComposite bi_bias_est;
+ for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ auto kf = elt.second;
+ kf_state = kf->getState();
+ cap_imu = kf->getCaptureOf(sen_imu);
+ bi_bias_est = cap_imu->getState();
+
+ file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << kf->getTimeStamp().get() << ","
+ << kf_state['P'](0) << ","
+ << kf_state['P'](1) << ","
+ << kf_state['P'](2) << ","
+ << kf_state['O'](0) << ","
+ << kf_state['O'](1) << ","
+ << kf_state['O'](2) << ","
+ << kf_state['O'](3) << ","
+ << kf_state['V'](0) << ","
+ << kf_state['V'](1) << ","
+ << kf_state['V'](2) << ","
+ << bi_bias_est['I'](0) << ","
+ << bi_bias_est['I'](1) << ","
+ << bi_bias_est['I'](2) << ","
+ << bi_bias_est['I'](3) << ","
+ << bi_bias_est['I'](4) << ","
+ << bi_bias_est['I'](5)
+ << "\n";
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+ problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+ problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+ file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << kf->getTimeStamp().get() << ","
+ << Qp(0,0) << ","
+ << Qp(1,1) << ","
+ << Qp(2,2) << ","
+ << Qo(0,0) << ","
+ << Qo(1,1) << ","
+ << Qo(2,2) << ","
+ << Qv(0,0) << ","
+ << Qv(1,1) << ","
+ << Qv(2,2) << ","
+ << Qbi(0,0) << ","
+ << Qbi(1,1) << ","
+ << Qbi(2,2) << ","
+ << Qbi(3,3) << ","
+ << Qbi(4,4) << ","
+ << Qbi(5,5)
+ << "\n";
+ }
+
+ file_gtr.close();
+ file_est.close();
+ file_fbk.close();
+ file_kfs.close();
+ file_cov.close();
+
+
+ // // Print factor energy
+ // for (auto kf: problem->getTrajectory()->getFrameMap()){
+ // for (auto cap: kf->getCaptureList()){
+ // for (auto feat: cap->getFeatureList()){
+ // for (auto fac: feat->getFactorList()){
+ // if (fac->getType() == "FactorForceTorquePreint"){
+ // std::cout << "Type: FactorForceTorquePreint" << std::endl;
+ // auto fac_ftp = std::static_pointer_cast<FactorForceTorquePreint>(fac);
+ // std::cout << "try the cost" << std::endl;
+ // std::cout << fac_ftp->cost() << std::endl;
+ // }
+ // }
+ // }
+ // }
+ // }
+
+
+}
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82bf7d64e2c9779a17a590fc7b9a6cf2b58cded1
--- /dev/null
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -0,0 +1,969 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 <iostream>
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// MCAPI
+#include <curves/fwd.h>
+#include <curves/so3_linear.h>
+#include <curves/se3_curve.h>
+#include <curves/polynomial.h>
+#include <curves/bezier_curve.h>
+#include <curves/piecewise_curve.h>
+#include <curves/exact_cubic.h>
+#include <curves/cubic_hermite_spline.h>
+#include "multicontact-api/scenario/contact-sequence.hpp"
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/processor/processor_odom_3d.h>
+#include <core/sensor/sensor_odom_3d.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_leg_odom.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_inertial_kinematics.h"
+#include "bodydynamics/factor/factor_force_torque_preint.h"
+
+
+// UTILS
+#include "mcapi_utils.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+using namespace multicontact_api::scenario;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+
+int main (int argc, char **argv) {
+ // retrieve parameters from yaml file
+ YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/mcapi_povcdl_estimation.yaml");
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ double dt = config["dt"].as<double>();
+ double min_t = config["min_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ double solve_every_sec = config["solve_every_sec"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+ bool noisy_measurements = config["noisy_measurements"].as<bool>();
+
+ // estimator sensor noises
+ double std_pbc_est = config["std_pbc_est"].as<double>();
+ double std_vbc_est = config["std_vbc_est"].as<double>();
+ double std_f_est = config["std_f_est"].as<double>();
+ double std_tau_est = config["std_tau_est"].as<double>();
+ double std_odom3d_est = config["std_odom3d_est"].as<double>();
+
+ // simulated sensor noises
+ double std_acc_sim = config["std_acc_sim"].as<double>();
+ double std_gyr_sim = config["std_gyr_sim"].as<double>();
+ double std_pbc_sim = config["std_pbc_sim"].as<double>();
+ double std_vbc_sim = config["std_vbc_sim"].as<double>();
+ double std_f_sim = config["std_f_sim"].as<double>();
+ double std_tau_sim = config["std_tau_sim"].as<double>();
+ // double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
+
+ // priors
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ std::vector<double> bias_pbc_prior_vec = config["bias_pbc_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> bias_pbc_prior(bias_pbc_prior_vec.data());
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+
+ double fz_thresh = config["fz_thresh"].as<double>();
+ double scale_dist = config["scale_dist"].as<double>();
+ bool base_dist_only = config["base_dist_only"].as<bool>();
+ bool mass_dist = config["mass_dist"].as<bool>();
+ bool vbc_is_dist = config["vbc_is_dist"].as<bool>();
+ bool Iw_is_dist = config["Iw_is_dist"].as<bool>();
+ bool Lgest_is_dist = config["Lgest_is_dist"].as<bool>();
+
+ std::string contact_sequence_file = config["contact_sequence_file"].as<std::string>();
+
+ ContactSequence cs;
+ std::cout << "Loading file " << contact_sequence_file << std::endl;
+ cs.loadFromBinary(contact_sequence_file);
+
+ auto q_traj = cs.concatenateQtrajectories();
+ auto dq_traj = cs.concatenateDQtrajectories();
+ auto ddq_traj = cs.concatenateDDQtrajectories();
+ auto c_traj = cs.concatenateCtrajectories();
+ auto dc_traj = cs.concatenateDCtrajectories();
+ auto L_traj = cs.concatenateLtrajectories();
+ auto tau_traj = cs.concatenateTauTrajectories();
+ std::vector<std::string> ee_names = cs.getAllEffectorsInContact();
+ unsigned int nbc = ee_names.size();
+ std::vector<curves::piecewise_t> cforce_traj_lst;
+ for (auto ee_name: ee_names){
+ std::cout << ee_name << std::endl;
+ auto cforce_traj = cs.concatenateContactForceTrajectories(ee_name);
+ cforce_traj_lst.push_back(cforce_traj);
+ }
+
+ if (min_t < 0){
+ min_t = q_traj.min();
+ }
+ if (max_t < 0){
+ max_t = q_traj.max();
+ }
+
+ // initialize some vectors and fill them with dicretized data
+ std::vector<double> t_arr; // timestamps
+ std::vector<VectorXd> q_traj_arr; // [p_ob, q_ob, qA]
+ std::vector<VectorXd> dq_traj_arr; // [spvel_b, qA_dot]
+ std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
+ std::vector<VectorXd> c_traj_arr; // w_p_wc
+ std::vector<VectorXd> dc_traj_arr; // w_v_wc
+ std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
+ std::vector<VectorXd> tau_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
+ std::vector<VectorXd> bp_traj_arr; // bias on b_p_bc: bias on the CoM position expressed in base frame
+ std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
+ int N = (max_t - min_t)/dt;
+ int i_t = 0;
+ for (double t=min_t; t <= max_t; t += dt){
+ t_arr.push_back(t);
+ q_traj_arr.push_back(q_traj(t));
+ dq_traj_arr.push_back(dq_traj(t));
+ ddq_traj_arr.push_back(ddq_traj(t));
+ c_traj_arr.push_back(c_traj(t));
+ dc_traj_arr.push_back(dc_traj(t));
+ L_traj_arr.push_back(L_traj(t));
+ tau_traj_arr.push_back(tau_traj(t));
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ cforce_traj_arr_lst[i_ee].push_back(cforce_traj_lst[i_ee](t));
+ }
+ i_t++;
+ }
+ std::cout << "Trajectory extracted, proceed to create sensor data" << std::endl;
+
+ // Creating measurements from simulated trajectory
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ Data data(model);
+
+ // distorted model (modified inertias)
+ Model model_dist;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model_dist);
+ Eigen::EigenMultivariateNormal<double> noise_mass = Eigen::EigenMultivariateNormal<double>(Vector1d::Zero(), Matrix1d::Identity(), false);
+ Eigen::EigenMultivariateNormal<double> noise_lever = Eigen::EigenMultivariateNormal<double>(Vector3d::Zero(), Matrix3d::Identity(), false);
+ if (base_dist_only){
+ if (!mass_dist){
+ int root_joint_id = model.getJointId("root_joint");
+ Vector3d lever_dist = model.inertias[root_joint_id].lever() + scale_dist * noise_lever.samples(1);
+ model_dist.inertias[root_joint_id] = pinocchio::Inertia(model.inertias[root_joint_id].mass(), lever_dist, model.inertias[root_joint_id].inertia());
+ }
+ }
+ else{
+ for (int i=0; i < model_dist.inertias.size(); i++){
+ if (mass_dist){
+ double mass_dist = model.inertias[i].mass() + scale_dist * model.inertias[i].mass() * noise_mass.samples(1)(0,0);
+ model_dist.inertias[i] = pinocchio::Inertia(mass_dist, model.inertias[i].lever(), model.inertias[i].inertia());
+ std::cout << "mass model " << model.inertias[i].mass() << std::endl;
+ std::cout << "mass model_dist " << model_dist.inertias[i].mass() << std::endl;
+ }
+ else {
+ Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_lever.samples(1);
+ model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
+ }
+ }
+ }
+ Data data_dist(model_dist);
+
+ // Recover end effector frame ids
+ std::vector<int> ee_ids;
+ std::cout << "Frame ids" << std::endl;
+ for (auto ee_name: ee_names){
+ ee_ids.push_back(model.getFrameId(ee_name));
+ std::cout << ee_name << std::endl;
+ }
+ // std::vector<Vector3d> contacts = contacts_from_footrect_center(); // for Humanoid only TODO
+
+
+ ///////////////////////////////////////////////
+ // Create some vectors to aggregate the groundtruth and measurements
+ // Groundtruth
+ std::vector<Vector3d> p_ob_gtr_v;
+ std::vector<Vector4d> q_ob_gtr_v;
+ std::vector<Vector3d> v_ob_gtr_v;
+ // std::vector<VectorXd>& c_gtr_v = c_traj_arr; // already have it directly
+ // std::vector<VectorXd>& cd_gtr_v = dc_traj_arr; // already have it directly
+ // std::vector<Vector3d> L_traj_arr; // already have it directly
+
+ // Measurements
+ std::vector<Vector3d> acc_b_meas_v;
+ std::vector<Vector3d> w_b_meas_v;
+ std::vector<std::vector<Vector3d>> p_bl_meas_v(nbc);
+ std::vector<std::vector<Vector4d>> q_bl_meas_v(nbc);
+ std::vector<std::vector<VectorXd>> wrench_meas_v(nbc);
+ std::vector<Vector3d> p_bc_meas_v;
+ std::vector<Vector3d> v_bc_meas_v;
+ std::vector<Vector3d> Lq_meas_v;
+ std::vector<Matrix3d> Iq_meas_v;
+ // define vectors to aggregate ground truth and simulated data
+ for (unsigned int i = 0; i < q_traj_arr.size(); i++)
+ {
+ /**
+ * Groundtruth to recover (in world frame, vels/inertial frame):
+ * b position --> OK
+ * b orientation (quat) --> OK
+ * b velocity: linear classical one --> OK
+ * CoM posi --> OK
+ * CoM vel --> OK
+ * Angular momentum --> OK
+ *
+ * Measures to recover (in local frames):
+ * IMU readings --> OK
+ * Kinematics readings --> OK
+ * Wrench readings --> OK
+ * CoM relative position/vel --> OK
+ * Iq, Lq --> OK
+ *
+ **/
+
+ // retrieve traj data
+ VectorXd q = q_traj_arr[i];
+ VectorXd dq = dq_traj_arr[i];
+ VectorXd ddq = ddq_traj_arr[i];
+ // Vector3d c = c_traj_arr[i]; // gtr
+ // VectorXd dc = dc_traj_arr[i]; // gtr
+ // VectorXd L = L_traj_arr[i]; // gtr
+ VectorXd tau = tau_traj_arr[i];
+
+ // std::cout << "q " << q.transpose() << std::endl;
+ // std::cout << "dq " << dq.transpose() << std::endl;
+ // std::cout << "ddq " << ddq.transpose() << std::endl;
+
+ //////////////////////////////////
+ //////////////////////////////////
+ // Set ground truth variables
+ Vector3d p_wb = q.head<3>(); // gtr
+ Vector4d quat_wb = q.segment<4>(3); // gtr
+ SE3 oTb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
+ // body vel is expressed in body frame
+ Vector3d b_v_b = dq.head<3>(); // vsp: spatial linear velocity
+ Vector3d b_w_b = dq.segment<3>(3); // meas
+ Vector3d b_asp_b = ddq.head<3>(); // body spatial acceleration in body plucker frame
+
+ Vector3d w_v_wb = oTb.rotation() * b_v_b; // gtr
+
+ ////////////////////////////////////
+ ////////////////////////////////////
+ // Get measurements
+
+ // IMU readings
+ // We use every quantity (spatial acc, body angvel, dr=lin spatial linvel) in the same body frame -> we get acc in body frame
+ Vector3d b_acc = b_asp_b + b_w_b.cross(b_v_b);
+ // universe o frame in mcapi/pinocchio is supposed to have same reference point and orientation as WOLF world frame
+ Vector3d b_proper_acc = b_acc - oTb.rotation().transpose() * gravity(); // meas
+
+ // update and retrieve kinematics
+ forwardKinematics(model, data, q);
+ updateFramePlacements(model, data);
+
+ // compute all linear jacobians (only for quadruped)
+ MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
+ for (int i_ee=0; i_ee < nbc; i_ee++){
+ MatrixXd Jee(6, model.nv); Jee.setZero();
+ computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
+ Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
+ }
+
+ // estimate forces from torques
+ VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
+ tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
+
+ // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
+ // Solve in Least square sense (3 ways):
+ // VectorXd forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
+ VectorXd forces = Jlinvel.transpose().colPivHouseholderQr().solve(tau_cf); // QR
+ // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
+
+ Vector3d o_f_tot = Vector3d::Zero();
+ std::cout << "" << std::endl;
+ std::cout << "t: " << t_arr[i] << std::endl;
+ // std::cout << "ddq: " << ddq.transpose() << std::endl;
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ auto oTl = data.oMf[ee_ids[i_ee]];
+ auto bTl = oTb.inverse() * oTl;
+ Vector3d b_p_l = bTl.translation(); // meas
+ Vector4d b_qvec_l = Quaterniond(bTl.rotation()).coeffs(); // meas
+ p_bl_meas_v[i_ee].push_back(b_p_l);
+ q_bl_meas_v[i_ee].push_back(b_qvec_l);
+ // TODO: Only for 6D wrench or 3D force
+
+ // Compute either from torques or from given force and compare
+ std::cout << "f_force_from_tau - f_force_from_cs_file: " << forces.segment(3*i_ee, 3).transpose() - cforce_traj_arr_lst[i_ee][i].transpose() << std::endl;
+
+ Vector3d l_f_l = forces.segment(3*i_ee, 3);
+ // wrench_meas_v[i_ee].push_back(cforce_traj_arr_lst[i_ee][i]); // EITHER
+ wrench_meas_v[i_ee].push_back(l_f_l);
+
+ Vector3d o_f_l = oTl.rotation() * l_f_l;
+ // std::cout << "o_p_" << ee_names[i_ee] << " " << oTl.translation().transpose() << std::endl;
+ // std::cout << "o_f_" << ee_names[i_ee] << " " << o_f_l.transpose() << std::endl;
+ o_f_tot += o_f_l;
+ }
+ // std::cout << "o_f_tot: " << o_f_tot.transpose() << std::endl;
+ std::cout << "o_f_tot + m*g: " << (o_f_tot + data.mass[0]*model.gravity.linear()).transpose() << std::endl;
+ std::cout << "pin.gravity - wolf gravity: " << model.gravity.linear() - wolf::gravity() << std::endl;
+
+
+ //////////////////////////////////////////////
+ // COM relative position and velocity measures
+ VectorXd q_static = q;
+ VectorXd dq_static = dq;
+ q_static.head<7>() << 0,0,0, 0,0,0,1;
+ dq_static.head<6>() << 0,0,0, 0,0,0;
+ // compute for both perfect and distorted models to get the bias
+ centerOfMass(model, data, q_static, dq_static);
+ centerOfMass(model_dist, data_dist, q_static, dq_static);
+
+ // c =def w_p_wc
+ // Vector3d b_p_bc = oTb.inverse().act(c); // meas
+ Vector3d b_p_bc = data.com[0]; // meas
+ Vector3d b_p_bc_dist = data_dist.com[0]; // meas
+ Vector3d bias_pbc = b_p_bc_dist - b_p_bc;
+
+ // velocity due to to gesticulation only in base frame
+ // -> set world frame = base frame and set body frame spatial vel to zero
+ Vector3d b_v_bc = data.vcom[0]; // meas
+ Vector3d b_v_bc_dist = data_dist.vcom[0]; // meas
+ Vector3d bias_vbc = b_v_bc_dist - b_v_bc;
+
+ /////////////////////////
+ // Centroidal inertia and gesticulation kinetic momentum
+ Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
+ Matrix3d b_Iw_dist = compute_Iw(model_dist, data_dist, q_static, b_p_bc_dist);
+ Matrix3d bias_Iw = b_Iw_dist - b_Iw;
+
+ // gesticulation in base frame: just compute centroidal momentum with static q and vq
+ Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
+ Vector3d b_Lc_gesti_dist = computeCentroidalMomentum(model_dist, data_dist, q_static, dq_static).angular();
+ Vector3d bias_Lc = b_Lc_gesti_dist - b_Lc_gesti;
+
+
+ // std::cout << "bias_pbc: " << bias_pbc.transpose() << std::endl;
+ // std::cout << "bias_vbc: " << bias_vbc.transpose() << std::endl;
+ // std::cout << "bias_Iw: \n" << bias_Iw << std::endl;
+ // std::cout << "bias_Lc: " << bias_Lc.transpose() << std::endl;
+
+ /////////////////////////
+ // Agreggate everything in vectors for easier WOLF consumption
+ p_ob_gtr_v.push_back(p_wb);
+ q_ob_gtr_v.push_back(quat_wb);
+ v_ob_gtr_v.push_back(w_v_wb);
+ bp_traj_arr.push_back(bias_pbc);
+ // Lc_gtr_v;
+
+ // Measurements
+ acc_b_meas_v.push_back(b_proper_acc);
+ w_b_meas_v.push_back(b_w_b);
+ p_bc_meas_v.push_back(b_p_bc_dist);
+ if (vbc_is_dist){
+ v_bc_meas_v.push_back(b_v_bc_dist);
+ }
+ else {
+ v_bc_meas_v.push_back(b_v_bc);
+ }
+ if (Lgest_is_dist){
+ Lq_meas_v.push_back(b_Lc_gesti_dist);
+ }
+ else{
+ Lq_meas_v.push_back(b_Lc_gesti);
+ }
+ if (Iw_is_dist){
+ Iq_meas_v.push_back(b_Iw_dist);
+ }
+ else {
+ Iq_meas_v.push_back(b_Iw);
+ }
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("POVCDL", 3);
+
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR INERTIAL KINEMATICS
+ ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
+ intr_ik->std_pbc = std_pbc_est;
+ intr_ik->std_vbc = std_vbc_est;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
+ // SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ SensorInertialKinematicsPtr sen_ikin = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
+
+ ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->std_bp_drift = std_bp_drift;
+ ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin, params_ik);
+ // ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
+ ProcessorInertialKinematicsPtr proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
+
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_mcapi_demo.yaml");
+ ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
+
+ // SENSOR + PROCESSOR FT PREINT
+ ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
+ intr_ft->std_f = std_f_est;
+ intr_ft->std_tau = std_tau_est;
+ intr_ft->mass = data.mass[0];
+ std::cout << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << "\n\nROBOT MASS: " << intr_ft->mass << std::endl;
+ SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
+ // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
+ SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
+ ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
+ params_sen_ft->sensor_ikin_name = "SenIK";
+ params_sen_ft->sensor_angvel_name = "SenImu";
+ params_sen_ft->nbc = nbc;
+ params_sen_ft->dimc = dimc;
+ params_sen_ft->time_tolerance = 0.0005;
+ params_sen_ft->unmeasured_perturbation_std = 0.000001;
+ params_sen_ft->max_time_span = 1000;
+ params_sen_ft->max_buff_length = 500;
+ params_sen_ft->dist_traveled = 20000.0;
+ params_sen_ft->angle_turned = 1000;
+ params_sen_ft->voting_active = false;
+ ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
+ ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
+
+ // SENSOR + PROCESSOR ODOM3D FOR LEG ODOMETRY
+ Vector7d extr_senlegodom; extr_senlegodom << 0,0,0, 0,0,0,1; // not used in practice
+ SensorBasePtr sen_odom3d_base = problem->installSensor("SensorOdom3d", "SenLegOdom", extr_senlegodom, bodydynamics_root_dir + "/demos/sensor_odom_3d.yaml");
+ SensorOdom3dPtr sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen_odom3d_base);
+ ParamsProcessorOdom3dPtr params_sen_odom3d = std::make_shared<ParamsProcessorOdom3d>();
+ // ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", sen_odom3d, params_sen_odom3d);
+ ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", "SenLegOdom", bodydynamics_root_dir + "/demos/processor_odom_3d.yaml");
+ ProcessorOdom3dPtr proc_legodom = std::static_pointer_cast<ProcessorOdom3d>(proc_legodom_base);
+ //////////////////////////////
+
+
+ // SETPRIOR RETRO-ENGINEERING
+ // We are not using setPrior because of processors initial captures problems so we have to
+ // - Manually create the first KF and its pose and velocity priors
+ // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
+ TimeStamp t0(t_arr[0]);
+ VectorXd x_origin; x_origin.resize(19);
+ x_origin << p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0], c_traj_arr[0], dc_traj_arr[0], L_traj_arr[0];
+ MatrixXd P_origin(9,9); P_origin.setIdentity();
+ P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity();
+ P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity();
+ P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity();
+ FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
+ // Prior pose factor
+ CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
+ pose_prior_capture->emplaceFeatureAndFactor();
+ // Prior velocity factor
+ CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1, "Vel0", t0);
+ FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.segment<3>(7), P_origin.block<3, 3>(6, 6));
+ FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1->getV(), nullptr, false);
+
+ // Special trick to make things work in the IMU + IKin + FTPreint case
+ // 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0
+ // 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
+ // 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
+ // 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
+ proc_inkin->keyFrameCallback(KF1, 0.0005);
+ proc_imu->setOrigin(KF1);
+ proc_legodom->setOrigin(KF1);
+
+ // INERTIAL KINEMATICS CAPTURE DATA ZERO MOTION
+ // momentum parameters
+ Matrix<double, 9, 1> meas_ikin; meas_ikin << p_bc_meas_v[0], v_bc_meas_v[0], w_b_meas_v[0];
+ auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, Iq_meas_v[0], Lq_meas_v[0]);
+ CIKin0->process();
+ proc_ftpreint->setOrigin(KF1);
+
+
+ ////////////////////////////////////////////
+ // INITIAL BIAS FACTORS
+ // Add absolute factor on Imu biases to simulate a fix()
+ Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+
+ // Add loose absolute factor on initial bp bias since dynamical trajectories
+ Matrix3d Q_bp = pow(std_abs_bias_pbc, 2)* Matrix3d::Identity();
+ CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
+ FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
+
+
+ ///////////////////////////////////////////
+ // process measurements in sequential order
+ // SE3 oTb_prev(Quaterniond(q_ob_gtr_v[0]).toRotationMatrix(), p_ob_gtr_v[0]);
+ // FrameBasePtr KF_prev = KF1;
+ double ts_since_last_kf = 0;
+
+ // Add gaussian noises
+ std::time_t epoch = std::time(nullptr);
+ int64_t seed = (int64_t)epoch;
+ Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc_sim, 2)*Matrix3d::Identity(), false, seed);
+ Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr_sim, 2)*Matrix3d::Identity(), false, seed);
+ Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc_sim, 2)*Matrix3d::Identity(), false, seed);
+ Eigen::EigenMultivariateNormal<double> noise_vbc(Vector3d::Zero(), pow(std_vbc_sim, 2)*Matrix3d::Identity(), false, seed);
+ Eigen::EigenMultivariateNormal<double> noise_f (Vector3d::Zero(), pow(std_f_sim, 2)*Matrix3d::Identity(), false, seed);
+ Eigen::EigenMultivariateNormal<double> noise_tau(Vector3d::Zero(), pow(std_tau_sim, 2)*Matrix3d::Identity(), false, seed);
+
+ //////////////////////////////////////////
+ // Vectors to store estimates at the time of data processing
+ // fbk -> feedback: to check if the estimation is stable enough for feedback control
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
+ std::vector<Vector3d> c_ob_fbk_v;
+ std::vector<Vector3d> cd_ob_fbk_v;
+ std::vector<Vector3d> Lc_ob_fbk_v;
+ //////////////////////////////////////////
+
+ for (unsigned int i=0; i < t_arr.size(); i++){
+ // TimeStamp ts(t_arr[i]+dt); // works best with tsid trajectories
+ TimeStamp ts(t_arr[i]); // works best with pyb trajectories
+
+ // IMU
+ Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
+
+ if (noisy_measurements){
+ acc_gyr_meas.head<3>() += noise_acc.samples(1);
+ acc_gyr_meas.tail<3>() += noise_gyr.samples(1);
+ }
+
+ Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
+
+ // Inertial kinematics
+ meas_ikin << p_bc_meas_v[i], v_bc_meas_v[i], w_b_meas_v[i];
+ // meas_ikin << p_bc_meas_v[ii], v_bc_meas_v[ii], w_b_meas_v[ii];
+ // meas_ikin << p_bc_meas_v[ii+1], v_bc_meas_v[ii+1], w_b_meas_v[ii+1];
+
+ if (noisy_measurements){
+ meas_ikin.segment<3>(0) += noise_pbc.samples(1);
+ meas_ikin.segment<3>(3) += noise_vbc.samples(1);
+ }
+
+ ////////////////////
+ // Force Torque
+ ////////////////////
+
+
+ VectorXd f_meas(nbc*3);
+ VectorXd tau_meas(nbc*3);
+ VectorXd kin_meas(nbc*7);
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ if (dimc == 3){
+ f_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i];
+ }
+ else if (dimc == 6){
+ f_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i].head<3>();
+ tau_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i].tail<3>();
+ }
+ kin_meas.segment<3>(i_ee*3) = p_bl_meas_v[i_ee][i];
+ kin_meas.segment<4>(nbc*3 + i_ee*4) = q_bl_meas_v[i_ee][i];
+ }
+
+ if (noisy_measurements){
+ f_meas.segment<3>(0) += noise_f.samples(1);
+ f_meas.segment<3>(3) += noise_f.samples(1);
+ tau_meas.segment<3>(0) += noise_tau.samples(1);
+ tau_meas.segment<3>(3) += noise_tau.samples(1);
+ }
+
+ VectorXd cap_ftp_data(dimc*nbc+3+3+nbc*7);
+ MatrixXd Qftp(dimc*nbc+3+3,dimc*nbc+3+3); // kin not in covariance mat
+ if (dimc == 3){
+ cap_ftp_data << f_meas, p_bc_meas_v[i], w_b_meas_v[i], kin_meas;
+ Qftp.setIdentity();
+ Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
+ Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc_est, 2);
+ Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
+ }
+ if (dimc == 6){
+ cap_ftp_data << f_meas, tau_meas, p_bc_meas_v[i], w_b_meas_v[i], kin_meas;
+ Qftp.setIdentity();
+ Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
+ Qftp.block(nbc*3,nbc*3, nbc*3,nbc*3) *= pow(std_tau_est, 2);
+ Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc_est, 2);
+ Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
+ }
+ ////////////////////
+ /////////////////
+
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CImu->process();
+ auto CIKin = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, Iq_meas_v[i], Lq_meas_v[i]);
+ CIKin->process();
+ auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
+ CFTpreint->process();
+
+
+
+ if (i > 0){
+ // Leg odometry
+ // 1) detect feet in contact
+ std::vector<int> feet_in_contact;
+ for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // basic contact detection based on normal foot vel, things break if no foot is in contact
+ if (wrench_meas_v[i_ee][i].norm() > fz_thresh){
+ feet_in_contact.push_back(i_ee);
+ }
+ }
+ // 2) fill the leg odometry data matrix, depending on the contact dimension
+ Eigen::MatrixXd data_legodom;
+ if (dimc == 6){
+ // cols organized as:
+ // [pbl1_km; qbl1_k], [pbl2_km; qbl2_k], ...
+ data_legodom.resize(7,feet_in_contact.size()*2);
+ data_legodom.setZero();
+ int j = 0;
+ for (int i_ee_c: feet_in_contact){
+ data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1], q_bl_meas_v[i_ee_c][i-1];
+ data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ], q_bl_meas_v[i_ee_c][i];
+ j++;
+ }
+ }
+ else if (dimc == 3){
+ // cols organized as:
+ // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
+ data_legodom.resize(3,feet_in_contact.size()*2 + 1);
+ data_legodom.setZero();
+ int j = 0;
+ for (int i_ee_c: feet_in_contact){
+ data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1];
+ data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ];
+ j++;
+ }
+ data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
+ }
+ // std::cout << "data_legodom\n" << data_legodom << std::endl;
+ // 3) process the data and its cov
+ Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
+ CLO->process();
+ }
+
+ ts_since_last_kf += dt;
+ if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
+ // problem->print(4,true,true,true);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+ ts_since_last_kf = 0;
+ }
+
+
+ // Store current state estimation
+ VectorComposite curr_state = problem->getState(ts);
+ p_ob_fbk_v.push_back(curr_state['P']);
+ q_ob_fbk_v.push_back(curr_state['O']);
+ v_ob_fbk_v.push_back(curr_state['V']);
+ c_ob_fbk_v.push_back(curr_state['C']);
+ cd_ob_fbk_v.push_back(curr_state['D']);
+ Lc_ob_fbk_v.push_back(curr_state['L']);
+ }
+
+
+
+ ///////////////////////////////////////////
+ //////////////// SOLVING //////////////////
+ ///////////////////////////////////////////
+ problem->print(4,true,true,true);
+ if (solve_end){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
+ std::cout << report << std::endl;
+ }
+
+ //////////////////////////////////////
+ //////////////////////////////////////
+ // STORE DATA //
+ //////////////////////////////////////
+ //////////////////////////////////////
+ std::fstream file_gtr;
+ std::fstream file_est;
+ std::fstream file_fbk;
+ std::fstream file_kfs;
+ std::fstream file_cov;
+ std::fstream file_wre;
+ file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
+ file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
+ file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
+ file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
+ file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
+ file_wre.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/wre.csv", std::fstream::out);
+ std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lcx,Lcy,Lcz\n";
+ file_est << header_est;
+ file_fbk << header_est;
+ std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz,cx,cy,cz,cdx,cdy,cdz,Lcx,Lcy,Lcz,bpx,bpy,bpz\n";
+ file_kfs << header_kfs;
+ file_gtr << header_kfs; // at the same frequency as "est" but ground truth biases added for comparison with kfs
+ std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLcx,QLcy,QLcz,Qbpx,Qbpy,Qbpz\n";
+ file_cov << header_cov;
+
+ for (unsigned int i=0; i < t_arr.size(); i++) {
+ // TODO: change if simulate a non zero imu bias
+ file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << p_ob_gtr_v[i](0) << ","
+ << p_ob_gtr_v[i](1) << ","
+ << p_ob_gtr_v[i](2) << ","
+ << q_ob_gtr_v[i](0) << ","
+ << q_ob_gtr_v[i](1) << ","
+ << q_ob_gtr_v[i](2) << ","
+ << q_ob_gtr_v[i](3) << ","
+ << v_ob_gtr_v[i](0) << ","
+ << v_ob_gtr_v[i](1) << ","
+ << v_ob_gtr_v[i](2) << ","
+ << 0.0 << ","
+ << 0.0 << ","
+ << 0.0 << ","
+ << 0.0 << ","
+ << 0.0 << ","
+ << 0.0 << ","
+ << c_traj_arr[i](0) << ","
+ << c_traj_arr[i](1) << ","
+ << c_traj_arr[i](2) << ","
+ << dc_traj_arr[i](0) << ","
+ << dc_traj_arr[i](1) << ","
+ << dc_traj_arr[i](2) << ","
+ << L_traj_arr[i](0) << ","
+ << L_traj_arr[i](1) << ","
+ << L_traj_arr[i](2) << ","
+ << bp_traj_arr[i](0) << ","
+ << bp_traj_arr[i](1) << ","
+ << bp_traj_arr[i](2)
+ << std::endl;
+ }
+
+ VectorComposite state_est;
+ for (unsigned int i=0; i < t_arr.size(); i++) {
+ state_est = problem->getState(t_arr[i]);
+ file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << state_est['P'](0) << ","
+ << state_est['P'](1) << ","
+ << state_est['P'](2) << ","
+ << state_est['O'](0) << ","
+ << state_est['O'](1) << ","
+ << state_est['O'](2) << ","
+ << state_est['O'](3) << ","
+ << state_est['V'](0) << ","
+ << state_est['V'](1) << ","
+ << state_est['V'](2) << ","
+ << state_est['C'](0) << ","
+ << state_est['C'](1) << ","
+ << state_est['C'](2) << ","
+ << state_est['D'](0) << ","
+ << state_est['D'](1) << ","
+ << state_est['D'](2) << ","
+ << state_est['L'](0) << ","
+ << state_est['L'](1) << ","
+ << state_est['L'](2)
+ << "\n";
+ }
+
+ for (unsigned int i=0; i < t_arr.size(); i++) {
+ file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << p_ob_fbk_v[i](0) << ","
+ << p_ob_fbk_v[i](1) << ","
+ << p_ob_fbk_v[i](2) << ","
+ << q_ob_fbk_v[i](0) << ","
+ << q_ob_fbk_v[i](1) << ","
+ << q_ob_fbk_v[i](2) << ","
+ << q_ob_fbk_v[i](3) << ","
+ << v_ob_fbk_v[i](0) << ","
+ << v_ob_fbk_v[i](1) << ","
+ << v_ob_fbk_v[i](2) << ","
+ << c_ob_fbk_v[i](0) << ","
+ << c_ob_fbk_v[i](1) << ","
+ << c_ob_fbk_v[i](2) << ","
+ << cd_ob_fbk_v[i](0) << ","
+ << cd_ob_fbk_v[i](1) << ","
+ << cd_ob_fbk_v[i](2) << ","
+ << Lc_ob_fbk_v[i](0) << ","
+ << Lc_ob_fbk_v[i](1) << ","
+ << Lc_ob_fbk_v[i](2)
+ << "\n";
+ }
+
+ VectorComposite kf_state;
+ CaptureBasePtr cap_ikin;
+ VectorComposite bp_bias_est;
+ CaptureBasePtr cap_imu;
+ VectorComposite bi_bias_est;
+ for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ auto kf = elt.second;
+ kf_state = kf->getState();
+ cap_ikin = kf->getCaptureOf(sen_ikin);
+ bp_bias_est = cap_ikin->getState();
+ cap_imu = kf->getCaptureOf(sen_imu);
+ bi_bias_est = cap_imu->getState();
+
+ file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << kf->getTimeStamp().get() << ","
+ << kf_state['P'](0) << ","
+ << kf_state['P'](1) << ","
+ << kf_state['P'](2) << ","
+ << kf_state['O'](0) << ","
+ << kf_state['O'](1) << ","
+ << kf_state['O'](2) << ","
+ << kf_state['O'](3) << ","
+ << kf_state['V'](0) << ","
+ << kf_state['V'](1) << ","
+ << kf_state['V'](2) << ","
+ << bi_bias_est['I'](0) << ","
+ << bi_bias_est['I'](1) << ","
+ << bi_bias_est['I'](2) << ","
+ << bi_bias_est['I'](3) << ","
+ << bi_bias_est['I'](4) << ","
+ << bi_bias_est['I'](5) << ","
+ << kf_state['C'](0) << ","
+ << kf_state['C'](1) << ","
+ << kf_state['C'](2) << ","
+ << kf_state['D'](0) << ","
+ << kf_state['D'](1) << ","
+ << kf_state['D'](2) << ","
+ << kf_state['L'](0) << ","
+ << kf_state['L'](1) << ","
+ << kf_state['L'](2) << ","
+ << bp_bias_est['I'](0) << ","
+ << bp_bias_est['I'](1) << ","
+ << bp_bias_est['I'](2)
+ << "\n";
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd QL = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
+ // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended...
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+ problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+ problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+ problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
+ problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
+ problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
+
+ solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
+ // std::cout << "Qbp\n" << Qbp << std::endl;
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+ file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << kf->getTimeStamp().get() << ","
+ << Qp(0,0) << ","
+ << Qp(1,1) << ","
+ << Qp(2,2) << ","
+ << Qo(0,0) << ","
+ << Qo(1,1) << ","
+ << Qo(2,2) << ","
+ << Qv(0,0) << ","
+ << Qv(1,1) << ","
+ << Qv(2,2) << ","
+ << Qbi(0,0) << ","
+ << Qbi(1,1) << ","
+ << Qbi(2,2) << ","
+ << Qbi(3,3) << ","
+ << Qbi(4,4) << ","
+ << Qbi(5,5) << ","
+ << Qc(0,0) << ","
+ << Qc(1,1) << ","
+ << Qc(2,2) << ","
+ << Qd(0,0) << ","
+ << Qd(1,1) << ","
+ << Qd(2,2) << ","
+ << QL(0,0) << ","
+ << QL(1,1) << ","
+ << QL(2,2) << ","
+ << Qbp(0,0) << ","
+ << Qbp(1,1) << ","
+ << Qbp(2,2)
+ << "\n";
+ }
+
+ file_gtr.close();
+ file_est.close();
+ file_kfs.close();
+ file_cov.close();
+ file_wre.close();
+
+
+}
diff --git a/demos/mcapi_povcdl_estimation.yaml b/demos/mcapi_povcdl_estimation.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..28b57f008d929631d65099e27a746a82c049c3da
--- /dev/null
+++ b/demos/mcapi_povcdl_estimation.yaml
@@ -0,0 +1,76 @@
+# trajectory handling
+dt: 0.001
+min_t: -1.0 # -1 means from the beginning of the trajectory
+max_t: -1 # -1 means until the end of the trajectory
+solve_every_sec: -1 # < 0 strict --> no solve
+solve_end: False
+# solve_every_sec: 0.3 # < 0 strict --> no solve
+# solve_end: True
+
+# estimator sensor noises
+std_acc_est: 0.001
+std_gyr_est: 0.001
+std_pbc_est: 0.001
+std_vbc_est: 0.001 # higher than simulated -> has to take care of the non-modeled bias as well... Good value for solo sin traj
+std_f_est: 0.001
+std_tau_est: 0.001
+# std_odom3d_est: 0.000001
+std_odom3d_est: 100000
+
+# simulated sensor noises
+std_acc_sim: 0.0001
+std_gyr_sim: 0.0001
+std_pbc_sim: 0.0001
+std_vbc_sim: 0.0001
+std_f_sim: 0.0001
+std_tau_sim: 0.0001
+std_odom3d_sim: 100000000
+
+# some controls
+fz_thresh: 1
+noisy_measurements: False
+
+# priors
+std_prior_p: 0.01
+std_prior_o: 0.1
+std_prior_v: 0.1
+
+# std_abs_biasimu: 100000
+std_abs_biasimu: 0.0000001 # almost fixed
+std_abs_bias_pbc: 10000 # noprior
+# std_abs_bias_pbc: 0.00001 # almost fixed
+std_bp_drift: 10000 # All the drift you want
+# std_bp_drift: 0.1 # no drift
+
+bias_pbc_prior: [0,0,0]
+
+
+# model disturbance
+scale_dist: 0.00 # disturbance of link inertia
+mass_dist: False
+base_dist_only: False
+
+# which measurement/parameter is disturbed?
+vbc_is_dist: False
+Iw_is_dist: False
+Lgest_is_dist: False
+
+
+# Robot model
+# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/talos_data/robots/talos_reduced.urdf"
+# dimc: 6
+robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
+dimc: 3
+
+
+#Â Trajectory files
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_sin_traj.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_contact_switch.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj.cs"
+contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj_pyb.cs"
+
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_notrunk.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+y.cs"
+
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+z.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping.cs"
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d893b4669ce2514f3f15b3bf73500f243923e19c
--- /dev/null
+++ b/demos/mcapi_utils.cpp
@@ -0,0 +1,77 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "mcapi_utils.h"
+
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+{
+ return ddr + w.cross(dr);
+}
+
+
+std::vector<Vector3d> contacts_from_footrect_center()
+{
+ // compute feet corners coordinates like from the leg_X_6_joint
+ double lx = 0.1;
+ double ly = 0.065;
+ double lz = 0.107;
+ std::vector<Vector3d> contacts; contacts.resize(4);
+ contacts[0] = {-lx, -ly, -lz};
+ contacts[1] = {-lx, ly, -lz};
+ contacts[2] = { lx, -ly, -lz};
+ contacts[3] = { lx, ly, -lz};
+ return contacts;
+}
+
+
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
+ const Matrix<double, 12, 1>& cf12)
+{
+ Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
+ Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
+ + contacts[1].cross(cf12.segment<3>(3))
+ + contacts[2].cross(cf12.segment<3>(6))
+ + contacts[3].cross(cf12.segment<3>(9));
+ Matrix<double, 6, 1> wrench; wrench << f, tau;
+ return wrench;
+}
+
+
+Matrix3d compute_Iw(pinocchio::Model& model,
+ pinocchio::Data& data,
+ VectorXd& q_static,
+ Vector3d& b_p_bc)
+{
+ MatrixXd b_Mc = pinocchio::crba(model, data, q_static); // mass matrix at b frame expressed in b frame
+ // MatrixXd b_Mc = crba(model_dist, data_dist, q_static); // mass matrix at b frame expressed in b frame
+ MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>(); // inertia matrix at b frame expressed in b frame
+ pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc); // "no free flyer robot -> c frame oriented the same as b"
+ pinocchio::SE3 cTb = bTc.inverse();
+ // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc
+ // c_I_c block diagonal:
+ // [m*Id3, 03;
+ // 0, Iw]
+ MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
+ Matrix3d b_Iw = c_I_c.bottomRightCorner<3,3>(); // meas
+ return b_Iw;
+}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..c62c4026a1dac41733350f2b96fb554285f30715
--- /dev/null
+++ b/demos/mcapi_utils.h
@@ -0,0 +1,76 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 <vector>
+#include "Eigen/Dense"
+#include "pinocchio/algorithm/crba.hpp"
+
+
+using namespace Eigen;
+
+
+/**
+ * \brief Compute a 3D acceleration from 6D spatial acceleration
+ *
+ * \param ddr spatial acc linear part
+ * \param w spatial acc rotational part
+ * \param dr spatial velocity linear part
+**/
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
+
+
+/**
+* \brief Compute the relative contact points from foot center of a Talos foot.
+*
+* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
+* Expressed in local foot coordinates.
+**/
+std::vector<Vector3d> contacts_from_footrect_center();
+
+
+/**
+* \brief Compute the wrench at the end effector center expressed in world coordinates.
+
+* Each contact force (at a foot for instance) is represented in MAPI as a set of
+* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
+* as the other quantities, are expressed in world coordinates. From this 4 3D forces,
+* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
+
+* \param contacts std vector of 3D contact forces
+* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
+* \param wRl rotation matrix, orientation from world frame to foot frame
+**/
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
+ const Matrix<double, 12, 1>& cf12);
+
+
+/**
+* \brief Compute the centroidal angular inertia used to compute the angular momentum.
+
+* \param model: pinocchio robot model used
+* \param data: pinocchio robot data used
+* \param q_static: configuration of the robot with free flyer pose at origin (local base frame = world frame)
+* \param b_p_bc: measure of the CoM position relative to the base
+**/
+Matrix3d compute_Iw(pinocchio::Model& model,
+ pinocchio::Data& data,
+ VectorXd& q_static,
+ Vector3d& b_p_bc);
diff --git a/demos/processor_ft_preint.yaml b/demos/processor_ft_preint.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d9b49d70b860fe2e3029a4cc9bd62ffcc2fdc6c8
--- /dev/null
+++ b/demos/processor_ft_preint.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorForceTorquePreint" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "PFTPreint" # This is ignored. The name provided to the SensorFactory prevails
+time_tolerance: 0.0005 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0000000
+sensor_ikin_name: "SenIK"
+sensor_angvel_name: "SenImu"
+keyframe_vote:
+ max_time_span: 100000000000 # seconds
+ max_buff_length: 50000 # motion deltas
+ dist_traveled: 20000.0 # meters
+ angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg)
+ voting_active: false
\ No newline at end of file
diff --git a/demos/processor_imu_mcapi_demo.yaml b/demos/processor_imu_mcapi_demo.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..38105baa1a5a6b582f67919fc9299d6eb05bfe52
--- /dev/null
+++ b/demos/processor_imu_mcapi_demo.yaml
@@ -0,0 +1,11 @@
+type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
+time_tolerance: 0.0005 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.000001
+keyframe_vote:
+ max_time_span: 0.29999 # seconds
+ max_buff_length: 100000000000 # motion deltas
+ dist_traveled: 20000.0 # meters
+ angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg)
+ voting_active: true
+ voting_aux_active: false
\ No newline at end of file
diff --git a/demos/processor_imu_solo12.yaml b/demos/processor_imu_solo12.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f92b74c06167c96ef4294e00600c7270bd0d20e2
--- /dev/null
+++ b/demos/processor_imu_solo12.yaml
@@ -0,0 +1,10 @@
+keyframe_vote:
+ angle_turned: 1000
+ dist_traveled: 20000.0
+ max_buff_length: 100000000000
+ max_time_span: 0.19999
+ voting_active: true
+name: Main imu
+time_tolerance: 0.0005
+type: ProcessorImu
+unmeasured_perturbation_std: 1.0e-06
diff --git a/demos/processor_inertial_kinematics.yaml b/demos/processor_inertial_kinematics.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..64c0d60f2f1b2e7f1af0b9e570bbcec1473c0207
--- /dev/null
+++ b/demos/processor_inertial_kinematics.yaml
@@ -0,0 +1,5 @@
+type: "ProcessorInertialKinematics" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "PInertialKinematics" # This is ignored. The name provided to the SensorFactory prevails
+
+sensor_angvel_name: "SenImu"
+std_bp_drift: 0.01
\ No newline at end of file
diff --git a/demos/processor_odom_3d.yaml b/demos/processor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..950b77442fd54510ac688dd8b4e1d884d6888d9d
--- /dev/null
+++ b/demos/processor_odom_3d.yaml
@@ -0,0 +1,11 @@
+type: "ProcessorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0005 # seconds
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+ voting_active: false
+ max_time_span: 100 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/demos/sensor_ft.yaml b/demos/sensor_ft.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5112ba8e3f474321a74bcb6c1cc01b1b948941db
--- /dev/null
+++ b/demos/sensor_ft.yaml
@@ -0,0 +1,5 @@
+type: "SensorForceTorque" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "SenFT" # This is ignored. The name provided to the SensorFactory prevails
+mass: 10
+std_f: 0.001
+std_tau: 0.001
\ No newline at end of file
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
index 24f99895d80a1cc8f3f8dc2d6cc52fbadc25e79b..47a6d19167d559a21de6d0b41f104729181a8bd5 100644
--- a/demos/sensor_imu.yaml
+++ b/demos/sensor_imu.yaml
@@ -1,9 +1,9 @@
-type: "SensorIMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-name: "Main IMU Sensor" # This is ignored. The name provided to the SensorFactory prevails
+type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main Imu Sensor" # This is ignored. The name provided to the SensorFactory prevails
motion_variances:
a_noise: 0.02 # standard deviation of Acceleration noise (same for all the axis) in m/s2
w_noise: 0.03 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
ab_initial_stdev: 0.00 # m/s2 - initial bias
wb_initial_stdev: 0.0 # rad/sec - initial bias
- ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
- wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
\ No newline at end of file
+ ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
\ No newline at end of file
diff --git a/demos/sensor_imu_solo12.yaml b/demos/sensor_imu_solo12.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..68ad34e3b0d14ec0711760551d8875ab005622cc
--- /dev/null
+++ b/demos/sensor_imu_solo12.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main Imu Sensor" # This is ignored. The name provided to the SensorFactory prevails
+motion_variances:
+ a_noise: 0.05 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+ w_noise: 0.005 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ ab_initial_stdev: 0.00 # m/s2 - initial bias
+ wb_initial_stdev: 0.0 # rad/sec - initial bias
+ ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
\ No newline at end of file
diff --git a/demos/sensor_inertial_kinematics.yaml b/demos/sensor_inertial_kinematics.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..62327e3c3e230f32b50d0d595cdcc8ec9a186248
--- /dev/null
+++ b/demos/sensor_inertial_kinematics.yaml
@@ -0,0 +1,5 @@
+type: "SensorInertialKinematics" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "SInertialKinematics" # This is ignored. The name provided to the SensorFactory prevails
+
+std_pbc: 0.01
+std_vbc: 0.01
\ No newline at end of file
diff --git a/demos/sensor_odom_3d.yaml b/demos/sensor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8eb2b235011cb3cfe4f35b1f73da6344991af0da
--- /dev/null
+++ b/demos/sensor_odom_3d.yaml
@@ -0,0 +1,7 @@
+type: "SensorOdom3d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+
+k_disp_to_disp: 0.02 # m^2 / m
+k_disp_to_rot: 0.02 # rad^2 / m
+k_rot_to_rot: 0.01 # rad^2 / rad
+min_disp_var: 0.01 # m^2
+min_rot_var: 0.01 # rad^2
diff --git a/demos/solo_mocap_imu.cpp b/demos/solo_mocap_imu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19f8dfaa700dbcb7876dfd2f3587bb996d68ab1b
--- /dev/null
+++ b/demos/solo_mocap_imu.cpp
@@ -0,0 +1,543 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 <iostream>
+#include <fstream>
+
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// CNPY
+#include <cnpy.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_pose_3d_with_extrinsics.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/processor/processor_odom_3d.h>
+#include <core/sensor/sensor_odom_3d.h>
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+
+#include <core/sensor/sensor_pose.h>
+#include <core/processor/processor_pose.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/factor/factor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+
+
+// UTILS
+#include "mcapi_utils.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+
+int main (int argc, char **argv) {
+ // retrieve parameters from yaml file
+ YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ double dt = config["dt"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+
+ // Ceres setup
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+
+ // priors
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+ std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
+ std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
+ Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
+
+ // MOCAP
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
+ std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
+
+ // Base to IMU transform
+ Quaterniond b_q_i(b_qvec_i);
+ assert(b_q_i.normalized().isApprox(b_q_i));
+ Quaterniond i_q_b = b_q_i.conjugate();
+ SE3 b_T_i(b_q_i, b_p_bi);
+ SE3 i_T_b = b_T_i.inverse();
+ Matrix3d b_R_i = b_T_i.rotation();
+ Vector3d i_p_ib = i_T_b.translation();
+ Matrix3d i_R_b = i_T_b.rotation();
+
+ // initialize some vectors and fill them with dicretized data
+ cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
+
+ //load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
+ cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
+ // cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
+ // cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
+ cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
+ cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
+ // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ // cnpy::NpyArray contact_npyarr = arr_map["contact"];
+ // cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
+
+ // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
+ double* t_arr = t_npyarr.data<double>();
+ double* imu_acc_arr = imu_acc_npyarr.data<double>();
+ // double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
+ double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
+ double* l_forces_arr = l_forces_npyarr.data<double>();
+ // double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
+ double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
+ double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ // double* o_R_i_arr = o_R_i_npyarr.data<double>();
+ double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
+ // double* contact_arr = contact_npyarr.data<double>();
+ // double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
+
+ // double* w_R_m_arr = w_R_m_npyarr.data<double>();
+ unsigned int N = t_npyarr.shape[0];
+ if (max_t > 0){
+ N = N < int(max_t/dt) ? N : int(max_t/dt);
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("POV", 3);
+
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ problem->setTreeManager(tree_manager);
+
+ //////////////////////
+ // COMPUTE INITIAL STATE
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
+ //////////////////////
+
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ solver->getSolverOptions().max_num_iterations = max_num_iterations;
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_imu_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_imu_base);
+
+ // SENSOR + PROCESSOR POSE (for mocap)
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ intr_sp->std_p = std_pose_p;
+ intr_sp->std_o = toRad(std_pose_o_deg);
+ Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
+ auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->time_tolerance = time_tolerance_mocap;
+ auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+ // somehow by default the sensor extrinsics is fixed
+ Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
+ Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+ sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
+ sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
+ if (unfix_extr_sensor_pose){
+ sensor_pose->unfixExtrinsics();
+ }
+ else{
+ sensor_pose->fixExtrinsics();
+ }
+
+ Matrix3d w_R_m_init = q2R(w_qvec_wm);
+ Vector3d w_p_wi_init = w_p_wm + w_R_m_init*b_p_bi;
+
+ VectorComposite x_origin("POV", {w_p_wi_init, w_qvec_wm, Vector3d::Zero()});
+ VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
+
+ proc_imu->setOrigin(KF1);
+
+ //////////////////////////////////////////
+ // INITIAL BIAS FACTORS
+ // Add absolute factor on Imu biases to simulate a fix()
+ Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
+ sen_imu->getStateBlock('I')->setState(bias_imu_prior);
+ // sen_imu->fixIntrinsics();
+
+
+ //////////////////////////////////////////
+ // Vectors to store estimates at the time of data processing
+ // fbk -> feedback: to check if the estimation is stable enough for feedback control
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
+ // Allocate on the heap to prevent stack overflow for large datasets
+ double* o_p_ob_fbk_carr = new double[3*N];
+ double* o_q_b_fbk_carr = new double[4*N];
+ double* o_v_ob_fbk_carr = new double[3*N];
+ double* imu_bias_fbk_carr = new double[6*N];
+ double* extr_mocap_fbk_carr = new double[7*N];
+
+ // covariances computed on the fly
+ std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+
+ //
+ std::vector<Vector3d> i_omg_oi_v;
+ //////////////////////////////////////////
+
+ unsigned int nb_kf = 1;
+ for (unsigned int i=0; i < N; i++){
+ TimeStamp ts(t_arr[i]);
+
+ ////////////////////////////////////
+ ////////////////////////////////////
+ // Get measurements
+ // retrieve traj data
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
+ // store i_omg_oi for later computation of o_v_ob from o_v_oi
+ i_omg_oi_v.push_back(i_omg_oi);
+
+ Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+ Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+
+ ////////////////////////////////////
+ // IMU
+ Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
+
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CImu->process();
+
+ // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+ Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+ CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+ CP->process();
+
+ // solve every new KF
+ if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << ts << " ";
+ std::cout << report << std::endl;
+
+ // recover covariances at this point
+ auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
+
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+
+ Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+
+
+ Qbi_fbk_v.push_back(Qbi_fbk_diag);
+
+ // std::cout << "Qbi_fbk: \n" << Qbi_fbk.topLeftCorner<3,3>() << std::endl;
+ // std::cout << "Qbi_fbk: \n" << Qbi_fbk << std::endl;
+
+
+ nb_kf++;
+ }
+
+ // Store current state estimation
+ VectorComposite state_fbk = problem->getState();
+ // VectorComposite state_fbk = problem->getState(ts);
+ Vector3d o_p_oi = state_fbk['P'];
+ Vector4d o_qvec_i = state_fbk['O'];
+ Vector3d o_v_oi = state_fbk['V'];
+
+ // IMU to base frame
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ // o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*i_R_b*b_p_bi);
+ // Vector3d o_p_ob = o_p_oi;
+ // Vector3d o_v_ob = o_v_oi;
+
+ imu_bias = sen_imu->getIntrinsic()->getState();
+ // imu_bias = sen_imu->getIntrinsic(ts)->getState();
+ Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+
+ mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(), 7*sizeof(double));
+
+ p_ob_fbk_v.push_back(o_p_ob);
+ q_ob_fbk_v.push_back(o_qvec_b);
+ v_ob_fbk_v.push_back(o_v_ob);
+ }
+
+
+
+ ///////////////////////////////////////////
+ //////////////// SOLVING //////////////////
+ ///////////////////////////////////////////
+ if (solve_end){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
+ std::cout << report << std::endl;
+ }
+
+ double* o_p_ob_carr = new double[3*N];
+ double* o_q_b_carr = new double[4*N];
+ double* imu_bias_carr = new double[6*N];
+ double* o_v_ob_carr = new double[6*N];
+ for (unsigned int i=0; i < N; i++) {
+ VectorComposite state_est = problem->getState(t_arr[i]);
+ Vector3d i_omg_oi = i_omg_oi_v[i];
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = state_est['V'];
+
+ auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+ Vector6d imu_bias = sb->getState();
+ // std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
+ Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
+ // Vector3d o_p_ob = o_p_oi;
+ // Vector3d o_v_ob = o_v_oi;
+
+ mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ }
+
+
+ // Compute Covariances
+ unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ double* tkf_carr = new double[1*Nkf];
+ double* Qp_carr = new double[3*Nkf];
+ double* Qo_carr = new double[3*Nkf];
+ double* Qv_carr = new double[3*Nkf];
+ double* Qbi_carr = new double[6*Nkf];
+ double* Qm_carr = new double[6*Nkf];
+ // feedback covariances
+ double* Qbi_fbk_carr = new double[6*Nkf];
+
+ // factor errors
+ double* fac_imu_err_carr = new double[9*Nkf];
+ double* fac_pose_err_carr = new double[6*Nkf];
+ int i = 0;
+ for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ std::cout << "Traj " << i << "/" << problem->getTrajectory()->getFrameMap().size() << std::endl;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
+ VectorComposite kf_state = kf->getState();
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+ problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+ problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+
+ CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
+
+ std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
+ solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
+ // std::cout << "Qbp\n" << Qbp << std::endl;
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+ // diagonal components
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
+ memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
+ memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
+ memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
+ memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+
+ // Recover feedback covariances
+ memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+
+
+ // Factor errors
+ // std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
+ FactorBasePtrList fac_lst;
+ kf->getFactorList(fac_lst);
+ Vector9d imu_err = Vector9d::Zero();
+ Vector6d pose_err = Vector6d::Zero();
+ for (auto fac: fac_lst){
+ if (fac->getProcessor() == proc_imu){
+ auto f = std::dynamic_pointer_cast<FactorImu>(fac);
+ // Matrix6d R_bias_drift = f->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
+ // Matrix6d R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
+ // std::cout << R_bias_drift << std::endl;
+ // MatrixXd R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper();
+ // MatrixXd Cov_bias_drift = f->getFeature()->getMeasurementCovariance();
+ // std::cout << "R_bias_drift" << std::endl;
+ // std::cout << R_bias_drift << std::endl;
+ // std::cout << "Cov_bias_drift" << std::endl;
+ // std::cout << Cov_bias_drift << std::endl;
+
+ if (f){
+ imu_err = f->error();
+ }
+ }
+ if (fac->getProcessor() == proc_pose){
+ auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
+ if (f){
+ pose_err = f->error();
+ }
+ }
+ }
+ memcpy(fac_imu_err_carr + 9*i, imu_err.data(), 9*sizeof(double));
+ memcpy(fac_pose_err_carr + 6*i, pose_err.data(), 6*sizeof(double));
+
+
+ i++;
+ }
+
+ // Save trajectories in npz file
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+
+ // save estimates
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+
+ // and biases/extrinsics
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "extr_mocap_fbk", extr_mocap_fbk_carr, {N,7}, "a");
+
+ // covariances
+ cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
+
+}
diff --git a/demos/solo_real_estimation.yaml b/demos/solo_real_estimation.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1867ec7a227dd3a3d623fc6db430a9cbb36d823b
--- /dev/null
+++ b/demos/solo_real_estimation.yaml
@@ -0,0 +1,164 @@
+# trajectory handling
+dt: 0.001
+min_t: -1.0 # -1 means from the beginning of the trajectory
+max_t: -1.0 # -1 means until the end of the trajectory
+# max_t: 4.8 # -1 means until the end of the trajectory
+# solve_every_sec: -1 # < 0 strict --> no solve
+# solve_end: False
+solve_every_sec: 0.05 # < 0 strict --> no solve
+solve_end: True
+
+# Ceres setup
+max_num_iterations: 1000
+
+# estimator sensor noises
+std_pbc_est: 0.01
+std_vbc_est: 0.01
+std_f_est: 1
+std_tau_est: 1000
+std_odom3d_est: 0.01
+# std_odom3d_est: 10000000
+
+# some controls
+fz_thresh: 1 # slow walk
+# fz_thresh: 3 # slow walk
+# fz_thresh: 3 # slow traj
+# fz_thresh: 4 # stamping
+# fz_thresh: 5.5 # walking
+
+# priors
+std_prior_p: 0.00001
+std_prior_o: 1
+std_prior_v: 1
+
+bias_pbc_prior: [0,0,0]
+# bias_pbc_prior: [0.0139338 , 0.02786759, 0.01857839]
+std_abs_bias_pbc: 0.1 # noprior
+# std_abs_bias_pbc: 0.00001 # almost fixed
+# std_bp_drift: 1000 # All the drift you want
+std_bp_drift: 0.005 # small drift
+
+std_pose_p: 0.001
+std_pose_o_deg: 1
+std_mocap_extr_o_deg: 0.1
+std_mocap_extr_p: 10
+unfix_extr_sensor_pose: true
+time_shift_mocap: 0
+time_tolerance_mocap: 0.001
+
+bias_imu_prior: [0,0,0, 0,0,0]
+# bias_imu_prior: [0,0,-0.011,-0.000079251, 0.00439540765, -0.0002120267]
+# bias_imu_prior: [0,0,-0.11, 0,0,0]
+# bias_imu_prior: [0,0,0, 0.1, 0.2, 0.3]
+std_abs_bias_acc: 100
+std_abs_bias_gyro: 100
+
+robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
+dimc: 3
+mass_offset: 0.0
+# mass_offset: -0.00405
+# mass_offset: -0.005
+nb_feet: 4
+
+l_p_lg: [0.0, 0, 0]
+# l_p_lg: [0, 0, -0.016]
+# l_p_lg: [0, 0, -0.031] # for sin traj, point to which position estimation on z starts to be less good
+
+# base to IMU transformation
+# b_p_bi: [0.0, 0.0, 0.0]
+# b_p_bi: [-0.2, 0.0, 0.0]
+b_p_bi: [0.1163, 0.0, 0.02]
+b_q_i: [0.0, 0.0, 0.0, 1.0]
+# b_q_i: [0.00000592745, -0.03255761280, -0.00025745595, 0.706732091]
+
+artificial_bias_p: [0.0, 0.0, 0.0]
+# artificial_bias_p: [0.03, 0.06, 0.04]
+# artificial_bias_p: [3, 6, 4]
+
+
+
+
+
+# Data files
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/solo12_mpi_stamping_2020-09-29.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_09_50_Walking_Novicon.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_10_04_StandingStill_format.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_10_04_StandingStill_shortened_format.npz"
+
+# 18:58 Fixe 4 stance phase (20s)
+# 19:00 Rotation 4 stance phase (30s)
+# 19:02 Mouvement avant arrière, rotation, rotation, mvt bas haut, roll (30s)
+# 19:03 Replay sin wave
+# 19:05 Replay stamping
+# 19:06 Marche 0.32 (30s)
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_18_58_format.npz" # OK
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_18_58_format_shortened.npz" # OK
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_00_format.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_02_format.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_03_format.npz" # OK
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_05_format.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_06_format.npz"
+
+
+
+
+# data_2020_10_15_14_34 standing still
+# data_2020_10_15_14_36 sin traj
+# data_2020_10_15_14_38 solo stamping
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened.npz"
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_0gyr.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_Ogyr_Gacc_0dqa.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_Ogyr_Gacc_Filtereddqa.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTmean_Filtereddqa.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTacc_0gyr_Filtereddqa.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_0gyr_Gacc.npz"
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTqa.npz"
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_VERY_shortened.npz"
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_36_format.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_38_format.npz"
+
+
+# data_2020_10_15_18_21: ...
+# data_2020_10_15_18_23: walk
+# data_2020_10_15_18_24: walk
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_21.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_23.npz"
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_24.npz"
+
+
+# same but with precomputed forces
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_forces.npz" # standing still
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_36_format_forces.npz" # sinusoid
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_38_format_forces.npz" # stamping
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_23_format_forces.npz" # fast walk
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_31_20_12_format_forces.npz" # 0.48s walk
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_05_10_2020_18h/data_2020_11_05_18_18_format_forces.npz" # standing still+walk 0.48 FAIL
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_05_10_2020_18h/data_2020_11_05_18_20_format_forces.npz" # standing still+walk 0.48
+
+
+# POINT FEET
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Point_feet_27_11_20/data_2020_11_27_14_46_format.npz" # standing still+walk 0.48
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_17_format.npz" # stamping
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_18_format.npz" # sin
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_22_format.npz" # walking
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/SinStamping_Corrected_09_12_2020/data_2020_12_09_17_54_format.npz" # sin
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/SinStamping_Corrected_09_12_2020/data_2020_12_09_17_56_format.npz" # stamping
+
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Walk_17_12_2020/data_2020_12_17_14_25_format.npz" # point feet walk with corrected kinematics
+# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Walk_17_12_2020/data_2020_12_17_14_29_format.npz" # point feet walk with corrected kinematics
+
+
+data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Mesures_Contact/data_2021_01_19_17_11_format.npz" # Contact experiment with contact status and mocap leg kin
+
+
+out_npz_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments_results/out.npz"
\ No newline at end of file
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbae4d51a0f97f30aa05c5329ca754986ce72cb2
--- /dev/null
+++ b/demos/solo_real_pov_estimation.cpp
@@ -0,0 +1,702 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 <iostream>
+#include <fstream>
+
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// CNPY
+#include <cnpy.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_pose_3d_with_extrinsics.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/processor/processor_odom_3d.h>
+#include <core/sensor/sensor_odom_3d.h>
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+
+#include <core/sensor/sensor_pose.h>
+#include <core/processor/processor_pose.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/factor/factor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_leg_odom.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_inertial_kinematics.h"
+#include "bodydynamics/factor/factor_force_torque_preint.h"
+
+#include "bodydynamics/capture/capture_point_feet_nomove.h"
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
+#include "bodydynamics/processor/processor_point_feet_nomove.h"
+
+
+
+// UTILS
+#include "mcapi_utils.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+
+int main (int argc, char **argv) {
+ // retrieve parameters from yaml file
+ YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ int nb_feet = config["nb_feet"].as<int>();
+ double dt = config["dt"].as<double>();
+ double min_t = config["min_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ double solve_every_sec = config["solve_every_sec"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+
+ // Ceres setup
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+
+ // estimator sensor noises
+ double std_pbc_est = config["std_pbc_est"].as<double>();
+ double std_vbc_est = config["std_vbc_est"].as<double>();
+ double std_f_est = config["std_f_est"].as<double>();
+ double std_tau_est = config["std_tau_est"].as<double>();
+ double std_odom3d_est = config["std_odom3d_est"].as<double>();
+
+ // priors
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+ std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
+ std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
+ Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
+ Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
+ double fz_thresh = config["fz_thresh"].as<double>();
+
+ // MOCAP
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
+ std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
+
+ std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
+ unsigned int nbc = ee_names.size();
+ // Base to IMU transform
+ Quaterniond b_q_i(b_qvec_i);
+ assert(b_q_i.normalized().isApprox(b_q_i));
+ Quaterniond i_q_b = b_q_i.conjugate();
+ SE3 b_T_i(b_q_i, b_p_bi);
+ SE3 i_T_b = b_T_i.inverse();
+ Matrix3d b_R_i = b_T_i.rotation();
+ Vector3d i_p_ib = i_T_b.translation();
+ Matrix3d i_R_b = i_T_b.rotation();
+
+ // initialize some vectors and fill them with dicretized data
+ cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
+
+ //load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
+ cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
+ // cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
+ // cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
+ cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
+ cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
+ // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ // cnpy::NpyArray contact_npyarr = arr_map["contact"];
+ // cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
+
+ // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
+ double* t_arr = t_npyarr.data<double>();
+ double* imu_acc_arr = imu_acc_npyarr.data<double>();
+ // double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
+ double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
+ double* l_forces_arr = l_forces_npyarr.data<double>();
+ // double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
+ double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
+ double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ // double* o_R_i_arr = o_R_i_npyarr.data<double>();
+ double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
+ // double* contact_arr = contact_npyarr.data<double>();
+ // double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
+
+ // double* w_R_m_arr = w_R_m_npyarr.data<double>();
+ unsigned int N = t_npyarr.shape[0];
+ if (max_t > 0){
+ N = N < int(max_t/dt) ? N : int(max_t/dt);
+ }
+
+ // Creating measurements from simulated trajectory
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ Data data(model);
+
+ // Recover end effector frame ids
+ std::vector<int> ee_ids;
+ std::cout << "Frame ids" << std::endl;
+ for (auto ee_name: ee_names){
+ ee_ids.push_back(model.getFrameId(ee_name));
+ std::cout << ee_name << std::endl;
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("POV", 3);
+
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ problem->setTreeManager(tree_manager);
+
+ //////////////////////
+ // COMPUTE INITIAL STATE
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
+ Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
+ // initial state
+ VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
+ VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
+
+ //////////////////////
+
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ solver->getSolverOptions().max_num_iterations = max_num_iterations;
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
+
+ // // SENSOR + PROCESSOR POINT FEET NOMOVE
+ // ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+ // intr_pfn->std_foot_nomove_ = std_odom3d_est;
+ // VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ // SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+ // SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ // ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+ // ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+
+ // SENSOR + PROCESSOR POSE (for mocap)
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ intr_sp->std_p = std_pose_p;
+ intr_sp->std_o = toRad(std_pose_o_deg);
+ Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
+ auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->time_tolerance = time_tolerance_mocap;
+ auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+ // somehow by default the sensor extrinsics is fixed
+ Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
+ Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+ sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
+ sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
+ if (unfix_extr_sensor_pose){
+ sensor_pose->unfixExtrinsics();
+ }
+ else{
+ sensor_pose->fixExtrinsics();
+ }
+
+
+
+ // SETPRIOR RETRO-ENGINEERING
+ // We are not using setPrior because of processors initial captures problems so we have to
+ // - Manually create the first KF and its pose and velocity priors
+ // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
+
+ VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
+ VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
+
+ proc_imu->setOrigin(KF1);
+
+ //////////////////////////////////////////
+ // INITIAL BIAS FACTORS
+ // Add absolute factor on Imu biases to simulate a fix()
+ Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
+ sen_imu->getStateBlock('I')->setState(bias_imu_prior);
+ // sen_imu->fixIntrinsics();
+
+
+ ///////////////////////////////////////////
+ // process measurements in sequential order
+ double ts_since_last_kf = 0;
+
+ std::vector<Vector3d> i_p_il_vec_prev;
+ std::vector<Vector4d> i_qvec_l_vec_prev;
+ std::vector<bool> feet_in_contact_prev;
+
+ //////////////////////////////////////////
+ // Vectors to store estimates at the time of data processing
+ // fbk -> feedback: to check if the estimation is stable enough for feedback control
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
+ // Allocate on the heap to prevent stack overflow for large datasets
+ double* o_p_ob_fbk_carr = new double[3*N];
+ double* o_q_b_fbk_carr = new double[4*N];
+ double* o_v_ob_fbk_carr = new double[3*N];
+ double* imu_bias_fbk_carr = new double[6*N];
+ double* extr_mocap_fbk_carr = new double[7*N];
+
+ std::vector<Vector3d> i_omg_oi_v;
+ //////////////////////////////////////////
+
+ unsigned int nb_kf = 1;
+ for (unsigned int i=0; i < N; i++){
+ TimeStamp ts(t_arr[i]);
+
+ ////////////////////////////////////
+ // Retrieve current state
+ VectorComposite curr_state = problem->getState();
+ Vector4d o_qvec_i_curr = curr_state['O'];
+ Quaterniond o_q_i_curr(o_qvec_i_curr);
+ Vector3d o_v_oi_curr = curr_state['V'];
+
+ ////////////////////////////////////
+ ////////////////////////////////////
+ // Get measurements
+ // retrieve traj data
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
+ Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+ // Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
+ // Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work!
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
+ // store i_omg_oi for later computation of o_v_ob from o_v_oi
+ i_omg_oi_v.push_back(i_omg_oi);
+
+ Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+ Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+
+ ////////////////////////////////////
+ // IMU
+ Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
+
+ ////////////////////////////////////
+ // Kinematics + forces
+ // SE3 o_T_i(o_q_i_curr, Vector3d::Zero());
+ // Matrix3d o_R_i = o_T_i.rotation();
+ // Matrix3d i_R_o = o_R_i.transpose();
+ // Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr;
+ // Vector3d i_acc = imu_acc + i_R_o * gravity();
+ // Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr);
+
+ // VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa;
+ // VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa;
+ // VectorXd ddq(model.nv); ddq.setZero();
+ // ddq.head<3>() = i_asp_i;
+ // // TODO: add other terms to compute forces (ddqa...)
+
+ // // update and retrieve kinematics
+ // forwardKinematics(model, data, q);
+ // updateFramePlacements(model, data);
+
+ // // compute all linear jacobians in feet frames (only for quadruped)
+ // MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
+ // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ // MatrixXd Jee(6, model.nv); Jee.setZero();
+ // computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
+ // Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
+ // }
+
+ // // estimate forces from torques
+ // VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
+ // tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
+
+ // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
+ // Solve in Least square sense (3 ways):
+ // VectorXd forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
+ // VectorXd l_forces = Jlinvel.transpose().colPivHouseholderQr().solve(tau_cf); // QR
+ // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
+
+ // Or else, retrieve from preprocessed dataset
+ // Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+
+ // Vector3d o_f_tot = Vector3d::Zero();
+ // std::vector<Vector3d> l_f_l_vec;
+ // std::vector<Vector3d> o_f_l_vec;
+ // std::vector<Vector3d> i_p_il_vec;
+ // std::vector<Vector4d> i_qvec_l_vec;
+ // // needing relative kinematics measurements
+ // VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
+ // VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
+ // forwardKinematics(model, data, q_static, dq_static);
+ // updateFramePlacements(model, data);
+ // // std::cout << "" << std::endl;
+ // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ // // std::cout << i_ee << std::endl;
+ // auto b_T_l = data.oMf[ee_ids[i_ee]];
+
+ // // overides with value from mocap
+ // // std::cout << b_T_l.translation().transpose() << std::endl;
+ // b_T_l.translation(b_p_bl_mocap.segment<3>(3*i_ee));
+ // // std::cout << b_T_l.translation().transpose() << std::endl;
+
+ // auto i_T_l = i_T_b * b_T_l;
+ // Vector3d i_p_il = i_T_l.translation(); // meas
+ // Matrix3d i_R_l = i_T_l.rotation();
+ // Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas
+ // i_p_il = i_p_il + i_R_l*l_p_lg;
+
+ // Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas
+ // // Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
+
+ // l_f_l_vec.push_back(l_f_l);
+ // // o_f_l_vec.push_back(o_f_l);
+ // i_p_il_vec.push_back(i_p_il);
+ // i_qvec_l_vec.push_back(i_qvec_l);
+ // }
+
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CImu->process();
+
+ // 1) detect feet in contact
+ // int nb_feet_in_contact = 0;
+ // std::unordered_map<int, Vector3d> kin_incontact;
+ // for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // // basic contact detection based on normal foot vel, things break if no foot is in contact
+ // // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
+ // // nb_feet: just for debugging/testing kinematic factor
+
+ // // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
+ // if (contact_gtr(i_ee)){
+ // nb_feet_in_contact++;
+ // kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
+ // }
+ // }
+
+ // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ // CPF->process();
+
+ // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+ Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+ CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+ CP->process();
+
+ // ts_since_last_kf += dt;
+ // if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
+ // // problem->print(4,true,true,true);
+ // auto kf = problem->emplaceFrame(ts);
+ // problem->keyFrameCallback(kf, nullptr, 0.0005);
+ // std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ // std::cout << report << std::endl;
+ // ts_since_last_kf = 0;
+ // // std::cout << "Extr SPose: " << sensor_pose->getP()->getState().transpose() << " " << sensor_pose->getO()->getState().transpose() << std::endl;
+ // }
+
+ // solve every new KF
+ if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << ts << " ";
+ std::cout << report << std::endl;
+ nb_kf = problem->getTrajectory()->getFrameMap().size();
+ }
+
+ // Store current state estimation
+ VectorComposite state_fbk = problem->getState(ts);
+ Vector3d o_p_oi = state_fbk['P'];
+ Vector4d o_qvec_i = state_fbk['O'];
+ Vector3d o_v_oi = state_fbk['V'];
+
+ // IMU to base frame
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ // o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*b_p_bi);
+ // Vector3d o_p_ob = o_p_oi;
+ // Vector3d o_v_ob = o_v_oi;
+
+ imu_bias = sen_imu->getIntrinsic()->getState();
+ Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+
+ mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(), 7*sizeof(double));
+
+ p_ob_fbk_v.push_back(o_p_ob);
+ q_ob_fbk_v.push_back(o_qvec_b);
+ v_ob_fbk_v.push_back(o_v_ob);
+ }
+
+
+
+ ///////////////////////////////////////////
+ //////////////// SOLVING //////////////////
+ ///////////////////////////////////////////
+ if (solve_end){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
+ std::cout << report << std::endl;
+ }
+
+ double* o_p_ob_carr = new double[3*N];
+ double* o_q_b_carr = new double[4*N];
+ double* imu_bias_carr = new double[6*N];
+ double* o_v_ob_carr = new double[6*N];
+ for (unsigned int i=0; i < N; i++) {
+ VectorComposite state_est = problem->getState(t_arr[i]);
+ Vector3d i_omg_oi = i_omg_oi_v[i];
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = state_est['V'];
+
+ auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+ Vector6d imu_bias = sb->getState();
+ // std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
+ Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
+ // Vector3d o_p_ob = o_p_oi;
+ // Vector3d o_v_ob = o_v_oi;
+
+ mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ }
+
+
+ // Compute Covariances
+ unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ double* tkf_carr = new double[1*Nkf];
+ double* Qp_carr = new double[3*Nkf];
+ double* Qo_carr = new double[3*Nkf];
+ double* Qv_carr = new double[3*Nkf];
+ double* Qbi_carr = new double[6*Nkf];
+ double* Qm_carr = new double[6*Nkf];
+ // factor errors
+ double* fac_imu_err_carr = new double[9*Nkf];
+ double* fac_pose_err_carr = new double[6*Nkf];
+ int i = 0;
+ for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ std::cout << "Traj " << i << "/" << problem->getTrajectory()->getFrameMap().size() << std::endl;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
+ VectorComposite kf_state = kf->getState();
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+ problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+ problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+
+ CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
+
+ std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
+ solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
+ // std::cout << "Qbp\n" << Qbp << std::endl;
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+ // diagonal components
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
+ memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
+ memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
+ memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
+ memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+
+ // Factor errors
+ // std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
+ FactorBasePtrList fac_lst;
+ kf->getFactorList(fac_lst);
+ Vector9d imu_err = Vector9d::Zero();
+ Vector6d pose_err = Vector6d::Zero();
+ for (auto fac: fac_lst){
+ if (fac->getProcessor() == proc_imu){
+ auto f = std::dynamic_pointer_cast<FactorImu>(fac);
+ // Matrix6d R_bias_drift = f->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
+ // Matrix6d R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
+ // std::cout << R_bias_drift << std::endl;
+ // MatrixXd R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper();
+ // MatrixXd Cov_bias_drift = f->getFeature()->getMeasurementCovariance();
+ // std::cout << "R_bias_drift" << std::endl;
+ // std::cout << R_bias_drift << std::endl;
+ // std::cout << "Cov_bias_drift" << std::endl;
+ // std::cout << Cov_bias_drift << std::endl;
+
+ if (f){
+ imu_err = f->error();
+ }
+ }
+ if (fac->getProcessor() == proc_pose){
+ auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
+ if (f){
+ pose_err = f->error();
+ }
+ }
+ }
+ memcpy(fac_imu_err_carr + 9*i, imu_err.data(), 9*sizeof(double));
+ memcpy(fac_pose_err_carr + 6*i, pose_err.data(), 6*sizeof(double));
+
+
+ i++;
+ }
+
+ // Save trajectories in npz file
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+
+ // save estimates
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+
+ // and biases/extrinsics
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "extr_mocap_fbk", extr_mocap_fbk_carr, {N,7}, "a");
+
+ // covariances
+ cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
+
+}
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0e4e6a4a6ce958563c3b4e84dfe3f31feb42f464
--- /dev/null
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -0,0 +1,876 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 <iostream>
+#include <fstream>
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// CNPY
+#include<cnpy.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_block_absolute.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_leg_odom.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_inertial_kinematics.h"
+#include "bodydynamics/factor/factor_force_torque_preint.h"
+
+#include "bodydynamics/capture/capture_point_feet_nomove.h"
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
+#include "bodydynamics/processor/processor_point_feet_nomove.h"
+
+// UTILS
+#include "mcapi_utils.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+
+int main (int argc, char **argv) {
+ // retrieve parameters from yaml file
+ YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ double mass_offset = config["mass_offset"].as<double>();
+ double dt = config["dt"].as<double>();
+ double min_t = config["min_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ double solve_every_sec = config["solve_every_sec"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+
+ // Ceres setup
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+
+ // estimator sensor noises
+ double std_pbc_est = config["std_pbc_est"].as<double>();
+ double std_vbc_est = config["std_vbc_est"].as<double>();
+ double std_f_est = config["std_f_est"].as<double>();
+ double std_tau_est = config["std_tau_est"].as<double>();
+ double std_odom3d_est = config["std_odom3d_est"].as<double>();
+
+ // priors
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ std::vector<double> bias_pbc_prior_vec = config["bias_pbc_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> bias_pbc_prior(bias_pbc_prior_vec.data());
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+
+ double fz_thresh = config["fz_thresh"].as<double>();
+ std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
+ std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
+ Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
+
+ std::vector<double> artificial_bias_p_vec = config["artificial_bias_p"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> artificial_bias_p(artificial_bias_p_vec.data());
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
+ std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
+
+ std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
+ unsigned int nbc = ee_names.size();
+ // Base to IMU transform
+ Quaterniond b_q_i(b_qvec_i);
+ SE3 b_T_i(b_q_i, b_p_bi);
+ SE3 i_T_b = b_T_i.inverse();
+ Matrix3d b_R_i = b_T_i.rotation();
+ Vector3d i_p_ib = i_T_b.translation();
+ Matrix3d i_R_b = i_T_b.rotation();
+
+ // initialize some vectors and fill them with dicretized data
+ cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
+
+ //load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
+ cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
+ // cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
+ // cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
+ cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
+ cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
+ // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
+ double* t_arr = t_npyarr.data<double>();
+ double* imu_acc_arr = imu_acc_npyarr.data<double>();
+ // double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
+ double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
+ double* l_forces_arr = l_forces_npyarr.data<double>();
+ // double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
+ double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
+ double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ // double* o_R_i_arr = o_R_i_npyarr.data<double>();
+ double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+ // double* w_R_m_arr = w_R_m_npyarr.data<double>();
+ unsigned int N = t_npyarr.shape[0];
+ if (max_t > 0){
+ N = N < int(max_t/dt) ? N : int(max_t/dt);
+ }
+
+ // Creating measurements from simulated trajectory
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ // set gravity of the model to the one decided by wolf
+ model.gravity.linear() = wolf::gravity();
+ // to accomodate the difference between real robot and model masses
+ model.inertias[1].mass() = model.inertias[1].mass() + mass_offset;
+
+ // add artificial lever to test bias estimation on real data
+ VectorXd q_toto(19); q_toto << 0,0,0, 0,0,0,1, 0,0,0,0,0,0,0,0,0,0,0,0; // TODO: put current base orientation estimate? YES
+
+ Data data(model);
+
+ centerOfMass(model, data, q_toto);
+ std::cout << "artificial_bias_p: " << artificial_bias_p.transpose() << std::endl;
+ std::cout << "model.inertias[1].lever(): " << model.inertias[1].lever().transpose() << std::endl;
+ std::cout << "data.com[0]: " << data.com[0].transpose() << std::endl;
+ model.inertias[1] = pinocchio::Inertia(model.inertias[1].mass(), artificial_bias_p, model.inertias[1].inertia());
+ centerOfMass(model, data, q_toto);
+ std::cout << "model.inertias[1].lever(): " << model.inertias[1].lever().transpose() << std::endl;
+ std::cout << "data.com[0]: " << data.com[0].transpose() << std::endl;
+
+
+ // Recover end effector frame ids
+ std::vector<int> ee_ids;
+ std::cout << "Frame ids" << std::endl;
+ for (auto ee_name: ee_names){
+ ee_ids.push_back(model.getFrameId(ee_name));
+ std::cout << ee_name << std::endl;
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("POVCDL", 3);
+
+ //////////////////////
+ // COMPUTE INITIAL STATE
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
+ Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
+ // initial state
+ VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
+ VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
+
+
+ centerOfMass(model, data, q, dq);
+ // Vector3d w_p_wc = data.com[0];
+ // Vector3d w_v_wc = data.vcom[0];
+
+ // gesticulation in base frame: just compute centroidal momentum with static q and vq
+ // Vector3d w_Lc = computeCentroidalMomentum(model, data, q, dq).angular();
+
+ // hyp: robot not moving initially (no vel and angular momentum)
+ VectorXd x_origin; x_origin.resize(19);
+ // x_origin << w_p_wm, w_qvec_wm, 0,0,0, w_p_wc, 0,0,0, 0,0,0;
+ x_origin << 0,0,0, o_q_i, 0,0,0, 0,0,0, 0,0,0, 0,0,0;
+ //////////////////////
+ //////////////////////
+
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ solver->getSolverOptions().max_num_iterations = max_num_iterations;
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR INERTIAL KINEMATICS
+ ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
+ intr_ik->std_pbc = std_pbc_est;
+ intr_ik->std_vbc = std_vbc_est;
+ VectorXd extr0; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr0, intr_ik);
+ // SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ SensorInertialKinematicsPtr sen_ikin = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
+
+ ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->std_bp_drift = std_bp_drift;
+ ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin, params_ik);
+ // ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
+ ProcessorInertialKinematicsPtr proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
+
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
+
+ // SENSOR + PROCESSOR FT PREINT
+ ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
+ intr_ft->std_f = std_f_est;
+ intr_ft->std_tau = std_tau_est;
+ intr_ft->mass = data.mass[0];
+ std::cout << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << "\n\nROBOT MASS: " << intr_ft->mass << std::endl;
+ SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
+ // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
+ SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
+ ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
+ params_sen_ft->sensor_ikin_name = "SenIK";
+ params_sen_ft->sensor_angvel_name = "SenImu";
+ params_sen_ft->nbc = nbc;
+ params_sen_ft->dimc = dimc;
+ params_sen_ft->time_tolerance = 0.0005;
+ params_sen_ft->unmeasured_perturbation_std = 0.000001;
+ params_sen_ft->max_time_span = 1000;
+ params_sen_ft->max_buff_length = 500;
+ params_sen_ft->dist_traveled = 20000.0;
+ params_sen_ft->angle_turned = 1000;
+ params_sen_ft->voting_active = false;
+ ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
+ ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
+
+ // SENSOR + PROCESSOR POINT FEET NOMOVE
+ ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+ intr_pfn->std_foot_nomove_ = std_odom3d_est;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+ ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+
+
+ // SETPRIOR RETRO-ENGINEERING
+ // We are not using setPrior because of processors initial captures problems so we have to
+ // - Manually create the first KF and its pose and velocity priors
+ // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
+
+ MatrixXd P_origin(9,9); P_origin.setIdentity();
+ P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity();
+ P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity();
+ P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity();
+ FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
+ // Prior pose factor
+ CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
+ pose_prior_capture->emplaceFeatureAndFactor();
+ // Prior velocity factor
+ CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1, "Vel0", t0);
+ FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.segment<3>(7), P_origin.block<3, 3>(6, 6));
+ FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1->getV(), nullptr, false);
+
+ // Special trick to make things work in the IMU + IKin + FTPreint case
+ // 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0
+ // 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
+ // 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
+ // 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
+ proc_inkin->keyFrameCallback(KF1, 0.0005);
+ proc_imu->setOrigin(KF1);
+ // proc_legodom->setOrigin(KF1);
+
+ // INERTIAL KINEMATICS CAPTURE DATA ZERO MOTION
+ VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
+ VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
+ centerOfMass(model, data, q_static, dq_static);
+ Vector3d b_p_bc = data.com[0]; // meas
+ Vector3d i_p_ic = i_T_b.act(b_p_bc);
+ Vector3d i_v_ic = i_R_b*data.vcom[0]; // meas
+ Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
+ Matrix3d i_Iw = i_R_b*b_Iw*b_R_i; // !! rotate on both sides to keep a symmetric matrix
+ // Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
+ auto b_centro_momentum_gesti = computeCentroidalMomentum(model, data, q_static, dq_static);
+ auto i_centro_momentum_gesti = i_T_b.act(b_centro_momentum_gesti);
+ Vector3d i_Lc_gesti = i_centro_momentum_gesti.angular();
+
+ // momentum parameters
+ VectorXd meas_ikin(9); meas_ikin << i_p_ic, i_v_ic, i_omg_oi;
+ auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti);
+ CIKin0->process();
+ proc_ftpreint->setOrigin(KF1);
+
+ ////////////////////////////////////////////
+ // INITIAL BIAS FACTORS
+ // Add absolute factor on Imu biases to simulate a fix()
+ Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ sen_imu->getStateBlock('I')->setState(bias_imu_prior);
+
+ // Add loose absolute factor on initial bp bias since dynamical trajectories
+ Matrix3d Q_bp = pow(std_abs_bias_pbc, 2)* Matrix3d::Identity();
+ CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
+ FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
+ sen_ikin->getStateBlock('I')->setState(bias_pbc_prior);
+
+
+ ///////////////////////////////////////////
+ // process measurements in sequential order
+ double ts_since_last_kf = 0;
+
+ // for point feet factor
+ std::vector<Vector3d> i_p_il_vec_origin;
+ FrameBasePtr KF_origin = KF1;
+
+ //////////////////////////////////////////
+ // Vectors to store estimates at the time of data processing
+ // fbk -> feedback: to check if the estimation is stable enough for feedback control
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
+ std::vector<Vector3d> c_ob_fbk_v;
+ std::vector<Vector3d> cd_ob_fbk_v;
+ std::vector<Vector3d> Lc_ob_fbk_v;
+
+ std::vector<Vector3d> i_omg_oi_v;
+ //////////////////////////////////////////
+
+ unsigned int nb_kf = 1;
+ for (unsigned int i=0; i < N; i++){
+ TimeStamp ts(t_arr[i]);
+
+ ////////////////////////////////////
+ // Retrieve current state
+ VectorComposite curr_state = problem->getState();
+ Vector4d o_qvec_i_curr = curr_state['O'];
+ Quaterniond o_q_i_curr(o_qvec_i_curr);
+ Vector3d o_v_oi_curr = curr_state['V'];
+
+ ////////////////////////////////////
+ ////////////////////////////////////
+ // Get measurements
+ // retrieve traj data
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
+ Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+
+ // store i_omg_oi for later computation of o_v_ob from o_v_oi
+ i_omg_oi_v.push_back(i_omg_oi);
+
+ ////////////////////////////////////
+ // IMU
+ // imu_acc << -wolf::gravity();
+ // i_omg_oi << 0,0,0;
+ Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
+
+ ////////////////////////////////////
+ // Kinematics + forces
+ SE3 o_T_i(o_q_i_curr, Vector3d::Zero()); // TODO: put current orientation estimate here
+ Matrix3d o_R_i = o_T_i.rotation();
+ Matrix3d i_R_o = o_R_i.transpose();
+ Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr;
+ Vector3d i_acc = imu_acc + i_R_o * gravity();
+ Vector3d i_asp_i = i_acc - i_omg_oi.cross(i_v_oi_curr);
+
+ VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa;
+ // VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi, dqa;
+ VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa;
+ VectorXd ddq(model.nv); ddq.setZero();
+ ddq.head<3>() = i_asp_i;
+ // TODO: add other terms to compute forces (ddqa...)
+
+ // update and retrieve kinematics
+ forwardKinematics(model, data, q);
+ updateFramePlacements(model, data);
+
+ // compute all linear jacobians in feet frames (only for quadruped)
+ MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ MatrixXd Jee(6, model.nv); Jee.setZero();
+ computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
+ Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
+ }
+
+ // estimate forces from torques
+ VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
+ tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
+
+ // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
+ // Solve in Least square sense (3 ways):
+ // VectorXd forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
+ // VectorXd l_forces = Jlinvel.transpose().colPivHouseholderQr().solve(tau_cf); // QR
+ // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
+
+ // Or else, retrieve from preprocessed dataset
+ Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+
+
+ Vector3d o_f_tot = Vector3d::Zero();
+ std::vector<Vector3d> l_f_l_vec;
+ std::vector<Vector3d> o_f_l_vec;
+ std::vector<Vector3d> i_p_il_vec;
+ std::vector<Vector4d> i_qvec_l_vec;
+ // needing relative kinematics measurements
+ VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
+ forwardKinematics(model, data, q_static);
+ updateFramePlacements(model, data);
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ auto b_T_l = data.oMf[ee_ids[i_ee]];
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation(); // meas
+ Vector4d i_qvec_l = Quaterniond(i_T_l.rotation()).coeffs(); // meas
+ Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas
+ Vector3d o_f_l = o_R_i*i_T_l.rotation() * l_f_l; // for contact test
+ o_f_tot += o_f_l;
+
+ // // put the measure to a constant for unit testing
+ // i_p_il << 0,0,0;
+ // i_qvec_l << 0,0,0,1;
+ // l_f_l << -(intr_ft->mass-0.01)*wolf::gravity()/4;
+
+ l_f_l_vec.push_back(l_f_l);
+ o_f_l_vec.push_back(o_f_l);
+ i_p_il_vec.push_back(i_p_il);
+ i_qvec_l_vec.push_back(i_qvec_l);
+ }
+ // i_p_il_vec[0] << 1, 0, 0;
+ // i_p_il_vec[1] << -1, 0, 0;
+ // i_p_il_vec[2] << 0, 1, 0;
+ // i_p_il_vec[3] << 0,-1, 0;
+
+ // initialization for point feet factor
+ if (i == 0){
+ i_p_il_vec_origin = i_p_il_vec;
+ }
+ // std::cout << "o_f_tot: " << o_f_tot.transpose() << std::endl;
+ // std::cout << "o_f_tot + m*g: " << (o_f_tot + data.mass[0]*model.gravity.linear()).transpose() << std::endl;
+
+
+ //////////////////////////////////////////////
+ // COM relative position and velocity measures
+ centerOfMass(model, data, q_static, dq_static);
+ Vector3d b_p_bc = data.com[0];
+ // std::cout << "b_p_bc: " << b_p_bc.transpose() << std::endl;
+ Vector3d i_p_ic = i_T_b.act(b_p_bc); // meas
+ // velocity due to gesticulation only in base frame
+ // -> set world frame = base frame and set body frame spatial vel to zero
+ Vector3d i_v_ic = i_R_b*data.vcom[0]; // meas
+ // Centroidal inertia and gesticulation kinetic momentum
+ Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
+ Matrix3d i_Iw = i_R_b*b_Iw*b_R_i; // !! rotate on both sides to keep a symmetric matrix
+ // gesticulation in base frame: just compute centroidal momentum with static q and vq
+ // Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
+ auto b_centro_momentum_gesti = computeCentroidalMomentum(model, data, q_static, dq_static);
+ auto i_centro_momentum_gesti = i_T_b.act(b_centro_momentum_gesti);
+ Vector3d i_Lc_gesti = i_centro_momentum_gesti.angular();
+
+ // Inertial kinematics
+ meas_ikin << i_p_ic, i_v_ic, i_omg_oi;
+
+ ////////////////////
+ // Force Torque
+ ////////////////////
+
+ VectorXd f_meas(nbc*3);
+ VectorXd tau_meas(nbc*3);
+ VectorXd kin_meas(nbc*7);
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ f_meas.segment<3>(3*i_ee) = l_f_l_vec[i_ee];
+ kin_meas.segment<3>(i_ee*3) = i_p_il_vec[i_ee];
+ kin_meas.segment<4>(nbc*3 + i_ee*4) = i_qvec_l_vec[i_ee];
+ }
+
+ VectorXd cap_ftp_data(dimc*nbc+3+3+nbc*7);
+ MatrixXd Qftp(dimc*nbc+3+3,dimc*nbc+3+3); // kin not in covariance mat
+ cap_ftp_data << f_meas, i_p_ic, i_omg_oi, kin_meas;
+ Qftp.setIdentity();
+ Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
+ Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(sen_ikin->getPbcNoiseStd(), 2);
+ Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
+ ////////////////////
+
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CImu->process();
+ auto CIKin = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti);
+ CIKin->process();
+ auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
+ CFTpreint->process();
+
+
+ // 1) detect feet in contact
+ std::vector<int> feet_in_contact;
+ int nb_feet_in_contact = 0;
+ std::unordered_map<int, Vector3d> kin_incontact;
+ for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // basic contact detection based on normal foot vel, things break if no foot is in contact
+ // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
+ // std::cout << "o_f_l_vec[i_ee]: " << o_f_l_vec[i_ee].transpose() << std::endl;
+ if (o_f_l_vec[i_ee](2) > fz_thresh){
+ feet_in_contact.push_back(i_ee);
+ nb_feet_in_contact++;
+ kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
+ }
+ }
+
+ CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ CPF->process();
+
+
+
+
+ // add zero vel artificial factor
+ if (problem->getTrajectory()->getFrameMap().size() > nb_kf){
+ auto kf_pair = problem->getTrajectory()->getFrameMap().rbegin();
+ std::cout << "New KF " << kf_pair->first << std::endl;
+ auto kf = kf_pair->second;
+
+ // CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf_pair->first);
+ // FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(), 0.00001*Matrix3d::Ones());
+ // FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, kf->getV(), nullptr, false);
+
+ nb_kf++;
+ KF_origin = kf;
+ }
+
+
+
+ ts_since_last_kf += dt;
+ if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
+ // problem->print(4,true,true,true);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+ ts_since_last_kf = 0;
+ }
+
+ // Store current state estimation
+ VectorComposite state_fbk = problem->getState(ts);
+ Vector3d o_p_oi_new = state_fbk['P'];
+ Vector4d o_qvec_i_new = state_fbk['O'];
+ Vector3d o_v_oi_new = state_fbk['V'];
+
+ Matrix3d o_R_i_new = Quaterniond(o_qvec_i_new).toRotationMatrix();
+ Matrix3d o_R_b_new = o_R_i_new * i_R_b;
+ Vector4d o_qvec_b_new = Quaterniond(o_R_b_new).coeffs();
+ Vector3d o_p_ob_new = o_p_oi_new + o_R_i_new * i_p_ib;
+ Vector3d o_v_ob_new = o_v_oi_new + o_R_b_new * i_omg_oi.cross(b_p_bi);
+
+ p_ob_fbk_v.push_back(o_p_ob_new);
+ q_ob_fbk_v.push_back(o_qvec_b_new);
+ v_ob_fbk_v.push_back(o_v_ob_new);
+ c_ob_fbk_v.push_back(state_fbk['C']);
+ cd_ob_fbk_v.push_back(state_fbk['D']);
+ Lc_ob_fbk_v.push_back(state_fbk['L']);
+ }
+
+
+
+ ///////////////////////////////////////////
+ //////////////// SOLVING //////////////////
+ ///////////////////////////////////////////
+ problem->print(4,true,true,true);
+ if (solve_end){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
+ std::cout << report << std::endl;
+ }
+
+ //////////////////////////////////////
+ //////////////////////////////////////
+ // STORE DATA //
+ //////////////////////////////////////
+ //////////////////////////////////////
+ std::fstream file_est;
+ std::fstream file_fbk;
+ std::fstream file_kfs;
+ std::fstream file_cov;
+ file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
+ file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
+ file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
+ file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
+ std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz\n";
+ file_est << header_est;
+ file_fbk << header_est;
+ std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz,bpx,bpy,bpz\n";
+ file_kfs << header_kfs;
+ std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLx,QLy,QLz,Qbpx,Qbpy,Qbpz\n";
+ file_cov << header_cov;
+
+ VectorComposite state_est;
+ double o_p_ob_carr[3*N];
+ double o_q_b_carr[4*N];
+ double o_v_ob_carr[3*N];
+ double o_p_oc_carr[3*N];
+ double o_v_oc_carr[3*N];
+ double o_Lc_carr[3*N];
+ for (unsigned int i=0; i < N; i++) {
+ state_est = problem->getState(t_arr[i]);
+ Vector3d i_omg_oi = i_omg_oi_v[i];
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = state_est['V'];
+
+ Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ // std::cout << "o_p_oi: " << o_p_oi.transpose() << std::endl;
+ // std::cout << "i_p_ib: " << i_p_ib.transpose() << std::endl;
+ // std::cout << "o_R_i * i_p_ib: " << (o_R_i * i_p_ib).transpose() << std::endl;
+ // std::cout << "o_p_oi + o_R_i * i_p_ib: " << (o_p_oi + o_R_i * i_p_ib).transpose() << std::endl;
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_b * (i_omg_oi - bias_acc_gyro.tail<3>()).cross(b_p_bi);
+
+ Vector3d o_p_oc = state_est['C'];
+ Vector3d o_v_oc = state_est['D'];
+ Vector3d o_Lc = state_est['L'];
+
+ mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_oc_carr+3*i, o_p_oc.data(), 3*sizeof(double));
+ mempcpy(o_v_oc_carr+3*i, o_v_oc.data(), 3*sizeof(double));
+ mempcpy(o_Lc_carr +3*i, o_Lc.data(), 3*sizeof(double));
+
+ file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << o_p_ob(0) << ","
+ << o_p_ob(1) << ","
+ << o_p_ob(2) << ","
+ << o_qvec_b(0) << ","
+ << o_qvec_b(1) << ","
+ << o_qvec_b(2) << ","
+ << o_qvec_b(3) << ","
+ << o_v_ob(0) << ","
+ << o_v_ob(1) << ","
+ << o_v_ob(2) << ","
+
+ << o_p_oc(0) << ","
+ << o_p_oc(1) << ","
+ << o_p_oc(2) << ","
+ << o_v_oc(0) << ","
+ << o_v_oc(1) << ","
+ << o_v_oc(2) << ","
+ << o_Lc(0) << ","
+ << o_Lc(1) << ","
+ << o_Lc(2)
+ << "\n";
+ }
+
+
+ VectorComposite kf_state;
+ CaptureBasePtr cap_ikin;
+ VectorComposite bp_bias_est;
+ CaptureBasePtr cap_imu;
+ VectorComposite bi_bias_est;
+ for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ auto kf = elt.second;
+ kf_state = kf->getState();
+ cap_ikin = kf->getCaptureOf(sen_ikin);
+ bp_bias_est = cap_ikin->getState();
+ cap_imu = kf->getCaptureOf(sen_imu);
+ bi_bias_est = cap_imu->getState();
+
+ file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << kf->getTimeStamp().get() << ","
+ << kf_state['P'](0) << ","
+ << kf_state['P'](1) << ","
+ << kf_state['P'](2) << ","
+ << kf_state['O'](0) << ","
+ << kf_state['O'](1) << ","
+ << kf_state['O'](2) << ","
+ << kf_state['O'](3) << ","
+ << kf_state['V'](0) << ","
+ << kf_state['V'](1) << ","
+ << kf_state['V'](2) << ","
+ << bi_bias_est['I'](0) << ","
+ << bi_bias_est['I'](1) << ","
+ << bi_bias_est['I'](2) << ","
+ << bi_bias_est['I'](3) << ","
+ << bi_bias_est['I'](4) << ","
+ << bi_bias_est['I'](5) << ","
+ << kf_state['C'](0) << ","
+ << kf_state['C'](1) << ","
+ << kf_state['C'](2) << ","
+ << kf_state['D'](0) << ","
+ << kf_state['D'](1) << ","
+ << kf_state['D'](2) << ","
+ << kf_state['L'](0) << ","
+ << kf_state['L'](1) << ","
+ << kf_state['L'](2) << ","
+ << bp_bias_est['I'](0) << ","
+ << bp_bias_est['I'](1) << ","
+ << bp_bias_est['I'](2)
+ << "\n";
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd QL = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
+ // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended...
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+ problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+ problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+ problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
+ problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
+ problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
+
+ solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
+ // std::cout << "Qbp\n" << Qbp << std::endl;
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+ file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << kf->getTimeStamp().get() << ","
+ << Qp(0,0) << ","
+ << Qp(1,1) << ","
+ << Qp(2,2) << ","
+ << Qo(0,0) << ","
+ << Qo(1,1) << ","
+ << Qo(2,2) << ","
+ << Qv(0,0) << ","
+ << Qv(1,1) << ","
+ << Qv(2,2) << ","
+ << Qbi(0,0) << ","
+ << Qbi(1,1) << ","
+ << Qbi(2,2) << ","
+ << Qbi(3,3) << ","
+ << Qbi(4,4) << ","
+ << Qbi(5,5) << ","
+ << Qc(0,0) << ","
+ << Qc(1,1) << ","
+ << Qc(2,2) << ","
+ << Qd(0,0) << ","
+ << Qd(1,1) << ","
+ << Qd(2,2) << ","
+ << QL(0,0) << ","
+ << QL(1,1) << ","
+ << QL(2,2) << ","
+ << Qbp(0,0) << ","
+ << Qbp(1,1) << ","
+ << Qbp(2,2)
+ << "\n";
+ }
+
+ for (unsigned int i=0; i < N; i++) {
+ file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t_arr[i] << ","
+ << p_ob_fbk_v[i](0) << ","
+ << p_ob_fbk_v[i](1) << ","
+ << p_ob_fbk_v[i](2) << ","
+ << q_ob_fbk_v[i](0) << ","
+ << q_ob_fbk_v[i](1) << ","
+ << q_ob_fbk_v[i](2) << ","
+ << q_ob_fbk_v[i](3) << ","
+ << v_ob_fbk_v[i](0) << ","
+ << v_ob_fbk_v[i](1) << ","
+ << v_ob_fbk_v[i](2) << ","
+ << c_ob_fbk_v[i](0) << ","
+ << c_ob_fbk_v[i](1) << ","
+ << c_ob_fbk_v[i](2) << ","
+ << cd_ob_fbk_v[i](0) << ","
+ << cd_ob_fbk_v[i](1) << ","
+ << cd_ob_fbk_v[i](2) << ","
+ << Lc_ob_fbk_v[i](0) << ","
+ << Lc_ob_fbk_v[i](1) << ","
+ << Lc_ob_fbk_v[i](2)
+ << "\n";
+ }
+
+ file_est.close();
+ file_kfs.close();
+ file_cov.close();
+
+
+ // Save trajectories in npz file
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+
+ // save estimates
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oc", o_p_oc_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oc", o_v_oc_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_Lc", o_Lc_carr, {N,3}, "a");
+
+}
diff --git a/demos/tree_manager.yaml b/demos/tree_manager.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ceb0dd77261e7361210a690b2ba563aaa58e7779
--- /dev/null
+++ b/demos/tree_manager.yaml
@@ -0,0 +1,7 @@
+config:
+ problem:
+ tree_manager:
+ n_fix_first_frames: 3
+ n_frames: 100000000000
+ type: TreeManagerSlidingWindow
+ viral_remove_empty_parent: true
diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque_preint.h
new file mode 100644
index 0000000000000000000000000000000000000000..12f4eb58c9e2a8b8d89e7923bea393e93bae48a5
--- /dev/null
+++ b/include/bodydynamics/capture/capture_force_torque_preint.h
@@ -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--------
+#ifndef CAPTURE_FORCE_TORQUE_PREINT_H
+#define CAPTURE_FORCE_TORQUE_PREINT_H
+
+//Wolf includes
+#include "bodydynamics/math/force_torque_delta_tools.h"
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_motion.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+
+#include <core/state_block/state_composite.h>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureForceTorquePreint);
+
+class CaptureForceTorquePreint : public CaptureMotion
+{
+ public:
+ /*
+ capture _data
+ name, size: description
+ f1 , 3 : force of foot 1 in foot 1 frame
+ f2 , 3 : force of foot 2 in foot 2 frame
+ tau1, 3 : torque of foot 1 in foot 1 frame
+ tau2, 3 : torque of foot 2 in foot 2 frame
+ pbc , 3 : position of COM relative to the base in base
+ wb , 3 : angular velocity of body frame in body frame
+ pbl1, 3 : position of foot 1 relative to the base in base frame
+ pbl2, 3 : position of foot 2 relative to the base in base frame
+ qbl1, 4 : orientation of foot 1 in base frame
+ qbl2, 4 : orientation of foot 2 in base frame
+ */
+ CaptureForceTorquePreint(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
+ CaptureMotionPtr _capture_motion_ptr, // to get the gyro bias
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ ~CaptureForceTorquePreint() override;
+
+ CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_; }
+ CaptureBasePtr getGyroCaptureOther(){ return cap_gyro_other_;}
+
+ private:
+ CaptureBasePtr cap_ikin_other_;
+ CaptureBasePtr cap_gyro_other_;
+};
+
+} // namespace wolf
+
+#endif // CAPTURE_FORCE_TORQUE_PREINT_H
diff --git a/include/bodydynamics/capture/capture_inertial_kinematics.h b/include/bodydynamics/capture/capture_inertial_kinematics.h
new file mode 100644
index 0000000000000000000000000000000000000000..70baff8f311f6fc17d19305559c959d62ef079a2
--- /dev/null
+++ b/include/bodydynamics/capture/capture_inertial_kinematics.h
@@ -0,0 +1,64 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_INERTIAL_KINEMATICS_H
+#define CAPTURE_INERTIAL_KINEMATICS_H
+
+//Wolf includes
+#include "bodydynamics/math/force_torque_delta_tools.h"
+#include "core/capture/capture_motion.h"
+#include "core/capture/capture_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureInertialKinematics);
+
+class CaptureInertialKinematics : public CaptureBase
+{
+ public:
+
+ CaptureInertialKinematics(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d & _B_Lc_m, // Centroidal angular momentum expressed in body frame
+ const Vector3d& _bias);
+
+ CaptureInertialKinematics(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d & _B_Lc_m); // Centroidal angular momentum expressed in body frame
+
+ ~CaptureInertialKinematics() override;
+
+ const Eigen::Vector9d& getData() {return data_;}
+ const Eigen::Matrix3d& getBIq() {return B_I_q_;}
+ const Eigen::Vector3d& getBLcm() {return B_Lc_m_;}
+
+ private:
+ Eigen::Vector9d data_;
+ Eigen::Matrix3d B_I_q_;
+ Eigen::Vector3d B_Lc_m_;
+};
+} // namespace wolf
+
+#endif // CAPTURE_INERTIAL_KINEMATICS_H
diff --git a/include/bodydynamics/capture/capture_leg_odom.h b/include/bodydynamics/capture/capture_leg_odom.h
new file mode 100644
index 0000000000000000000000000000000000000000..b1d8652e0f3f9bd8c0dbbb001bc10714fa3cf8b9
--- /dev/null
+++ b/include/bodydynamics/capture/capture_leg_odom.h
@@ -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 gtest_capture_leg_odom.h
+ *
+ * Created on: May 12, 2020
+ * \author: mfourmy
+ */
+
+
+#ifndef CAPTURE_LEG_ODOM_H
+#define CAPTURE_LEG_ODOM_H
+
+//Wolf includes
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_motion.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureLegOdom);
+
+typedef enum
+{
+ POINT_FEET_RELATIVE_KIN,
+ POINT_FEET_DIFFERENTIAL_KIN,
+ HUMANOID_FEET_RELATIVE_KIN
+} CaptureLegOdomType;
+
+
+class CaptureLegOdom : public CaptureMotion
+{
+ public:
+ /*
+ capture _data
+ */
+ CaptureLegOdom(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::MatrixXd& _data_kin,
+ Eigen::Matrix6d _data_cov, // for the moment only 6x6 mat, see if more involved computations are necessary (contact confidence...)
+ double dt,
+ CaptureLegOdomType lo_type);
+
+ ~CaptureLegOdom() override;
+
+};
+
+// inline Eigen::VectorXd CaptureLegOdom::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error_biases) const
+// {
+// return
+// }
+
+} // namespace wolf
+
+#endif // CAPTURE_LEG_ODOM_H
diff --git a/include/bodydynamics/capture/capture_point_feet_nomove.h b/include/bodydynamics/capture/capture_point_feet_nomove.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e57c0cdd71d3cc2cd85f196a51e16df57dce844
--- /dev/null
+++ b/include/bodydynamics/capture/capture_point_feet_nomove.h
@@ -0,0 +1,56 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ *
+ * Created on: Oct 23, 2020
+ * \author: mfourmy
+ */
+
+
+#ifndef CAPTURE_POINT_FEET_NOMOVE_H
+#define CAPTURE_POINT_FEET_NOMOVE_H
+
+//Wolf includes
+#include "core/capture/capture_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CapturePointFeetNomove);
+
+class CapturePointFeetNomove : public CaptureBase
+{
+ public:
+ CapturePointFeetNomove(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const std::unordered_map<int, Eigen::Vector3d>& kin_incontact
+ );
+
+ ~CapturePointFeetNomove() override;
+
+ // <foot in contact number (0,1,2,3), b_p_bf>
+ std::unordered_map<int, Eigen::Vector3d> kin_incontact_;
+};
+
+} // namespace wolf
+
+#endif // CAPTURE_POINT_FEET_NOMOVE_H
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 305669b8d8f661412589118e7fad7ca92fe5c351..85d74de654cfd2976eae06ab640086fec74870aa 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
/**
* \file factor_force_torque.h
*
@@ -12,9 +33,11 @@
#ifndef FACTOR_FORCE_TORQUE_H_
#define FACTOR_FORCE_TORQUE_H_
-#include "core/math/rotations.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/feature/feature_base.h"
+#include <core/math/rotations.h>
+#include <core/math/covariance.h>
+#include <core/factor/factor_autodiff.h>
+#include <core/feature/feature_base.h>
+
#include "bodydynamics/feature/feature_force_torque.h"
namespace wolf
@@ -27,16 +50,12 @@ class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3
public:
FactorForceTorque(const FeatureBasePtr& _feat,
const FrameBasePtr& _frame_other,
+ const StateBlockPtr& _sb_bp_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
- virtual ~FactorForceTorque() { /* nothing */ }
-
- virtual std::string getTopology() const override
- {
- return std::string("GEOM");
- }
+ ~FactorForceTorque() override { /* nothing */ }
/*
_ck : COM position in world frame, time k
@@ -71,12 +90,13 @@ class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3
// const Eigen::VectorXd& _bp,
// Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
- void computeMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov);
+ void retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov);
+ StateBlockPtr sb_bp_other_;
double mass_;
double dt_;
Eigen::Matrix<double,15,15> cov_meas_;
- // Eigen::Matrix<double, 9, 15> J_ny_nz_;
+ // Eigen::Matrix<double, 9, 15> J_ny_nz_; // cannot be modified in operator() since const function
// Eigen::Matrix9d errCov_;
};
@@ -88,43 +108,43 @@ namespace wolf {
FactorForceTorque::FactorForceTorque(
const FeatureBasePtr& _feat,
const FrameBasePtr& _frame_other,
+ const StateBlockPtr& _sb_bp_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorForceTorque",
+ TOP_GEOM,
+ _feat,
_frame_other, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
_processor_ptr,
_apply_loss_function,
_status,
- _feat->getFrame()->getStateBlock("C"), // COM position
- _feat->getFrame()->getStateBlock("D"), // COM velocity (bad name)
- _feat->getFrame()->getStateBlock("L"), // Angular momentum
- _frame_other->getStateBlock("C"), // COM position
- _frame_other->getStateBlock("D"), // COM velocity (bad name)
- _frame_other->getStateBlock("L"), // Angular momentum
- _frame_other->getStateBlock("B"), // BC relative position bias (bad name)
- _frame_other->getStateBlock("O") // Base orientation
- )
+ _feat->getFrame()->getStateBlock('C'), // COM position, current
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum, current
+ _frame_other->getStateBlock('C'), // COM position, previous
+ _frame_other->getStateBlock('D'), // COM velocity (bad name), previous
+ _frame_other->getStateBlock('L'), // Angular momentum, previous
+ _sb_bp_other, // BC relative position bias, previous
+ _frame_other->getStateBlock('O') // Base orientation, previous
+ ),
+ sb_bp_other_(_sb_bp_other)
{
FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
mass_ = feat->getMass();
dt_ = feat->getDt();
- computeMeasurementCovariance(feat, cov_meas_);
- // Eigen::Matrix<double, 9, 15> J_ny_nz; J_ny_nz.setZero();
- // computeJac(feat, mass_, dt_, _feat->getFrame()->getStateBlock("B")->getState(), J_ny_nz);
- // std::cout << cov_meas << std::endl;
- // errCov_ = J_ny_nz_ * cov_meas * J_ny_nz_.transpose(); // not necessary
+ retrieveMeasurementCovariance(feat, cov_meas_);
}
-void FactorForceTorque::computeMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov)
+void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov)
{
cov.setZero();
- cov.block<6,6>(0,0) = feat->getCovForcesMeas(); // reset some extra zeros
- cov.block<6,6>(6,6) = feat->getCovTorquesMeas(); // reset some extra zeros
+ cov.block<6,6>(0,0) = feat->getCovForcesMeas(); // reset some extra zeros
+ cov.block<6,6>(6,6) = feat->getCovTorquesMeas(); // reset some extra zeros
cov.block<3,3>(12,12) = feat->getCovPbcMeas();
}
@@ -232,10 +252,8 @@ bool FactorForceTorque::operator () (
// Recompute the error variable covariance according to the new bias bp estimation
- // computeJac(feat, mass_, dt_, feat->getFrame()->getStateBlock("B")->getState(), J_ny_nz_);
- // recomputeJac(feat, dt_, getFeature()->getFrame()->getStateBlock("B")->getState(), J_ny_nz_);
- Eigen::Matrix<double, 9, 15> J_ny_nz; J_ny_nz.setZero();
- computeJac(feat, mass_, dt_, feat->getFrame()->getStateBlock("B")->getState(), J_ny_nz);
+ Eigen::Matrix<double, 9, 15> J_ny_nz;
+ computeJac(feat, mass_, dt_, sb_bp_other_->getState(), J_ny_nz); // bp
Eigen::Matrix9d errCov = J_ny_nz * cov_meas_ * J_ny_nz.transpose();
errCov.block<6,6>(0,0) = errCov.block<6,6>(0,0) + 1e-12 * Eigen::Matrix6d::Identity();
@@ -250,33 +268,12 @@ bool FactorForceTorque::operator () (
err_cd = qkm.conjugate()*(cdk - cdkm - wolf::gravity()*dt_)
- (1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_);
-
- // std::cout << "err_cd.conjugate()\n" << err_cd << std::endl;
- // std::cout << "qkm.conjugate()\n" << qkm.coeffs().conjugate() << std::endl;
- // std::cout << "cdk.conjugate()\n" << ck.conjugate() << std::endl;
- // std::cout << "cdkm.conjugate()\n" << ckm.conjugate() << std::endl;
- // std::cout << "wolf::gravity()*dt_\n" << (wolf::gravity()*dt_).conjugate() << std::endl;
- // std::cout << "(1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_)\n" << ((1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_)).conjugate() << std::endl;
- // std::cout << "mass_\n" << mass_ << std::endl;
- // std::cout << "dt_\n" << dt_ << std::endl;
- // std::cout << "wolf::gravity()\n" << wolf::gravity() << std::endl;
-
err_Lc = qkm.conjugate()*(Lck - Lckm)
- ( bql1 * tau1 + (pbl1 - pbc + bpkm).cross(bql1 * f1)
+ bql2 * tau2 + (pbl2 - pbc + bpkm).cross(bql2 * f2));
-
- // std::cout << "\n\n\ncov_meas_\n" << cov_meas_ << std::endl;
- // std::cout << "\n\n\nerrCov\n" << errCov << std::endl;
- // std::cout << "\n\n\nerrCov.inverse()\n" << errCov.inverse() << std::endl;
- // Eigen::FullPivLU<Eigen::Matrix<double,9,15>> lu_decompJ_ny_nz(J_ny_nz);
- // std::cout << "\n\nJ_ny_nz rank: " << lu_decompJ_ny_nz.rank() << std::endl;
- // Eigen::FullPivLU<Eigen::Matrix9d> lu_decomperrCov(errCov);
- // std::cout << "errCov rank: " << lu_decomperrCov.rank() << std::endl;
-
-
- res = feat->computeSqrtUpper(errCov.inverse()) * err;
+ res = wolf::computeSqrtUpper(errCov.inverse()) * err;
return true;
}
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
new file mode 100644
index 0000000000000000000000000000000000000000..841bb63d6f94ffa9dc4ec115e5f4a3c3dd06d20a
--- /dev/null
+++ b/include/bodydynamics/factor/factor_force_torque_preint.h
@@ -0,0 +1,343 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_FORCE_TORQUE_PREINT_THETA_H_
+#define FACTOR_FORCE_TORQUE_PREINT_THETA_H_
+
+//Wolf includes
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/feature/feature_force_torque_preint.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/math/rotations.h"
+
+//Eigen include
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorForceTorquePreint);
+
+//class
+class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>
+{
+ public:
+ FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr,
+ const CaptureForceTorquePreintPtr& _cap_origin_ptr, // gets to bp1, bw1
+ const ProcessorBasePtr& _processor_ptr,
+ const CaptureBasePtr& _cap_ikin_other,
+ const CaptureBasePtr& _cap_gyro_other,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorForceTorquePreint() 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 _c1,
+ const T* const _cd1,
+ const T* const _Lc1,
+ const T* const _q1,
+ const T* const _bp1,
+ const T* const _bw1,
+ const T* const _c2,
+ const T* const _cd2,
+ const T* const _Lc2,
+ const T* const _q2,
+ T* _res) const;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
+ -> 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;)
+ * params :
+ * _c1 : COM position at time t1 in world frame
+ * _cd1: COM velocity at time t1 in world frame
+ * _Lc1: Angular momentum at time t1 in world frame
+ * _q1 : Base orientation at time t1
+ * _bp1: COM position measurement at time t1 in body frame
+ * _bw1: gyroscope bias at time t1 in body frame
+ * _c2 : COM position at time t2 in world frame
+ * _cd2: COM velocity at time t2 in world frame
+ * _Lc2: Angular momentum at time t2 in world frame
+ * _q2 : Base orientation at time t2
+ * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
+ */
+ template<typename D1, typename D2, typename D3, typename D4>
+ bool residual(const MatrixBase<D1> & _c1,
+ const MatrixBase<D1> & _cd1,
+ const MatrixBase<D1> & _Lc1,
+ const QuaternionBase<D2> & _q1,
+ const MatrixBase<D1> & _bp1,
+ const MatrixBase<D1> & _bw1,
+ const MatrixBase<D1> & _c2,
+ const MatrixBase<D1> & _cd2,
+ const MatrixBase<D1> & _Lc2,
+ const QuaternionBase<D3> & _q2,
+ MatrixBase<D4> & _res) const;
+
+ // Matrix<double,12,1> residual() const;
+ // double cost() const;
+
+ private:
+ /// Preintegrated delta
+ Vector3d dc_preint_;
+ Vector3d dcd_preint_;
+ Vector3d dLc_preint_;
+ Quaterniond dq_preint_;
+
+ // Biases used during preintegration
+ Vector3d pbc_bias_preint_;
+ Vector3d gyro_bias_preint_;
+
+ // Jacobians of preintegrated deltas wrt biases
+ Matrix3d J_dLc_pb_;
+ Matrix3d J_dc_wb_;
+ Matrix3d J_dcd_wb_;
+ Matrix3d J_dLc_wb_;
+ Matrix3d J_dq_wb_;
+
+ /// Metrics
+ double dt_; ///< delta-time and delta-time-squared between keyframes
+ double mass_; ///< constant total robot mass
+
+};
+
+///////////////////// IMPLEMENTATION ////////////////////////////
+
+inline FactorForceTorquePreint::FactorForceTorquePreint(
+ const FeatureForceTorquePreintPtr& _ftr_ptr,
+ const CaptureForceTorquePreintPtr& _cap_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ const CaptureBasePtr& _cap_ikin_other,
+ const CaptureBasePtr& _cap_gyro_other,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>(
+ "FactorForceTorquePreint",
+ TOP_MOTION,
+ _ftr_ptr,
+ _cap_origin_ptr->getFrame(),
+ _cap_origin_ptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _cap_origin_ptr->getFrame()->getStateBlock('C'),
+ _cap_origin_ptr->getFrame()->getStateBlock('D'),
+ _cap_origin_ptr->getFrame()->getStateBlock('L'),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_ikin_other->getSensorIntrinsic(),
+ _cap_gyro_other->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getStateBlock('C'),
+ _ftr_ptr->getFrame()->getStateBlock('D'),
+ _ftr_ptr->getFrame()->getStateBlock('L'),
+ _ftr_ptr->getFrame()->getO()
+ ),
+ dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
+ dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
+ dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
+ dq_preint_(_ftr_ptr->getMeasurement().data()+9),
+ pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()),
+ gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()),
+ J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
+ J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
+ J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
+ J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias
+ J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
+ mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
+{
+//
+}
+
+template<typename T>
+inline bool FactorForceTorquePreint::operator ()(const T* const _c1,
+ const T* const _cd1,
+ const T* const _Lc1,
+ const T* const _q1,
+ const T* const _bp1,
+ const T* const _bw1,
+ const T* const _c2,
+ const T* const _cd2,
+ const T* const _Lc2,
+ const T* const _q2,
+ T* _res) const
+{
+ Map<const Matrix<T,3,1> > c1(_c1);
+ Map<const Matrix<T,3,1> > cd1(_cd1);
+ Map<const Matrix<T,3,1> > Lc1(_Lc1);
+ Map<const Quaternion<T> > q1(_q1);
+ Map<const Matrix<T,3,1> > bp1(_bp1);
+ Map<const Matrix<T,3,1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw]
+ Map<const Matrix<T,3,1> > c2(_c2);
+ Map<const Matrix<T,3,1> > cd2(_cd2);
+ Map<const Matrix<T,3,1> > Lc2(_Lc2);
+ Map<const Quaternion<T> > q2(_q2);
+ Map<Matrix<T,12,1> > res(_res);
+
+ residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
+
+ return true;
+}
+
+template<typename D1, typename D2, typename D3, typename D4>
+inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
+ const MatrixBase<D1> & _cd1,
+ const MatrixBase<D1> & _Lc1,
+ const QuaternionBase<D2> & _q1,
+ const MatrixBase<D1> & _bp1,
+ const MatrixBase<D1> & _bw1,
+ const MatrixBase<D1> & _c2,
+ const MatrixBase<D1> & _cd2,
+ const MatrixBase<D1> & _Lc2,
+ const QuaternionBase<D3> & _q2,
+ MatrixBase<D4> & _res) const
+{
+ /* Help for the Imu residual function
+ *
+ * Notations:
+ * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv]
+ * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv]
+ * b* : a bias
+ * J* : a Jacobian matrix
+ *
+ * We use the following functions from imu_tools.h:
+ * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
+ * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T
+ * T = diff(D1,D2) : minus operator, T = D2 (-) D1
+ *
+ * Two methods are possible (select with #define below this note) :
+ *
+ * Computations common to methods 1 and 2:
+ * D_exp = betweenStates(x1,x2,dt) // Predict delta from states
+ * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
+ *
+ * Method 1:
+ * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change
+ * T_err = diff(D_exp, D_corr) // compare against expected delta
+ * res = W.sqrt * T_err
+ *
+ * results in:
+ * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
+ *
+ * Method 2:
+ * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta
+ * T_err = T_diff - T_step // the difference should match the correction step due to bias change
+ * res = W.sqrt * err
+ *
+ * results in :
+ * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
+ *
+ * NOTE: See optimization report at the end of this file for comparisons of both methods.
+ */
+
+ //needed typedefs
+ typedef typename D1::Scalar T;
+
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12);
+
+ // 1. Expected delta from states
+ Matrix<T, 3, 1 > dc_exp;
+ Matrix<T, 3, 1 > dcd_exp;
+ Matrix<T, 3, 1 > dLc_exp;
+ Quaternion<T> dq_exp;
+
+ bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp);
+
+ // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
+
+ // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
+ Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
+
+ // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
+ Matrix<T,3,1> dc_correct;
+ Matrix<T,3,1> dcd_correct;
+ Matrix<T,3,1> dLc_correct;
+ Quaternion<T> dq_correct;
+
+ bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(),
+ dc_step, dcd_step, dLc_step, do_step,
+ dc_correct, dcd_correct, dLc_correct, dq_correct);
+
+ // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
+ // Note the Dt here is zero because it's the delta-time between the same time stamps!
+ Matrix<T, 12, 1> d_error;
+ Map<Matrix<T, 3, 1> > dc_error (d_error.data() );
+ Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3);
+ Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6);
+ Map<Matrix<T, 3, 1> > do_error (d_error.data() + 9);
+
+
+ bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp,
+ dc_correct, dcd_correct, dLc_correct, dq_correct,
+ dc_error, dcd_error, dLc_error, do_error);
+
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
+
+ return true;
+}
+
+// Matrix<double,12,1> FactorForceTorquePreint::residual() const
+// {
+// Matrix<double,12,1> res;
+
+
+// FrameBasePtr frm_prev = getFrameOther();
+// FrameBasePtr frm_curr = getFeature()->getFrame();
+
+// CaptureBaseWPtrList cap_lst = getCaptureOtherList(); // !! not retrieving the rigt captures...
+// auto cap_ikin_other = cap_lst.front().lock();
+// auto cap_gyro_other = cap_lst.back().lock();
+
+// Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data());
+// Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data());
+// Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data());
+// Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data());
+// Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data());
+// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu = [ba, bw]
+// Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data());
+// Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data());
+// Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data());
+// Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data());
+
+// residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
+
+// return res;
+// }
+
+// double FactorForceTorquePreint::cost() const
+// {
+// Matrix<double,12,1> toto = residual();
+// }
+
+} // namespace wolf
+
+#endif
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index 28e41ee822442d46fd12767bb363590ac3c36223..c0b36d22bdd65b63b575e6d80863ea2eea978405 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
/**
* \file factor_inertial_kinematics.h
*
@@ -25,22 +46,21 @@ class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
- virtual ~FactorInertialKinematics() { /* nothing */ }
+ ~FactorInertialKinematics() override { /* nothing */ }
- virtual std::string getTopology() const override
- {
- return std::string("GEOM");
- }
+ // to keep track of the imu bias, not the standard way to proceed
+ StateBlockPtr sb_bias_imu_;
/*
+ Factor state blocks:
_pb: W_p_WB -> base position in world frame
_qb: W_q_B -> base orientation in world frame
_vb: W_v_WB -> base velocity in world frame
_c: W_p_WC -> COM position in world frame
_cd: W_v_WC -> COM velocity in world frame
- _Lc: W_Lc -> angular momentum at the COM in world frame
+ _Lc: W_Lc -> angular momentum at the COM in body frame
_bp: B_b_BC -> bias on relative body->COM position kinematic measurement
- _baw: 6D IMU bias containing [acc bias, gyro bias]
+ _baw: 6d Imu bias containing [acc bias, gyro bias]
*/
template<typename T>
bool operator () (
@@ -53,6 +73,10 @@ class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics,
const T* const _bp,
const T* const _baw,
T* _res) const;
+
+ Vector9d residual() const;
+ double cost() const;
+
};
} /* namespace wolf */
@@ -67,6 +91,8 @@ FactorInertialKinematics::FactorInertialKinematics(
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorInertialKinematics",
+ TOP_GEOM,
+ _feat,
nullptr, // _frame_other_ptr
nullptr, // _capture_other_ptr
nullptr, // _feature_other_ptr
@@ -77,15 +103,37 @@ FactorInertialKinematics::FactorInertialKinematics(
_feat->getFrame()->getP(),
_feat->getFrame()->getO(),
_feat->getFrame()->getV(),
- _feat->getFrame()->getStateBlock("C"), // COM position
- _feat->getFrame()->getStateBlock("D"), // COM velocity (bad name)
- _feat->getFrame()->getStateBlock("L"), // Angular momentum
- _feat->getFrame()->getStateBlock("B"), // BC relative position bias (bad name)
+ _feat->getFrame()->getStateBlock('C'), // COM position
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name)
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum
+ _feat->getCapture()->getStateBlock('I'), // body-to-COM relative position bias
_sb_bias_imu
- )
+ ),
+ sb_bias_imu_(_sb_bias_imu)
{
}
+Vector9d FactorInertialKinematics::residual() const{
+ Vector9d res;
+ double * pb, * qb, * vb, * c, * cd, * Lc, * bp, * baw;
+ pb = getFrame()->getP()->getState().data();
+ qb = getFrame()->getO()->getState().data();
+ vb = getFrame()->getV()->getState().data();
+ c = getFrame()->getStateBlock('C')->getState().data();
+ cd = getFrame()->getStateBlock('D')->getState().data();
+ Lc = getFrame()->getStateBlock('L')->getState().data();
+ bp = getCapture()->getStateBlock('I')->getState().data();
+ baw = sb_bias_imu_->getState().data();
+
+ operator() (pb, qb, vb, c, cd, Lc, bp, baw, res.data());
+ // std::cout << "res: " << res.transpose() << std::endl;
+ return res;
+}
+
+double FactorInertialKinematics::cost() const{
+ return residual().squaredNorm();
+}
+
template<typename T>
bool FactorInertialKinematics::operator () (
const T* const _pb,
@@ -98,34 +146,34 @@ bool FactorInertialKinematics::operator () (
const T* const _baw,
T* _res) const
{
- FeatureInertialKinematicsPtr feat = std::static_pointer_cast<FeatureInertialKinematics>(getFeature());
-
// State variables instanciation
- Eigen::Map<const Eigen::Matrix<T,3,1> > pb(_pb);
- Eigen::Map<const Eigen::Quaternion<T> > qb(_qb);
- Eigen::Map<const Eigen::Matrix<T,3,1> > vb(_vb);
- Eigen::Map<const Eigen::Matrix<T,3,1> > c(_c);
- Eigen::Map<const Eigen::Matrix<T,3,1> > cd(_cd);
- Eigen::Map<const Eigen::Matrix<T,3,1> > Lc(_Lc);
- Eigen::Map<const Eigen::Matrix<T,3,1> > bp(_bp);
- Eigen::Map<const Eigen::Matrix<T,6,1> > baw(_baw);
- Eigen::Map<Eigen::Matrix<T,9,1> > res(_res);
+ Map<const Matrix<T,3,1> > pb(_pb);
+ Map<const Quaternion<T> > qb(_qb);
+ Map<const Matrix<T,3,1> > vb(_vb);
+ Map<const Matrix<T,3,1> > c(_c);
+ Map<const Matrix<T,3,1> > cd(_cd);
+ Map<const Matrix<T,3,1> > Lc(_Lc);
+ Map<const Matrix<T,3,1> > bp(_bp);
+ Map<const Matrix<T,6,1> > baw(_baw);
+
+ Map<Matrix<T,9,1> > res(_res);
+
+ FeatureInertialKinematicsPtr feat = std::static_pointer_cast<FeatureInertialKinematics>(getFeature());
// Measurements retrieval
- Eigen::Map<const Eigen::Vector3d> pBC_m(getMeasurement().data()); // B_p_BC
- Eigen::Map<const Eigen::Vector3d> vBC_m(getMeasurement().data() + 3); // B_v_BC
- Eigen::Map<const Eigen::Vector3d> w_m(getMeasurement().data() + 6); // B_wB
+ Map<const Vector3d> pBC_m(getMeasurement().data() ); // B_p_BC
+ Map<const Vector3d> vBC_m(getMeasurement().data() + 3); // B_v_BC
+ Map<const Vector3d> w_m (getMeasurement().data() + 6); // B_wB
// Error variable instanciation
- Eigen::Matrix<T, 9, 1> err;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_c(err.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_cd(err.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_Lc(err.data() + 6);
-
- err_c = pBC_m - (qb.inverse()*(c - pb) + bp);
- err_cd = vBC_m + (w_m - baw.tail(3)).cross(pBC_m - bp) - qb.inverse()*(cd - vb);
- // err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() - qb.inverse()*Lc;
- err_Lc = feat->getBLcm() - feat->getBIq()*baw.tail(3) - qb.inverse()*Lc; // w_m is actually used to compute BLcm beforehand
+ Matrix<T, 9, 1> err;
+ Map<Matrix<T, 3, 1> > err_c (err.data() );
+ Map<Matrix<T, 3, 1> > err_cd(err.data() + 3);
+ Map<Matrix<T, 3, 1> > err_Lc(err.data() + 6);
+
+ err_c = pBC_m - (qb.conjugate()*(c - pb) + bp);
+ err_cd = vBC_m + (w_m - baw.tail(3)).cross(pBC_m - bp) - qb.conjugate()*(cd - vb);
+ err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() - qb.conjugate()*Lc;
res = getMeasurementSquareRootInformationUpper() * err;
diff --git a/include/bodydynamics/factor/factor_point_feet_nomove.h b/include/bodydynamics/factor/factor_point_feet_nomove.h
new file mode 100644
index 0000000000000000000000000000000000000000..58e7804f43b3c3e1ffe0de8e85711c3f3d595e06
--- /dev/null
+++ b/include/bodydynamics/factor/factor_point_feet_nomove.h
@@ -0,0 +1,166 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file factor_point_feet_nomove.h
+ *
+ * Created on: Feb 22, 2020
+ * \author: mfourmy
+ */
+
+#ifndef FACTOR_POINT_FEET_NOMOVE_H_
+#define FACTOR_POINT_FEET_NOMOVE_H_
+
+#include "core/factor/factor_autodiff.h"
+
+
+// namespace
+// {
+
+// constexpr std::size_t RESIDUAL_SIZE = 3;
+// constexpr std::size_t POSITION_SIZE = 3;
+// constexpr std::size_t ORIENTATION_SIZE = 4;
+
+// }
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorPointFeetNomove);
+
+class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove,
+ 3,
+ 3,
+ 4,
+ 3,
+ 4
+ >
+{
+ public:
+ FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorPointFeetNomove() override { /* nothing */ }
+
+ /*
+ Factor state blocks:
+ _pb: W_p_WB -> base position in world frame
+ _qb: W_q_B -> base orientation in world frame
+ _pbm: W_p_WB -> base position in world frame, previous KF
+ _qbm: W_q_B -> base orientation in world frame, previous KF
+ */
+ template<typename T>
+ bool operator () (
+ const T* const _pb,
+ const T* const _qb,
+ const T* const _pbm,
+ const T* const _qbm,
+ T* _res) const;
+
+ // Vector9d residual() const;
+ // double cost() const;
+
+};
+
+} /* namespace wolf */
+
+
+namespace wolf {
+
+FactorPointFeetNomove::FactorPointFeetNomove(
+ const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff("FactorPointFeetNomove",
+ TOP_GEOM,
+ _ftr_current_ptr,
+ _frame_past_ptr, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(),
+ _ftr_current_ptr->getFrame()->getO(),
+ _frame_past_ptr->getP(),
+ _frame_past_ptr->getO()
+ )
+{
+}
+
+// Vector9d FactorPointFeetNomove::residual() const{
+// Vector9d res;
+// double * pb, * qb, * vb, * c, * cd, * Lc, * bp, * baw;
+// pb = getFrame()->getP()->getState().data();
+// qb = getFrame()->getO()->getState().data();
+// vb = getFrame()->getV()->getState().data();
+// c = getFrame()->getStateBlock('C')->getState().data();
+// cd = getFrame()->getStateBlock('D')->getState().data();
+// Lc = getFrame()->getStateBlock('L')->getState().data();
+// bp = getCapture()->getStateBlock('I')->getState().data();
+// baw = sb_bias_imu_->getState().data();
+
+// operator() (pb, qb, vb, c, cd, Lc, bp, baw, res.data());
+// // std::cout << "res: " << res.transpose() << std::endl;
+// return res;
+// }
+
+// double FactorPointFeetNomove::cost() const{
+// return residual().squaredNorm();
+// }
+
+template<typename T>
+bool FactorPointFeetNomove::operator () (
+ const T* const _pb,
+ const T* const _qb,
+ const T* const _pbm,
+ const T* const _qbm,
+ T* _res) const
+{
+ Map<Matrix<T,3,1> > res(_res);
+
+ // State variables instanciation
+ Map<const Matrix<T,3,1> > pb(_pb);
+ Map<const Quaternion<T> > qb(_qb);
+ Map<const Matrix<T,3,1> > pbm(_pbm);
+ Map<const Quaternion<T> > qbm(_qbm);
+
+ // Measurements retrieval
+ Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
+ Matrix<T,3,1> bm_p_bml = meas.topRows(3); // Bprev_p_BprevL
+ Matrix<T,3,1> b_p_bl = meas.bottomRows(3); // B_p_BL
+
+ Matrix<T,3,1> err = (pbm + qbm*bm_p_bml) - (pb + qb*b_p_bl);
+
+ res = getMeasurementSquareRootInformationUpper() * err;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif /* FACTOR__POINT_FEET_NOMOVE_H_ */
diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h
index b2434de9b82691a2990765d5a45f26563756f5fd..bbe2fda19d7fcdbde4e88aa564efc3547a6293ef 100644
--- a/include/bodydynamics/feature/feature_force_torque.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_FORCE_TORQUE_H_
#define FEATURE_FORCE_TORQUE_H_
@@ -27,7 +48,7 @@ class FeatureForceTorque : public FeatureBase
const Eigen::Matrix<double,14,14>& _cov_kin_meas = Eigen::Matrix<double,14,14>::Zero()
);
- virtual ~FeatureForceTorque();
+ ~FeatureForceTorque() override;
const double & getDt(){return dt_;}
const double & getMass(){return mass_;}
@@ -50,7 +71,7 @@ class FeatureForceTorque : public FeatureBase
void setCovForcesMeas(const Eigen::Matrix6d& _cov_forces_meas){cov_forces_meas_ = _cov_forces_meas;}
void setCovTorquesMeas(const Eigen::Matrix6d& _cov_torques_meas){cov_torques_meas_ = _cov_torques_meas;}
void setCovPbcMeas(const Eigen::Matrix3d& _cov_pbc_meas){cov_pbc_meas_ = _cov_pbc_meas;}
- void getCovKinMeas(const Eigen::Matrix<double,14,14>& _cov_kin_meas){cov_kin_meas_ = _cov_kin_meas;}
+ void setCovKinMeas(const Eigen::Matrix<double,14,14>& _cov_kin_meas){cov_kin_meas_ = _cov_kin_meas;}
private:
double dt_;
diff --git a/include/bodydynamics/feature/feature_force_torque_preint.h b/include/bodydynamics/feature/feature_force_torque_preint.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a03d2a73e668123b29dd948c7988795ea45f9cf
--- /dev/null
+++ b/include/bodydynamics/feature/feature_force_torque_preint.h
@@ -0,0 +1,95 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_FORCE_TORQUE_PREINT_H_
+#define FEATURE_FORCE_TORQUE_PREINT_H_
+
+//Wolf includes
+#include <core/feature/feature_base.h>
+#include <core/common/wolf.h>
+
+//std includes
+
+namespace wolf {
+
+//WOLF_PTR_TYPEDEFS(CaptureImu);
+WOLF_PTR_TYPEDEFS(FeatureForceTorquePreint);
+
+class FeatureForceTorquePreint : public FeatureBase
+{
+ public:
+
+ /** \brief Constructor from and measures
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases
+ * \param _pbc_bias COM position relative to bias bias of origin frame time
+ * \param _gyro_bias gyroscope bias of origin frame time
+ * \param _cap_ftpreint_ptr pointer to parent CaptureMotion (CaptureForceTorquePreint)
+ */
+ FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _biases_preint,
+ const Eigen::Matrix<double,12,6>& _J_delta_biases);
+
+// /** \brief Constructor from capture pointer
+// *
+// * \param _cap_imu_ptr pointer to parent CaptureMotion
+// */
+// FeatureForceTorquePreint(CaptureMotionPtr _cap_imu_ptr);
+
+ ~FeatureForceTorquePreint() override = default;
+
+ const Eigen::Vector3d& getPbcBiasPreint() const;
+ const Eigen::Vector3d& getGyroBiasPreint() const;
+ const Eigen::Matrix<double, 12, 6>& getJacobianBias() const;
+
+ private:
+
+ // Used biases
+ Eigen::Vector3d pbc_bias_preint_; ///< Acceleration bias used for delta preintegration
+ Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
+
+ Eigen::Matrix<double, 12, 6> J_delta_bias_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+
+inline const Eigen::Vector3d& FeatureForceTorquePreint::getPbcBiasPreint() const
+{
+ return pbc_bias_preint_;
+}
+
+inline const Eigen::Vector3d& FeatureForceTorquePreint::getGyroBiasPreint() const
+{
+ return gyro_bias_preint_;
+}
+
+inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorquePreint::getJacobianBias() const
+{
+ return J_delta_bias_;
+}
+
+} // namespace wolf
+
+#endif // FEATURE_FORCE_TORQUE_PREINT_H_
diff --git a/include/bodydynamics/feature/feature_inertial_kinematics.h b/include/bodydynamics/feature/feature_inertial_kinematics.h
index 00685a7102cc4d86713c859eb2657035f6be5201..bb9588ffbc24e78162bedddc5df7a914f7d55c63 100644
--- a/include/bodydynamics/feature/feature_inertial_kinematics.h
+++ b/include/bodydynamics/feature/feature_inertial_kinematics.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_INERTIAL_KINEMATICS_H_
#define FEATURE_INERTIAL_KINEMATICS_H_
@@ -14,23 +35,47 @@ class FeatureInertialKinematics : public FeatureBase
{
public:
- FeatureInertialKinematics(const Eigen::Vector9d & _measurement,
- const Eigen::Matrix9d & _meas_uncertainty,
- const Eigen::Matrix3d & _BIq,
- const Eigen::Vector3d & _BLcm,
- UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
- virtual ~FeatureInertialKinematics();
+ FeatureInertialKinematics(const Eigen::Vector9d & _meas_pvw,
+ const Eigen::Matrix9d & _Qerr,
+ const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d & _B_Lc_m, // Centroidal angular momentum expressed in body frame
+ UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
+ ~FeatureInertialKinematics() override = default;
+
const Eigen::Matrix3d & getBIq(){return BIq_;}
const Eigen::Vector3d & getBLcm(){return BLcm_;}
void setBIq(const Eigen::Matrix3d & _BIq){BIq_ = _BIq;}
void setBLcm(const Eigen::Vector3d & _BLcm){BLcm_ = _BLcm;}
+
private:
Eigen::Matrix3d BIq_;
Eigen::Vector3d BLcm_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
};
+ Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& _w_unb,
+ const Eigen::Vector3d& _p_unb,
+ const Eigen::Matrix3d& _Iq);
+
+ Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& _Qp,
+ const Eigen::Matrix3d& _Qv,
+ const Eigen::Matrix3d& _Qw,
+ const Eigen::Vector3d& _w_unb,
+ const Eigen::Vector3d& _p_unb,
+ const Eigen::Matrix3d& _Iq);
+
+ void recomputeIKinCov(const Eigen::Matrix3d& Qp,
+ const Eigen::Matrix3d& Qv,
+ const Eigen::Matrix3d& Qw,
+ const Eigen::Vector3d& p_unb,
+ const Eigen::Vector3d& w_unb,
+ const Eigen::Matrix3d& Iq);
+
} // namespace wolf
#endif
diff --git a/include/bodydynamics/math/force_torque_delta_tools.h b/include/bodydynamics/math/force_torque_delta_tools.h
index d778e96b7c3f670c415006af63db85bfd13d3905..fdc3d3a8034f5fa4372b0b0796b27b60ce889eb5 100644
--- a/include/bodydynamics/math/force_torque_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_delta_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
*
@@ -36,7 +57,7 @@
* - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
* - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
* - exp_FT: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 (+) exp_IMU(d)
+ * - plus: D2 = D1 (+) exp_imu(d)
* - diff: d = log_FT( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes
*/
@@ -49,10 +70,10 @@ using namespace Eigen;
template<typename D1, typename D2, typename D3, typename D4>
inline void identity(MatrixBase<D1>& dc, MatrixBase<D2>& dcd, MatrixBase<D3>& dLc, QuaternionBase<D4>& dq)
{
- dc = MatrixBase<D1>::Zero(3,1);
+ dc = MatrixBase<D1>::Zero(3,1);
dcd = MatrixBase<D3>::Zero(3,1);
dLc = MatrixBase<D3>::Zero(3,1);
- dq = QuaternionBase<D4>::Identity();
+ dq = QuaternionBase<D4>::Identity();
}
template<typename D1, typename D2, typename D3, typename D4>
@@ -62,10 +83,10 @@ inline void identity(MatrixBase<D1>& dc, MatrixBase<D2>& dcd, MatrixBase<D3>& dL
typedef typename D2::Scalar T2;
typedef typename D3::Scalar T3;
typedef typename D4::Scalar T4;
- dc << T1(0), T1(0), T1(0);
+ dc << T1(0), T1(0), T1(0);
dcd << T2(0), T2(0), T2(0);
dLc << T3(0), T3(0), T3(0);
- dq << T4(0), T4(0), T4(0), T4(1);
+ dq << T4(0), T4(0), T4(0), T4(1);
}
template<typename T = double>
@@ -198,435 +219,506 @@ inline void compose(const MatrixBase<D1>& d1,
MatrixSizeCheck< 12, 12>::check(J_sum_d1);
MatrixSizeCheck< 12, 12>::check(J_sum_d2);
- // // Some useful temporaries
- // Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment<4>(9));
- // Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment<4>(9));
- // Matrix<typename D2::Scalar, 3, 1> dc2 = d1.head<3>();
- // Matrix<typename D2::Scalar, 3, 1> dcd2 = d1.segment<3>(3);
- // Matrix<typename D2::Scalar, 3, 1> dLc2 = d1.segment<3>(6);
+ // Some useful temporaries
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 9 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2( 0 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2( 6 ) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 9 ) );
+
+ Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(dq1);
+ Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(dq2);
// // Jac wrt first delta
- // J_sum_d1.setIdentity();
- // J_sum_d1.block<3,3>(0,3) = Matrix3d::Identity() * dt;
- // J_sum_d1.block<3,3>(0,9) = - dR1 * skew(dc2);
- // J_sum_d1.block<3,3>(3,9) = - dR1 * skew(dcd2);
- // J_sum_d1.block<3,3>(6,9) = - dR1 * skew(dLc2);
- // J_sum_d1.block<3,3>(9,9) = dR2.transpose();
-
- // // Jac wrt second delta
- // J_sum_d2.setIdentity();
- // J_sum_d2.block<3,3>(0,0) = dR1;
- // J_sum_d2.block<3,3>(3,3) = dR1;
- // J_sum_d2.block<3,3>(6,6) = dR1;
+ J_sum_d1.setIdentity();
+ J_sum_d1.template block<3,3>(0,3) = Matrix3d::Identity() * dt;
+ J_sum_d1.template block<3,3>(0,9) = - dR1 * skew(dc2);
+ J_sum_d1.template block<3,3>(3,9) = - dR1 * skew(dcd2);
+ J_sum_d1.template block<3,3>(6,9) = - dR1 * skew(dLc2);
+ J_sum_d1.template block<3,3>(9,9) = dR2.transpose();
+
+ // // // Jac wrt second delta
+ J_sum_d2.setIdentity();
+ J_sum_d2.template block<3,3>(0,0) = dR1;
+ J_sum_d2.template block<3,3>(3,3) = dR1;
+ J_sum_d2.template block<3,3>(6,6) = dR1;
// 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, typename D7, typename D8, typename D9, class T>
-// inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
-// const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
-// const T dt,
-// MatrixBase<D7>& diff_p, QuaternionBase<D8>& diff_q, MatrixBase<D9>& diff_v )
-// {
-// MatrixSizeCheck<3, 1>::check(dp1);
-// MatrixSizeCheck<3, 1>::check(dv1);
-// MatrixSizeCheck<3, 1>::check(dp2);
-// MatrixSizeCheck<3, 1>::check(dv2);
-// MatrixSizeCheck<3, 1>::check(diff_p);
-// MatrixSizeCheck<3, 1>::check(diff_v);
-
-// diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
-// diff_v = dq1.conjugate() * ( dv2 - dv1 );
-// diff_q = dq1.conjugate() * dq2;
-// }
-
-// 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<10, 1>::check(d1);
-// MatrixSizeCheck<10, 1>::check(d2);
-// MatrixSizeCheck<10, 1>::check(d2_minus_d1);
-
-// 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 ( & d2_minus_d1(0) );
-// Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(3) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & d2_minus_d1(7) );
-
-// between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v);
-// }
-
-// template<typename D1, typename D2, class T>
-// inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
-// const MatrixBase<D2>& d2,
-// T dt)
-// {
-// Matrix<typename D1::Scalar, 10, 1> diff;
-// between(d1, d2, dt, diff);
-// return diff;
-// }
-
-// 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 ) );
-
-// p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
-// v_plus_d = v + gravity()*dt + q*dv;
-// q_plus_d = q*dq; // dq here to avoid possible aliasing between x and x_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<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, typename D7, typename D8, typename D9>
-// inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
-// const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2,
-// MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v )
-// {
-// plus_p = dp1 + dp2;
-// plus_v = dv1 + dv2;
-// plus_q = dq1 * exp_q(do2);
-// }
-
-// 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, 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 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) );
-
-// plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p);
-// }
-
-// template<typename D1, typename D2>
-// inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& d1,
-// const MatrixBase<D2>& d2)
-// {
-// Matrix<typename D1::Scalar, 10, 1> ret;
-// plus(d1, d2, ret);
-// return ret;
-// }
-
-// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-// 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 )
-// {
-// diff_p = dp2 - dp1;
-// diff_v = dv2 - dv1;
-// diff_o = log_q(dq1.conjugate() * dq2);
-// }
-
-// 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, 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 ( & err(0) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & err(6) );
-
-// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, 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, 9, 1> diff(const MatrixBase<D1>& d1,
-// const MatrixBase<D2>& d2)
-// {
-// Matrix<typename D1::Scalar, 9, 1> ret;
-// diff(d1, d2, ret);
-// return ret;
-// }
-
-// 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,
-// QuaternionBase<D4>& dq,
-// MatrixBase<D5>& dv)
-// {
-// MatrixSizeCheck<3,1>::check(a);
-// MatrixSizeCheck<3,1>::check(w);
-// MatrixSizeCheck<3,1>::check(dp);
-// MatrixSizeCheck<3,1>::check(dv);
-
-// dp = 0.5 * a * dt * dt;
-// dv = a * dt;
-// dq = exp_q(w * dt);
-// }
-
-// template<typename D1>
-// inline Matrix<typename D1::Scalar, 10, 1> body2delta(const MatrixBase<D1>& body,
-// const typename D1::Scalar& dt)
-// {
-// MatrixSizeCheck<6,1>::check(body);
-
-// typedef typename D1::Scalar T;
-
-// Matrix<T, 10, 1> delta;
-
-// Map< Matrix<T, 3, 1>> dp ( & delta(0) );
-// Map< Quaternion<T>> dq ( & delta(3) );
-// Map< Matrix<T, 3, 1>> dv ( & delta(7) );
-
-// body2delta(body.block(0,0,3,1), body.block(3,0,3,1), dt, dp, dq, dv);
-
-// return delta;
-// }
-
-// 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<6,1>::check(body);
-// MatrixSizeCheck<9,6>::check(jac_body);
-
-// typedef typename D1::Scalar T;
-
-// delta = body2delta(body, dt);
-
-// Matrix<T, 3, 1> w = body.block(3,0,3,1);
-
-// jac_body.setZero();
-// jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
-// jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
-// jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
-// }
-
-// 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 imu
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12, class T>
+inline void between(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& dc2, const MatrixBase<D6>& dcd2, const MatrixBase<D7>& dLc2, const QuaternionBase<D8>& dq2,
+ const T dt,
+ MatrixBase<D9>& diff_c, MatrixBase<D10>& diff_cd, MatrixBase<D11>& diff_Lc, QuaternionBase<D12>& diff_q )
+{
+ MatrixSizeCheck<3, 1>::check(dc1);
+ MatrixSizeCheck<3, 1>::check(dcd1);
+ MatrixSizeCheck<3, 1>::check(dLc1);
+ MatrixSizeCheck<3, 1>::check(dc2);
+ MatrixSizeCheck<3, 1>::check(dcd2);
+ MatrixSizeCheck<3, 1>::check(dLc2);
+ MatrixSizeCheck<3, 1>::check(diff_c);
+ MatrixSizeCheck<3, 1>::check(diff_cd);
+ MatrixSizeCheck<3, 1>::check(diff_Lc);
+
+ diff_c = dq1.conjugate() * (dc2 - dc1 - dcd1*dt);
+ diff_cd = dq1.conjugate() * (dcd2 - dcd1);
+ diff_Lc = dq1.conjugate() * (dLc2 - dLc1);
+ diff_q = dq1.conjugate() * dq2;
+}
+
+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<13, 1>::check(d1);
+ MatrixSizeCheck<13, 1>::check(d2);
+ MatrixSizeCheck<13, 1>::check(d2_minus_d1);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2(6) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(9) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_c ( & d2_minus_d1(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_cd ( & d2_minus_d1(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_Lc ( & d2_minus_d1(6) );
+ Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(9) );
+
+ between(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, dt, diff_c, diff_cd, diff_Lc, diff_q);
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 13, 1> diff;
+ between(d1, d2, dt, diff);
+ return diff;
+}
+
+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<13, 1>::check(x);
+ MatrixSizeCheck<13, 1>::check(d);
+ MatrixSizeCheck<13, 1>::check(x_plus_d);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > c ( & x(0) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > cd ( & x(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > Lc ( & x(6) );
+ Map<const Quaternion<typename D1::Scalar> > q ( & x(9) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dc ( & d(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dcd ( & d(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dLc ( & d(6) );
+ Map<const Quaternion<typename D2::Scalar> > dq ( & d(9) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > c_plus_d ( & x_plus_d(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > cd_plus_d ( & x_plus_d(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > Lc_plus_d ( & x_plus_d(6) );
+ Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d(9) );
+
+ c_plus_d = c + cd*dt + 0.5*gravity()*dt*dt + q*dc;
+ cd_plus_d = cd + gravity()*dt + q*dcd;
+ Lc_plus_d = Lc + q*dLc;
+ q_plus_d = q*dq; // dq here to avoid possible aliasing between x and x_plus_d --> WHAT?
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> composeOverState(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 13, 1> ret;
+ 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, typename D10, typename D11, typename D12, class T>
+inline void betweenStates(const MatrixBase<D1>& c1, const MatrixBase<D2>& cd1, const MatrixBase<D3>& Lc1, const QuaternionBase<D4>& q1,
+ const MatrixBase<D5>& c2, const MatrixBase<D6>& cd2, const MatrixBase<D7>& Lc2, const QuaternionBase<D8>& q2,
+ const T dt,
+ MatrixBase<D9>& dc, MatrixBase<D10>& dcd, MatrixBase<D11>& dLc, QuaternionBase<D12>& dq)
+{
+ MatrixSizeCheck<3, 1>::check(c1);
+ MatrixSizeCheck<3, 1>::check(cd1);
+ MatrixSizeCheck<3, 1>::check(Lc1);
+ MatrixSizeCheck<3, 1>::check(c2);
+ MatrixSizeCheck<3, 1>::check(cd2);
+ MatrixSizeCheck<3, 1>::check(Lc2);
+ MatrixSizeCheck<3, 1>::check(dc);
+ MatrixSizeCheck<3, 1>::check(dcd);
+ MatrixSizeCheck<3, 1>::check(dLc);
+
+ dc = q1.conjugate() * ( c2 - c1 - cd1*dt - 0.5*gravity() *dt * dt );
+ dcd = q1.conjugate() * ( cd2 - cd1 - gravity()*dt );
+ dLc = q1.conjugate() * ( Lc2 - Lc1);
+ 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<13, 1>::check(x1);
+ MatrixSizeCheck<13, 1>::check(x2);
+ MatrixSizeCheck<13, 1>::check(x2_minus_x1);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > c1 ( & x1(0) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > cd1 ( & x1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > Lc1 ( & x1(6) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(9) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > c2 ( & x2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > cd2 ( & x2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > Lc2 ( & x2(6) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(9) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > dc ( & x2_minus_x1(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > dcd ( & x2_minus_x1(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > dLc ( & x2_minus_x1(6) );
+ Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(9) );
+
+ betweenStates(c1, cd1, Lc1, q1, c2, cd2, Lc2, q2, dt, dc, dcd, dLc, dq);
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> betweenStates(const MatrixBase<D1>& x1,
+ const MatrixBase<D2>& x2,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 13, 1> ret;
+ betweenStates(x1, x2, dt, ret);
+ return ret;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 12, 1> log_FT(const MatrixBase<Derived>& delta_in)
+{
+ MatrixSizeCheck<13, 1>::check(delta_in);
+
+ Matrix<typename Derived::Scalar, 12, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dc_in ( & delta_in(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dcd_in ( & delta_in(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dLc_in ( & delta_in(6) );
+ Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(9) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dc_ret ( & ret(0) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dcd_ret ( & ret(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dLc_ret ( & ret(6) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(9) );
+
+ dc_ret = dc_in;
+ dcd_ret = dcd_in;
+ dLc_ret = dLc_in;
+ do_ret = log_q(dq_in);
+
+ return ret;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 13, 1> exp_FT(const MatrixBase<Derived>& d_alg_in)
+{
+ MatrixSizeCheck<12, 1>::check(d_alg_in);
+
+ Matrix<typename Derived::Scalar, 13, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dc_alg_in ( & d_alg_in(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dcd_alg_in ( & d_alg_in(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dLc_alg_in ( & d_alg_in(6) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > do_alg_in ( & d_alg_in(9) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dc_ret ( & ret(0) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dcd_ret ( & ret(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dLc_ret ( & ret(6) );
+ Map<Quaternion<typename Derived::Scalar> > dq_ret ( & ret(9) );
+
+ dc_ret = dc_alg_in;
+ dcd_ret = dcd_alg_in;
+ dLc_ret = dLc_alg_in;
+ dq_ret = exp_q(do_alg_in);
+
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12>
+inline void plus(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& diff_c2, const MatrixBase<D6>& diff_cd2, const MatrixBase<D7>& diff_Lc2, const MatrixBase<D8>& diff_o2,
+ MatrixBase<D9>& plus_c, MatrixBase<D10>& plus_cd, MatrixBase<D11>& plus_Lc, QuaternionBase<D12>& plus_q)
+{
+ plus_c = dc1 + diff_c2;
+ plus_cd = dcd1 + diff_cd2;
+ plus_Lc = dLc1 + diff_Lc2;
+ plus_q = dq1 * exp_q(diff_o2);
+}
+
+
+// Composite plus
+template<typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& diff2,
+ MatrixBase<D3>& d)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > diff_dc2 ( & diff2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > diff_dcd2 ( & diff2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > diff_dLc2 ( & diff2(6) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > diff_do2 ( & diff2(9) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > d_c ( & d(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > d_cd ( & d(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > d_Lc ( & d(6) );
+ Map<Quaternion<typename D3::Scalar> > d_q ( & d(9) );
+
+ plus(dc1, dcd1, dLc1, dq1,
+ diff_dc2, diff_dcd2, diff_dLc2, diff_do2,
+ d_c, d_cd, d_Lc, d_q);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 13, 1> plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 13, 1> ret;
+ plus(d1, d2, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12>
+inline void diff(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& dc2, const MatrixBase<D6>& dcd2, const MatrixBase<D7>& dLc2, const QuaternionBase<D8>& dq2,
+ MatrixBase<D9>& diff_c, MatrixBase<D10>& diff_cd, MatrixBase<D11>& diff_Lc, MatrixBase<D12>& diff_o)
+{
+ diff_c = dc2 - dc1;
+ diff_cd = dcd2 - dcd1;
+ diff_Lc = dLc2 - dLc1;
+ diff_o = log_q(dq1.conjugate() * dq2);
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12, typename D13, typename D14>
+inline void diff(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& dc2, const MatrixBase<D6>& dcd2, const MatrixBase<D7>& dLc2, const QuaternionBase<D8>& dq2,
+ MatrixBase<D9>& diff_c, MatrixBase<D10>& diff_cd, MatrixBase<D11>& diff_Lc, MatrixBase<D12>& diff_o,
+ MatrixBase<D13>& J_do_dq1, MatrixBase<D14>& J_do_dq2)
+{
+ diff(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, diff_c, diff_cd, diff_Lc, diff_o);
+
+ 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>& diff_d1_d2)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2(6) );
+ Map<const Quaternion<typename D1::Scalar> > dq2 ( & d2(9) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_c ( & diff_d1_d2(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_cd ( & diff_d1_d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_Lc ( & diff_d1_d2(6) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & diff_d1_d2(9) );
+
+ diff(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, diff_c, diff_cd, diff_Lc, diff_o);
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& diff_d1_d2,
+ MatrixBase<D4>& J_diff_d1,
+ MatrixBase<D5>& J_diff_d2)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2(6) );
+ Map<const Quaternion<typename D1::Scalar> > dq2 ( & d2(9) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_c ( & diff_d1_d2(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_cd ( & diff_d1_d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_Lc ( & diff_d1_d2(6) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & diff_d1_d2(9) );
+
+ Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+
+ diff(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, diff_c, diff_cd, diff_Lc, diff_o, J_do_dq1, J_do_dq2);
+
+ /* d = diff(d1, d2) is
+ * dc = dc2 - dc1
+ * dcd = dcd2 - dcd1
+ * dLc = dLc2 - dLc1
+ * do = Log(dq1.conj * dq2)
+ *
+ * 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, 12, 12>::Identity(); // d(c2 - c1) / d(c1) = - Identity, for instance
+ J_diff_d1.template block<3,3>(9,9) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+ J_diff_d2.setIdentity(); // d(c2 - c1) / d(c2) = Identity, for instance
+ J_diff_d2.template block<3,3>(9,9) = J_do_dq2; // d(R1.tr*R2) / d(R2) = J_r_inv(theta)
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 12, 1> diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 12, 1> ret;
+ diff(d1, d2, ret);
+ return ret;
+}
+
+template<typename D1, typename D2>
+inline void body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt,
+ const typename D1::Scalar& mass,
+ int nbc,
+ int dimc,
+ MatrixBase<D2>& delta)
+{
+ typedef typename D1::Scalar T;
+
+ delta.setZero();
+ Map< Matrix<T, 3, 1>> dc ( & delta(0) );
+ Map< Matrix<T, 3, 1>> dcd( & delta(3) );
+ Map< Matrix<T, 3, 1>> dLc( & delta(6) );
+ Map< Quaternion<T>> dq ( & delta(9) );
+
+ Vector3d pbc = body.segment(nbc*dimc, 3);
+ Vector3d wb = body.segment(nbc*dimc + 3, 3);
+
+ // general computation for any 3D or 6D contacts
+ for (int i=0; i<nbc; i++){
+ Vector3d pbli = body.segment(nbc*dimc + 6 + i*3, 3);
+ Vector4d qbli_v = body.segment(nbc*dimc + 6 + nbc*3 + i*4, 4);
+ Quaterniond qbli(qbli_v);
+ Vector3d fi = body.segment(i*3, 3);
+
+ dc += qbli*fi;
+ dcd += qbli*fi;
+ dLc += (pbli - pbc).cross(qbli * fi);
+ if (dimc == 6){
+ Vector3d taui = body.segment(3*nbc + i*3, 3);
+ dLc += qbli*taui;
+ }
+ }
+
+ // formulas for 2 6D contacts:
+ // dc = 0.5 * (qbl1*f1 + qbl2*f2) * dt * dt / mass;
+ // dcd = (qbl1*f1 + qbl2*f2) * dt / mass;
+ // dLc = (qbl1*tau1 + (pbl1-pbc).cross(qbl1*f1)
+ // + qbl2*tau2 + (pbl2-pbc).cross(qbl2*f2)) * dt;
+
+ dc *= (0.5*dt*dt/mass);
+ dcd *= (dt/mass);
+ dLc *= dt;
+ dq = exp_q(wb * dt);
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 13, 1> body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt,
+ const typename D1::Scalar& mass,
+ int nbc,
+ int dimc)
+{
+ typedef typename D1::Scalar T;
+ Matrix<T, 13, 1> delta;
+ body2delta(body, dt, mass, nbc, dimc, delta);
+
+ return delta;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt,
+ const typename D1::Scalar& mass,
+ int nbc,
+ int dimc,
+ MatrixBase<D2>& delta,
+ MatrixBase<D3>& jac_delta_body)
+{
+ Vector3d pbc = body.segment(nbc*dimc, 3);
+ Vector3d wb = body.segment(nbc*dimc + 3, 3);
+
+ jac_delta_body.setZero();
+ for (int i=0; i<nbc; i++){
+ Vector3d pbli = body.segment(nbc*dimc + 6 + i*3, 3);
+ Vector4d qbli_v = body.segment(nbc*dimc + 6 + nbc*3 + i*4, 4);
+ Quaterniond qbli(qbli_v);
+ Matrix3d bRli = q2R(qbli);
+ Vector3d fi = body.segment(i*3, 3);
+
+ jac_delta_body.template block<3,3>(0,i*3) = 0.5 * bRli * dt * dt / mass;
+ jac_delta_body.template block<3,3>(3,i*3) = bRli * dt / mass;
+ jac_delta_body.template block<3,3>(6,i*3) = skew(pbli - pbc) * bRli * dt;
+ if (dimc == 6){
+ jac_delta_body.template block<3,3>(6,nbc*3 + i*3) = bRli * dt;
+ }
+ jac_delta_body.template block<3,3>(6,nbc*dimc) += skew(bRli*fi) * dt;
+ }
+ jac_delta_body.template block<3,3>(9,nbc*dimc+3) = dt * jac_SO3_right(wb * dt);
+
+ // formulas for 2 6D contacts:
+ // jac_delta_body.template block<3,3>(0,0) = 0.5 * bRl1 * dt * dt / mass;
+ // jac_delta_body.template block<3,3>(0,3) = 0.5 * bRl2 * dt * dt / mass;
+ // jac_delta_body.template block<3,3>(3,0) = bRl1 * dt / mass;
+ // jac_delta_body.template block<3,3>(3,3) = bRl2 * dt / mass;
+ // jac_delta_body.template block<3,3>(6,0) = skew(pbl1 - pbc) * bRl1 * dt;
+ // jac_delta_body.template block<3,3>(6,3) = skew(pbl2 - pbc) * bRl2 * dt;
+ // jac_delta_body.template block<3,3>(6,6) = bRl1 * dt;
+ // jac_delta_body.template block<3,3>(6,9) = bRl2 * dt;
+ // jac_delta_body.template block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2)) * dt;
+ // jac_delta_body.template block<3,3>(9,15) = dt * jac_SO3_right(wb * dt);
+
+ // TODO: kinematics uncertainties (bRl1, bRl2, bpl1, bpl2) are not implemented currently
+
+ body2delta(body, dt, mass, nbc, dimc, delta);
+}
+
+template<typename D1, typename D2, typename D3>
+inline void debiasData(const MatrixBase<D1>& _data, const MatrixBase<D2>& _bias, int nbc, int dimc, MatrixBase<D3>& _body)
+{
+ MatrixSizeCheck<6 , 1>::check(_bias);
+ _body = _data;
+ _body.template block<6,1>(nbc*dimc,0) = _data.template block<6,1>(nbc*dimc,0) - _bias;
+}
+
+template<typename D1, typename D2, typename D3, typename D4>
+inline void debiasData(const MatrixBase<D1>& _data, const MatrixBase<D2>& _bias, int nbc, int dimc, MatrixBase<D3>& _body, MatrixBase<D4>& _J_body_bias)
+{
+ debiasData(_data, _bias, nbc, dimc, _body);
+ // MatrixSizeCheck<30, 6>::check(_J_body_bias); // 30 because the 2 last body quantities are quaternions
+ _J_body_bias.setZero();
+ _J_body_bias.template block<3,3>(nbc*dimc, 0) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
+ _J_body_bias.template block<3,3>(nbc*dimc+3, 3) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
+}
+
+
+
+} // namespace bodydynamics
} // namespace wolf
#endif /* FORCE_TORQUE_DELTA_TOOLS_H_ */
diff --git a/include/bodydynamics/processor/.blank b/include/bodydynamics/processor/.blank
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
new file mode 100644
index 0000000000000000000000000000000000000000..4de0c9357ff363e87790e3429aa9e85b171a8777
--- /dev/null
+++ b/include/bodydynamics/processor/processor_force_torque_preint.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 PROCESSOR_FORCE_TORQUE_PREINT_H
+#define PROCESSOR_FORCE_TORQUE_PREINT_H
+
+// Wolf core
+#include <core/processor/processor_motion.h>
+
+namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorquePreint);
+
+struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
+{
+ std::string sensor_ikin_name;
+ std::string sensor_angvel_name;
+ int nbc; // Number of contacts
+ int dimc; // Dimension of the contact: 3D == point feet = 3D force, 6D = humanoid = wrench
+
+ ParamsProcessorForceTorquePreint() = default;
+ ParamsProcessorForceTorquePreint(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorMotion(_unique_name, _server)
+ {
+ sensor_ikin_name = _server.getParam<std::string>(_unique_name + "/sensor_ikin_name");
+ sensor_angvel_name = _server.getParam<std::string>(_unique_name + "/sensor_angvel_name");
+ nbc = _server.getParam<int>(_unique_name + "/nbc_");
+ dimc = _server.getParam<int>(_unique_name + "/dimc_");
+ }
+ ~ParamsProcessorForceTorquePreint() override = default;
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print() + "\n"
+ + "sensor_ikin_name: " + sensor_ikin_name + "\n"
+ + "sensor_angvel_name: " + sensor_angvel_name + "\n"
+ + "nbc_: " + std::to_string(nbc) + "\n"
+ + "dimc_: " + std::to_string(dimc) + "\n";
+
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorForceTorquePreint);
+
+//class
+class ProcessorForceTorquePreint : public ProcessorMotion{
+ public:
+ ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint);
+ ~ProcessorForceTorquePreint() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorquePreint, ParamsProcessorForceTorquePreint);
+
+ 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 = nullptr) 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;
+
+ private:
+ ParamsProcessorForceTorquePreintPtr params_motion_force_torque_preint_;
+ SensorBasePtr sensor_ikin_;
+ SensorBasePtr sensor_angvel_;
+ int nbc_;
+ int dimc_;
+};
+
+}
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+namespace wolf{
+
+inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor)
+{
+ sensor_ikin_ = _sensor->getProblem()->getSensor(params_motion_force_torque_preint_->sensor_ikin_name);
+ sensor_angvel_ = _sensor->getProblem()->getSensor(params_motion_force_torque_preint_->sensor_angvel_name);
+};
+
+inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const
+{
+ return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // com, com vel, ang momentum, orientation
+}
+
+} // namespace wolf
+
+#endif // PROCESSOR_FORCE_TORQUE_PREINT_H
diff --git a/include/bodydynamics/processor/processor_inertial_kinematics.h b/include/bodydynamics/processor/processor_inertial_kinematics.h
new file mode 100644
index 0000000000000000000000000000000000000000..468fd9fa27c9b1983b4723f6938c0651632d8374
--- /dev/null
+++ b/include/bodydynamics/processor/processor_inertial_kinematics.h
@@ -0,0 +1,147 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_INERTIAL_KINEMATICS_H
+#define PROCESSOR_INERTIAL_KINEMATICS_H
+
+// Wolf
+#include "core/sensor/sensor_base.h"
+#include "core/processor/processor_base.h"
+
+#include "core/processor/processor_base.h"
+#include "core/factor/factor_base.h"
+#include "core/factor/factor_block_difference.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/feature/feature_inertial_kinematics.h"
+#include "bodydynamics/factor/factor_inertial_kinematics.h"
+
+namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorInertialKinematics);
+
+struct ParamsProcessorInertialKinematics : public ParamsProcessorBase
+{
+ std::string sensor_angvel_name;
+ double std_bp_drift;
+
+ ParamsProcessorInertialKinematics() = default;
+ ParamsProcessorInertialKinematics(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ sensor_angvel_name = _server.getParam<std::string>(_unique_name + "/sensor_angvel_name");
+ std_bp_drift = _server.getParam<double>(_unique_name + "/std_bp_drift");
+ }
+ ~ParamsProcessorInertialKinematics() override = default;
+ std::string print() const override
+ {
+ return ParamsProcessorBase::print() + "\n"
+ + "sensor_angvel_name: " + sensor_angvel_name + "\n"
+ + "std_bp_drift: " + std::to_string(std_bp_drift) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorInertialKinematics);
+
+//class
+class ProcessorInertialKinematics : public ProcessorBase{
+ public:
+ ProcessorInertialKinematics(ParamsProcessorInertialKinematicsPtr _params_ikin);
+ ~ProcessorInertialKinematics() override = default;
+ WOLF_PROCESSOR_CREATE(ProcessorInertialKinematics, ParamsProcessorInertialKinematics);
+
+ void configure(SensorBasePtr _sensor) override;
+
+ bool createInertialKinematicsFactor(CaptureInertialKinematicsPtr _cap_ikin, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_ikin_origin);
+
+ void processCapture(CaptureBasePtr _incoming) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
+ bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
+ bool storeKeyFrame(FrameBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override;
+ bool voteForKeyFrame() const override;
+
+
+ protected:
+ ParamsProcessorInertialKinematicsPtr params_ikin_;
+ CaptureBasePtr cap_origin_ptr_;
+};
+
+
+
+inline bool ProcessorInertialKinematics::triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const
+{
+ return false;
+}
+
+inline bool ProcessorInertialKinematics::triggerInCapture(CaptureBasePtr _capture) const
+{
+ return true;
+}
+
+inline bool ProcessorInertialKinematics::storeKeyFrame(FrameBasePtr)
+{
+ return true;
+}
+
+inline bool ProcessorInertialKinematics::storeCapture(CaptureBasePtr)
+{
+ return true;
+}
+
+inline bool ProcessorInertialKinematics::voteForKeyFrame() const
+{
+ return false;
+}
+
+inline void ProcessorInertialKinematics::processKeyFrame(FrameBasePtr _keyframe_ptr)
+{
+}
+
+
+//////////////////////////
+// Covariance computations
+//////////////////////////
+
+
+Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& w_unb,
+ const Eigen::Vector3d& p_unb,
+ const Eigen::Matrix3d& Iq);
+
+Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
+ const Eigen::Matrix3d& Qv,
+ const Eigen::Matrix3d& Qw,
+ const Eigen::Vector3d& p_unb,
+ const Eigen::Vector3d& w_unb,
+ const Eigen::Matrix3d& Iq);
+
+} /* namespace wolf */
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+
+namespace wolf{
+
+} // namespace wolf
+
+#endif // PROCESSOR_INERTIAL_KINEMATICS_H
diff --git a/include/bodydynamics/processor/processor_point_feet_nomove.h b/include/bodydynamics/processor/processor_point_feet_nomove.h
new file mode 100644
index 0000000000000000000000000000000000000000..49c40be86b331397416bc8bf806aac4a4b6f9419
--- /dev/null
+++ b/include/bodydynamics/processor/processor_point_feet_nomove.h
@@ -0,0 +1,116 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_POINT_FEET_NOMOVE_H
+#define PROCESSOR_POINT_FEET_NOMOVE_H
+
+// Wolf
+#include "core/sensor/sensor_base.h"
+#include "core/processor/processor_base.h"
+
+#include "bodydynamics/capture/capture_point_feet_nomove.h"
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
+#include "bodydynamics/factor/factor_point_feet_nomove.h"
+
+namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPointFeetNomove);
+
+struct ParamsProcessorPointFeetNomove : public ParamsProcessorBase
+{
+ ParamsProcessorPointFeetNomove() = default;
+ ParamsProcessorPointFeetNomove(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ }
+ ~ParamsProcessorPointFeetNomove() override = default;
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print() + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorPointFeetNomove);
+
+//class
+class ProcessorPointFeetNomove : public ProcessorBase{
+ public:
+ ProcessorPointFeetNomove(ParamsProcessorPointFeetNomovePtr _params_pfnomove);
+ ~ProcessorPointFeetNomove() override = default;
+ WOLF_PROCESSOR_CREATE(ProcessorPointFeetNomove, ParamsProcessorPointFeetNomove);
+
+ void configure(SensorBasePtr _sensor) override;
+
+ bool createPointFeetNomoveFactor(CapturePointFeetNomovePtr _cap_pfnomove, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_pfnomove_origin);
+
+ void processCapture(CaptureBasePtr _incoming) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
+ bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
+ bool storeKeyFrame(FrameBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override;
+ bool voteForKeyFrame() const override;
+
+ void createFactorIfNecessary();
+
+
+ protected:
+ ParamsProcessorPointFeetNomovePtr params_pfnomove_;
+};
+
+
+
+inline bool ProcessorPointFeetNomove::triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const
+{
+ return true;
+}
+
+inline bool ProcessorPointFeetNomove::triggerInCapture(CaptureBasePtr _capture) const
+{
+ return true;
+}
+
+inline bool ProcessorPointFeetNomove::storeKeyFrame(FrameBasePtr)
+{
+ return true;
+}
+
+inline bool ProcessorPointFeetNomove::storeCapture(CaptureBasePtr)
+{
+ return true;
+}
+
+inline bool ProcessorPointFeetNomove::voteForKeyFrame() const
+{
+ return false;
+}
+
+} /* namespace wolf */
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+
+namespace wolf{
+
+} // namespace wolf
+
+#endif // PROCESSOR_POINT_FEET_NOMOVE_H
diff --git a/include/bodydynamics/sensor/.blank b/include/bodydynamics/sensor/.blank
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/include/bodydynamics/sensor/sensor_force_torque.h b/include/bodydynamics/sensor/sensor_force_torque.h
new file mode 100644
index 0000000000000000000000000000000000000000..eea790dd1b52e104b730e9a7bd20a4d0f52a31af
--- /dev/null
+++ b/include/bodydynamics/sensor/sensor_force_torque.h
@@ -0,0 +1,100 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_FORCE_TORQUE_H
+#define SENSOR_FORCE_TORQUE_H
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+#include <iostream>
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorForceTorque);
+
+
+struct ParamsSensorForceTorque : public ParamsSensorBase
+{
+ //noise std dev
+ double mass = 10; // total mass of the robot (kg)
+ double std_f = 0.001; // standard deviation of the force sensors (N)
+ double std_tau = 0.001; // standard deviation of the torque sensors (N.m)
+
+ ParamsSensorForceTorque() = default;
+ ParamsSensorForceTorque(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ mass = _server.getParam<double>(_unique_name + "/mass");
+ std_f = _server.getParam<double>(_unique_name + "/std_f");
+ std_tau = _server.getParam<double>(_unique_name + "/std_tau");
+ }
+ ~ParamsSensorForceTorque() override = default;
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n"
+ + "mass: " + std::to_string(mass) + "\n"
+ + "std_f: " + std::to_string(std_f) + "\n"
+ + "std_tau: " + std::to_string(std_tau) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(SensorForceTorque);
+
+class SensorForceTorque : public SensorBase
+{
+
+ protected:
+ //noise std dev
+ double mass_; // total mass of the robot (kg)
+ double std_f_; // standard deviation of the force sensors (N)
+ double std_tau_; // standard deviation of the torque sensors (N.m)
+
+ public:
+
+ SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params);
+ ~SensorForceTorque() override = default;
+
+ WOLF_SENSOR_CREATE(SensorForceTorque, ParamsSensorForceTorque, 0);
+
+ double getMass() const;
+ double getForceNoiseStd() const;
+ double getTorqueNoiseStd() const;
+};
+
+inline double SensorForceTorque::getMass() const
+{
+ return mass_;
+}
+
+inline double SensorForceTorque::getForceNoiseStd() const
+{
+ return std_f_;
+}
+
+inline double SensorForceTorque::getTorqueNoiseStd() const
+{
+ return std_tau_;
+}
+
+} // namespace wolf
+
+#endif // SENSOR_FORCE_TORQUE_H
diff --git a/include/bodydynamics/sensor/sensor_inertial_kinematics.h b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
new file mode 100644
index 0000000000000000000000000000000000000000..76e74adc98c6934efae76c27c4c0efea6527db3e
--- /dev/null
+++ b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
@@ -0,0 +1,88 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_INERTIAL_KINEMATICS_H
+#define SENSOR_INERTIAL_KINEMATICS_H
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+#include <iostream>
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorInertialKinematics);
+
+
+struct ParamsSensorInertialKinematics : public ParamsSensorBase
+{
+ //noise std dev
+ double std_pbc; // standard deviation of the com position measurement relative to the base frame (m)
+ double std_vbc; // standard deviation of the com velocity measurement relative to the base frame (m/s)
+
+ ParamsSensorInertialKinematics() = default;
+ ParamsSensorInertialKinematics(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ std_pbc = _server.getParam<double>(_unique_name + "/std_pbc");
+ std_vbc = _server.getParam<double>(_unique_name + "/std_vbc");
+ }
+ ~ParamsSensorInertialKinematics() override = default;
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n"
+ + "std_pbc: " + std::to_string(std_pbc) + "\n"
+ + "std_vbc: " + std::to_string(std_vbc) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(SensorInertialKinematics);
+
+class SensorInertialKinematics : public SensorBase
+{
+
+ protected:
+ ParamsSensorInertialKinematicsPtr intr_ikin_;
+
+ public:
+
+ SensorInertialKinematics(const Eigen::VectorXd& _extrinsics, ParamsSensorInertialKinematicsPtr _intr_ikin);
+ ~SensorInertialKinematics() override;
+
+ WOLF_SENSOR_CREATE(SensorInertialKinematics, ParamsSensorInertialKinematics, 0);
+
+ double getPbcNoiseStd() const;
+ double getVbcNoiseStd() const;
+};
+
+inline double SensorInertialKinematics::getPbcNoiseStd() const
+{
+ return intr_ikin_->std_pbc;
+}
+
+inline double SensorInertialKinematics::getVbcNoiseStd() const
+{
+ return intr_ikin_->std_vbc;
+}
+
+} // namespace wolf
+
+#endif // SENSOR_INERTIAL_KINEMATICS_H
diff --git a/include/bodydynamics/sensor/sensor_point_feet_nomove.h b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f58ad65e96d1ca4c249cdd23a4f8e88b03f8060
--- /dev/null
+++ b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
@@ -0,0 +1,80 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_POINT_FEET_NOMOVE_H
+#define SENSOR_POINT_FEET_NOMOVE_H
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+#include <iostream>
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPointFeetNomove);
+
+
+struct ParamsSensorPointFeetNomove : public ParamsSensorBase
+{
+ //noise std dev
+ double std_foot_nomove_; // standard deviation on the assumption that the feet are not moving when in contact
+
+ ParamsSensorPointFeetNomove() = default;
+ ParamsSensorPointFeetNomove(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ std_foot_nomove_ = _server.getParam<double>(_unique_name + "/std_foot_nomove");
+ }
+ ~ParamsSensorPointFeetNomove() override = default;
+ std::string print() const override
+ {
+ return "\n" + ParamsSensorBase::print() + "\n"
+ + "std_foot_nomove: " + std::to_string(std_foot_nomove_) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(SensorPointFeetNomove);
+
+class SensorPointFeetNomove : public SensorBase
+{
+
+ protected:
+ Matrix3d cov_foot_nomove_;
+
+ public:
+
+ SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics, const ParamsSensorPointFeetNomovePtr& _intr_pfnm);
+ ~SensorPointFeetNomove() override;
+
+ WOLF_SENSOR_CREATE(SensorPointFeetNomove, ParamsSensorPointFeetNomove, 0);
+
+ Matrix3d getCovFootNomove() const;
+};
+
+inline Matrix3d SensorPointFeetNomove::getCovFootNomove() const
+{
+ return cov_foot_nomove_;
+}
+
+
+} // namespace wolf
+
+#endif // SENSOR_POINT_FEET_NOMOVE_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/.blank b/src/capture/.blank
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/capture/capture_force_torque_preint.cpp b/src/capture/capture_force_torque_preint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..83e145e4b85a4895a81dce2251fcddd10681460d
--- /dev/null
+++ b/src/capture/capture_force_torque_preint.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 "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "core/state_block/state_quaternion.h"
+
+namespace wolf {
+
+CaptureForceTorquePreint::CaptureForceTorquePreint(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
+ CaptureMotionPtr _capture_motion_ptr, // to get the gyro bias
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov, // TODO: no uncertainty from kinematics
+ CaptureBasePtr _capture_origin_ptr) :
+ CaptureMotion("CaptureForceTorquePreint", _init_ts, _sensor_ptr, _data, _data_cov,
+ _capture_origin_ptr, nullptr, nullptr, nullptr),
+ cap_ikin_other_(_capture_IK_ptr),
+ cap_gyro_other_(_capture_motion_ptr)
+{
+}
+
+CaptureForceTorquePreint::~CaptureForceTorquePreint()
+{
+
+}
+
+} //namespace wolf
diff --git a/src/capture/capture_inertial_kinematics.cpp b/src/capture/capture_inertial_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3640f6b966c49cde3ed4fb8f3db94856fbf14bc7
--- /dev/null
+++ b/src/capture/capture_inertial_kinematics.cpp
@@ -0,0 +1,72 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/capture/capture_base.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "core/state_block/state_quaternion.h"
+
+namespace wolf {
+
+CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d & _B_Lc_m, // Centroidal angular momentum expressed in body frame
+ const Vector3d& _bias_pbc) :
+ CaptureBase("CaptureInertialKinematics",
+ _init_ts,
+ _sensor_ptr,
+ nullptr,
+ nullptr,
+ std::make_shared<StateBlock>(_bias_pbc, false)),
+ data_(_data),
+ B_I_q_(_B_I_q),
+ B_Lc_m_(_B_Lc_m)
+{
+ //
+}
+
+
+CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d & _B_Lc_m) : // Centroidal angular momentum expressed in body frame
+ CaptureBase("CaptureInertialKinematics",
+ _init_ts,
+ _sensor_ptr,
+ nullptr,
+ nullptr,
+ std::make_shared<StateBlock>(3, false)),
+ data_(_data),
+ B_I_q_(_B_I_q),
+ B_Lc_m_(_B_Lc_m)
+{
+ //
+}
+
+CaptureInertialKinematics::~CaptureInertialKinematics()
+{
+ //
+}
+
+} //namespace wolf
diff --git a/src/capture/capture_leg_odom.cpp b/src/capture/capture_leg_odom.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..69336fa53b1e686d3434bf3a0e821f0315f844c1
--- /dev/null
+++ b/src/capture/capture_leg_odom.cpp
@@ -0,0 +1,135 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file capture_leg_odom.cpp
+ *
+ * Created on: May 12, 2020
+ * \author: mfourmy
+ */
+
+
+#include "core/math/rotations.h"
+#include "core/capture/capture_base.h"
+#include "bodydynamics/capture/capture_leg_odom.h"
+
+
+namespace wolf {
+
+// bm: base at time t-dt
+// b: base at current time
+CaptureLegOdom::CaptureLegOdom(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::MatrixXd& _data_kin, // cols: b_p_bl1m, b_p_bl1, b_p_bl2m, b_p_bl2..., rows: xyz
+ Eigen::Matrix6d _data_cov, // for the moment only 6x6 mat, see if more involved computations are necessary (contact confidence...)
+ double dt,
+ CaptureLegOdomType lo_type) :
+ CaptureMotion("CaptureLegOdom", _init_ts, _sensor_ptr,
+ Eigen::Vector7d::Zero(), Eigen::Matrix6d::Identity(), // dummy data and data covariance filled in the constructor
+ nullptr, nullptr, nullptr, nullptr)
+{
+ Vector7d data;
+ if (_data_kin.cols() < 2){
+ // no feet in contact, flying robot -> dummy data with high cov
+ data << 0,0,0, 0,0,0,1;
+ _data_cov = pow(1000, 2) * _data_cov;
+ return;
+ }
+
+ if (lo_type == CaptureLegOdomType::POINT_FEET_RELATIVE_KIN){
+ assert(_data_kin.rows() == 3);
+
+ // relative base-feet position + angular velocity
+ Eigen::Vector3d i_omg_oi = _data_kin.rightCols<1>();
+ Eigen::Quaterniond bm_q_b (v2q(i_omg_oi*dt));
+ std::vector<Vector3d> bm_p_bmb_vec;
+ for (unsigned int i=0; i < (_data_kin.cols()-1); i+=2){
+ Eigen::Vector3d bm_p_bml = _data_kin.col(i);
+ Eigen::Vector3d b_p_bl = _data_kin.col(i+1);
+ bm_p_bmb_vec.push_back(bm_p_bml - bm_q_b * b_p_bl);
+ }
+ // for the moment simple mean, to be improved
+ Vector3d bm_p_bmb = Vector3d::Zero();
+ for (unsigned int i=0; i < bm_p_bmb_vec.size(); i++){
+ bm_p_bmb += bm_p_bmb_vec[i];
+ }
+
+ bm_p_bmb /= bm_p_bmb_vec.size();
+
+ data.head<3>() = bm_p_bmb;
+ data.tail<4>() = bm_q_b.coeffs();
+
+ }
+ else if(lo_type == CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN){
+ assert(_data_kin.rows() == 10);
+ // TODO: document data matrix organization
+
+ std::vector<Vector3d> b_v_ob_vec;
+ Vector3d i_omg_oi = _data_kin.block<3,1>(0, _data_kin.cols()-1); // retrieve angular vel first
+ Eigen::Quaterniond bm_q_b (v2q(i_omg_oi*dt));
+ for (unsigned int i=0; i < _data_kin.cols()-1; i++){
+ Vector3d b_p_bl = _data_kin.block<3,1>(0,i);
+ Quaterniond b_q_l(_data_kin.block<4,1>(3,i));
+ Vector3d l_v_bl = _data_kin.block<3,1>(7,i);
+ Vector3d b_v_ob = b_p_bl.cross(i_omg_oi) - b_q_l * l_v_bl;
+ b_v_ob_vec.push_back(b_v_ob);
+ }
+
+ // Simple mean for the moment
+ Vector3d b_v_ob_mean = Vector3d::Zero();
+ for (unsigned int i=0; i < b_v_ob_vec.size(); i++){
+ b_v_ob_mean += b_v_ob_vec[i];
+ }
+ b_v_ob_mean /= b_v_ob_vec.size();
+
+ data.head<3>() = b_v_ob_mean*dt;
+ data.tail<4>() = bm_q_b.coeffs();
+ }
+ else if(lo_type == CaptureLegOdomType::HUMANOID_FEET_RELATIVE_KIN){
+ assert(_data_kin.rows() == 7);
+ // relative transformations
+ // most simple option: odometry of one of the feet
+ Eigen::Vector7d bm_pose_l = _data_kin.col(0);
+ Eigen::Vector7d b_pose_l = _data_kin.col(1);
+ Eigen::Quaterniond bm_q_l(bm_pose_l.tail<4>());
+ Eigen::Quaterniond b_q_l(b_pose_l.tail<4>());
+ Eigen::Vector3d bm_p_bml(bm_pose_l.head<3>());
+ Eigen::Vector3d b_p_bl(b_pose_l.head<3>());
+
+ Eigen::Quaterniond bm_q_b(bm_q_l * b_q_l.inverse());
+ Eigen::Vector3d bm_p_bmb = bm_p_bml - bm_q_b * b_p_bl;
+
+ data.head<3>() = bm_p_bmb;
+ data.tail<4>() = bm_q_b.coeffs();
+ }
+ else {
+ std::cout << "Unkown CaptureLegOdomType! ERROR (TODO)" << std::endl;
+ }
+ setData(data);
+ setDataCovariance(_data_cov);
+}
+
+CaptureLegOdom::~CaptureLegOdom()
+{
+ //
+}
+
+} //namespace wolf
diff --git a/src/capture/capture_point_feet_nomove.cpp b/src/capture/capture_point_feet_nomove.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d4dca45ced860903bfdfabc3f4a86e088b63859
--- /dev/null
+++ b/src/capture/capture_point_feet_nomove.cpp
@@ -0,0 +1,54 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file capture_point_feet_nomove.cpp
+ *
+ * Created on: Oct 23, 2020
+ * \author: mfourmy
+ */
+
+
+#include "core/capture/capture_base.h"
+#include "bodydynamics/capture/capture_point_feet_nomove.h"
+
+
+namespace wolf {
+
+// bm: base at time t-dt
+// b: base at current time
+CapturePointFeetNomove::CapturePointFeetNomove(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const std::unordered_map<int, Eigen::Vector3d>& kin_incontact
+ ) :
+ CaptureBase("CapturePointFeetNomove",
+ _init_ts,
+ _sensor_ptr),
+ kin_incontact_(kin_incontact)
+{}
+
+CapturePointFeetNomove::~CapturePointFeetNomove()
+{
+ //
+}
+
+} //namespace wolf
diff --git a/src/factor/.blank b/src/factor/.blank
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp
index 343e7dc0e97dd567610397c8001e3783da1cb7e1..83c12548fd923e7621b56465ecedbcc12b673500 100644
--- a/src/feature/feature_force_torque.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "bodydynamics/feature/feature_force_torque.h"
namespace wolf {
@@ -5,29 +26,29 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureForceTorque);
FeatureForceTorque::FeatureForceTorque(
- const double& _dt,
- const double& _mass,
- const Eigen::Vector6d& _forces_meas,
- const Eigen::Vector6d& _torques_meas,
- const Eigen::Vector3d& _pbc_meas,
- const Eigen::Matrix<double,14,1>& _kin_meas,
- const Eigen::Matrix6d& _cov_forces_meas,
- const Eigen::Matrix6d& _cov_torques_meas,
- const Eigen::Matrix3d& _cov_pbc_meas,
- const Eigen::Matrix<double,14,14>& _cov_kin_meas) :
-FeatureBase("FORCETORQUE", 42*Eigen::Matrix1d::Identity(), 42*Eigen::Matrix1d::Identity(), UNCERTAINTY_IS_COVARIANCE), // Pass dummmy values -> meas and cov not stored where it is usually because too big == error prone
-dt_(_dt),
-mass_(_mass),
-forces_meas_(_forces_meas),
-torques_meas_(_torques_meas),
-pbc_meas_(_pbc_meas),
-kin_meas_(_kin_meas),
-cov_forces_meas_(_cov_forces_meas),
-cov_torques_meas_(_cov_torques_meas),
-cov_pbc_meas_(_cov_pbc_meas),
-cov_kin_meas_(_cov_kin_meas)
+ const double& _dt,
+ const double& _mass,
+ const Eigen::Vector6d& _forces_meas,
+ const Eigen::Vector6d& _torques_meas,
+ const Eigen::Vector3d& _pbc_meas,
+ const Eigen::Matrix<double,14,1>& _kin_meas,
+ const Eigen::Matrix6d& _cov_forces_meas,
+ const Eigen::Matrix6d& _cov_torques_meas,
+ const Eigen::Matrix3d& _cov_pbc_meas,
+ const Eigen::Matrix<double,14,14>& _cov_kin_meas) :
+ FeatureBase("FORCETORQUE", 42*Eigen::Matrix1d::Identity(), 42*Eigen::Matrix1d::Identity(), UNCERTAINTY_IS_COVARIANCE), // Pass dummmy values -> we are bypassing the way meas and cov is usually stored because too many of them == vector is too big -> error prone
+ dt_(_dt),
+ mass_(_mass),
+ forces_meas_(_forces_meas),
+ torques_meas_(_torques_meas),
+ pbc_meas_(_pbc_meas),
+ kin_meas_(_kin_meas),
+ cov_forces_meas_(_cov_forces_meas),
+ cov_torques_meas_(_cov_torques_meas),
+ cov_pbc_meas_(_cov_pbc_meas),
+ cov_kin_meas_(_cov_kin_meas)
{}
FeatureForceTorque::~FeatureForceTorque(){}
-} // namespace wolf
\ No newline at end of file
+} // namespace wolf
diff --git a/src/feature/feature_force_torque_preint.cpp b/src/feature/feature_force_torque_preint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1a21081d827843645a62c75508b3ceda10794c71
--- /dev/null
+++ b/src/feature/feature_force_torque_preint.cpp
@@ -0,0 +1,36 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "bodydynamics/feature/feature_force_torque_preint.h"
+namespace wolf {
+
+FeatureForceTorquePreint::FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _biases_preint,
+ const Eigen::Matrix<double,12,6>& _J_delta_biases) :
+ FeatureBase("FeatureForceTorquePreint", _delta_preintegrated, _delta_preintegrated_covariance),
+ pbc_bias_preint_(_biases_preint.head<3>()),
+ gyro_bias_preint_(_biases_preint.tail<3>()),
+ J_delta_bias_(_J_delta_biases)
+{
+}
+
+} // namespace wolf
diff --git a/src/feature/feature_inertial_kinematics.cpp b/src/feature/feature_inertial_kinematics.cpp
index 68c11faab9a60c64c7b3554ddd7a1e623b57f829..8f80b0ddc96b0029211ed5b66940c1a5af6db117 100644
--- a/src/feature/feature_inertial_kinematics.cpp
+++ b/src/feature/feature_inertial_kinematics.cpp
@@ -1,23 +1,43 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "bodydynamics/feature/feature_inertial_kinematics.h"
-
+#include "Eigen/Dense"
+#include "core/math/rotations.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureInertialKinematics);
FeatureInertialKinematics::FeatureInertialKinematics(
- const Eigen::Vector9d & _measurement,
- const Eigen::Matrix9d & _meas_uncertainty,
+ const Eigen::Vector9d & _meas_pbc_vbc_w,
+ const Eigen::Matrix9d & _Qerr,
const Eigen::Matrix3d & _BIq,
const Eigen::Vector3d & _BLcm,
UncertaintyType _uncertainty_type) :
- FeatureBase("INERTIALKINEMATICS", _measurement, _meas_uncertainty, _uncertainty_type),
+ FeatureBase("InertialKinematics", _meas_pbc_vbc_w, _Qerr, _uncertainty_type),
BIq_(_BIq),
BLcm_(_BLcm)
{}
-FeatureInertialKinematics::~FeatureInertialKinematics(){}
-
-} // namespace wolf
\ No newline at end of file
+} // namespace wolf
diff --git a/src/processor/.blank b/src/processor/.blank
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..994b91a49c911dc5835b1d642ead04d2f5ca0223
--- /dev/null
+++ b/src/processor/processor_force_torque_preint.cpp
@@ -0,0 +1,272 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "bodydynamics/math/force_torque_delta_tools.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/feature/feature_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_force_torque_preint.h"
+
+namespace wolf {
+
+int compute_data_size(int nbc, int dimc){
+ // compute the size of the data/body vectors used by processorMotion API depending
+ // on the number of contacts (nbc) and contact dimension (dimc)
+ // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
+ return nbc*dimc + 3 + 3 + nbc*3 + nbc*4;
+}
+
+using namespace Eigen;
+
+ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint) :
+ ProcessorMotion("ProcessorForceTorquePreint", "CDLO", 3, 13, 13, 12,
+ compute_data_size(_params_motion_force_torque_preint->nbc, _params_motion_force_torque_preint->dimc),
+ 6, _params_motion_force_torque_preint),
+ params_motion_force_torque_preint_(std::make_shared<ParamsProcessorForceTorquePreint>(*_params_motion_force_torque_preint)),
+ nbc_(_params_motion_force_torque_preint->nbc),
+ dimc_(_params_motion_force_torque_preint->dimc)
+
+{
+ //
+}
+
+ProcessorForceTorquePreint::~ProcessorForceTorquePreint()
+{
+ //
+}
+
+bool ProcessorForceTorquePreint::voteForKeyFrame() const
+{
+ // time span
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_preint_->max_time_span)
+ {
+ WOLF_DEBUG( "PM: vote: time span" );
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_motion_force_torque_preint_->max_buff_length)
+ {
+ WOLF_DEBUG( "PM: vote: buffer length" );
+ return true;
+ }
+
+ // Some other measure of movement?
+
+ //WOLF_DEBUG( "PM: do not vote" );
+ return false;
+}
+
+
+CaptureMotionPtr ProcessorForceTorquePreint::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)
+{
+
+ // Here we have to retrieve the origin capture no
+ // !! PROBLEM:
+ // when doing setOrigin, emplaceCapture is called 2 times
+ // - first on the first KF (usually created by setPrior), this one contains the biases
+ // - secondly on the inner frame (last) which does not contains other captures
+ auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
+ auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_);
+ if ((capture_ikin == nullptr) || (capture_angvel == nullptr)){
+ // We have to retrieve the bias from a former Keyframe: origin
+ capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
+ capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
+ }
+ auto cap = CaptureBase::emplace<CaptureForceTorquePreint>(
+ _frame_own,
+ _ts,
+ _sensor,
+ std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
+ std::static_pointer_cast<CaptureMotion>(capture_angvel),
+ _data,
+ _data_cov,
+ _capture_origin);
+
+ // Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_
+ // which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
+
+ auto cap_ftpreint = std::static_pointer_cast<CaptureForceTorquePreint>(cap);
+ Vector6d calib = getCalibration(cap_ftpreint);
+ setCalibration(cap_ftpreint, calib);
+ cap_ftpreint->setCalibrationPreint(calib);
+
+ return cap;
+}
+
+FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capture_motion)
+{
+ // Retrieving the origin capture and getting the bias from here should be done here?
+ auto feature = FeatureBase::emplace<FeatureForceTorquePreint>(_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;
+}
+
+Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ return bodydynamics::plus(delta_preint, delta_step);
+}
+
+VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBasePtr _capture) const
+{
+
+ VectorXd bias_vec(6);
+
+ if (_capture) // access from capture is quicker
+ {
+ CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
+
+ // get calib part from Ikin capture
+ bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
+
+ // get calib part from IMU capture
+ bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ }
+ else // access from sensor is slower
+ {
+ // get calib part from Ikin capture
+ bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
+
+ // get calib part from IMU capture
+ bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ }
+ return bias_vec;
+}
+
+void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
+
+ // set calib part in Ikin capture
+ cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>());
+
+ // set calib part in IMU capture
+ Vector6d calib_imu = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState();
+ calib_imu.tail<3>() = _calibration.tail<3>();
+
+ cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
+}
+
+FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+{
+ CaptureForceTorquePreintPtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorquePreint>(_capture_origin);
+ FeatureForceTorquePreintPtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorquePreint>(_feature_motion);
+ CaptureForceTorquePreintPtr cap_ftpreint_new = std::static_pointer_cast<CaptureForceTorquePreint>(ftr_ftpreint->getCapture());
+
+ auto fac_ftpreint = FactorBase::emplace<FactorForceTorquePreint>(
+ ftr_ftpreint,
+ ftr_ftpreint,
+ cap_ftpreint_origin,
+ shared_from_this(),
+ cap_ftpreint_origin->getIkinCaptureOther(), // to retrieve sb_bp1
+ cap_ftpreint_origin->getGyroCaptureOther(), // to retrieve sb_w1
+ false);
+
+ return fac_ftpreint;
+}
+
+void ProcessorForceTorquePreint::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
+{
+ assert(_data.size() == data_size_ && "Wrong data size!");
+
+ // create delta
+ MatrixXd jac_body_bias(data_size_-nbc_,6);
+ VectorXd body(data_size_);
+ bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
+
+ MatrixXd jac_delta_body(12,data_size_-nbc_);
+ bodydynamics::body2delta(body, _dt, std::static_pointer_cast<SensorForceTorque>(getSensor())->getMass(),
+ nbc_, dimc_,
+ _delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
+
+ // [f], [tau], pbc, wb
+ MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_*dimc_+6);
+
+ // compute delta_cov (using uncertainty on f,tau,pbc)
+ _delta_cov = jac_delta_body_reduced * _data_cov * jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity
+ // _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose(); // trivially: jac_body_delta = Identity
+
+ // compute jacobian_calib
+ _jac_delta_calib = jac_delta_body * jac_body_bias;
+}
+
+void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
+{
+ _delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
+}
+
+void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
+{
+ assert(_delta.size() == 13 && "Wrong _delta vector size");
+ assert(_dt >= 0 && "Time interval _dt is negative!");
+
+ // verbose way : create Eigen vectors, then compute, then convert back to Composite
+
+ auto x = _x.vector("CDLO");
+
+ VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
+
+ _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,4});
+}
+
+void ProcessorForceTorquePreint::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
+{
+ bodydynamics::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in bodydynamics_tools
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorquePreint)
+}
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9dc8630d7dbd048bd8be48e3cb919b52a4c0014
--- /dev/null
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -0,0 +1,223 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_inertial_kinematics.cpp
+ *
+ * Created on: Feb 27, 2020
+ * \author: jsola
+ */
+
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+
+
+namespace wolf{
+
+
+inline ProcessorInertialKinematics::ProcessorInertialKinematics(ParamsProcessorInertialKinematicsPtr _params_ikin) :
+ ProcessorBase("ProcessorInertialKinematics", 3, _params_ikin),
+ params_ikin_(std::make_shared<ParamsProcessorInertialKinematics>(*_params_ikin)),
+ cap_origin_ptr_(nullptr)
+{
+}
+
+void ProcessorInertialKinematics::configure(SensorBasePtr _sensor)
+{
+}
+
+
+inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
+{
+ if (!_capture)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+
+ // nothing to do if any of the two buffer is empty
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+
+ // PROBLEM with this method:
+ // if the incoming capture _capture matches one of the stored KF it will not be used to create
+ // a factor until the next one arrives since it is not in the capture buffer yet (storeCapture is
+ // done AFTER processCapture)
+
+ // 1. get corresponding KF
+ FrameBasePtr kf;
+ auto buffer_frame_it = buffer_frame_.getContainer().begin();
+ auto buffer_capture_it = buffer_capture_.getContainer().begin();
+
+ auto sensor_angvel = getProblem()->getSensor(params_ikin_->sensor_angvel_name);
+ while ((buffer_frame_it != buffer_frame_.getContainer().end())
+ && (buffer_capture_it != buffer_capture_.getContainer().end()))
+ {
+
+ bool time_ok = checkTimeTolerance(buffer_frame_it->first, buffer_capture_it->first);
+ if (time_ok) {
+ CaptureBasePtr cap_angvel = buffer_frame_it->second->getCaptureOf(sensor_angvel);
+ auto min_ts = (buffer_frame_it->first < buffer_capture_it->first) ? buffer_frame_it->first : buffer_capture_it->first;
+ if (cap_angvel && cap_angvel->getStateBlock('I')){ // TODO: or only cap_angvel?
+ // cast incoming capture to the InertialKinematics type, add it to the keyframe
+ auto kf = buffer_frame_it->second;
+ auto cap_ikin = std::static_pointer_cast<CaptureInertialKinematics>(buffer_capture_it->second);
+ cap_ikin->link(kf);
+ createInertialKinematicsFactor(cap_ikin,
+ cap_angvel,
+ cap_origin_ptr_);
+ // update pointer to origin capture (the previous one attached to a KF) if we have created a new factor
+ cap_origin_ptr_ = buffer_capture_it->second;
+ buffer_capture_it++;
+ buffer_frame_it++;
+ }
+ else {
+ // if time ok but no capture angvel yet, there is not gonna be any in the next KF of the buffer
+ break;
+ buffer_capture_it++;
+ buffer_frame_it++;
+ }
+
+ ////////////////
+ // remove everything before (Inclusive if equal) this timestamp -> the cap_angvel is yet to come
+ buffer_frame_.removeUpTo(min_ts);
+ buffer_capture_.removeUpTo(min_ts);
+ }
+ else {
+ // if a time difference between captures and KF pack, we increment the oldest iterator
+ if (buffer_capture_it->first < buffer_frame_it->first){
+ buffer_capture_it++;
+ }
+ else {
+ buffer_frame_it++;
+ }
+ }
+ }
+}
+
+
+inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureInertialKinematicsPtr _cap_ikin, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_ikin_origin)
+{
+ // 3. explore all observations in the capture and create feature
+ Eigen::Vector9d data = _cap_ikin->getData();
+ Eigen::Matrix3d B_I_q = _cap_ikin->getBIq();
+ Eigen::Vector3d B_Lc_m = _cap_ikin->getBLcm();
+ auto sensor_ikin = std::static_pointer_cast<SensorInertialKinematics>(getSensor());
+ auto sensor_angvel_base = getProblem()->getSensor(params_ikin_->sensor_angvel_name);
+ Eigen::Matrix3d Qp = pow(sensor_ikin->getPbcNoiseStd(), 2) * Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qv = pow(sensor_ikin->getVbcNoiseStd(), 2) * Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qw = sensor_angvel_base->getNoiseCov().bottomRightCorner<3,3>();
+ Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp, Qv, Qw,
+ data.head<3>() - _cap_ikin->getSensorIntrinsic()->getState(), // bias value of the capture that was just created
+ data.tail<3>() - _cap_angvel->getSensorIntrinsic()->getState().tail<3>(),
+ B_I_q);
+ auto feat_ikin = FeatureBase::emplace<FeatureInertialKinematics>(_cap_ikin, data, Q_ikin_err, B_I_q, B_Lc_m);
+
+ // 4. Create inertial kinematics factor
+ auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_ikin,
+ feat_ikin,
+ _cap_angvel->getSensorIntrinsic(),
+ shared_from_this(),
+ false);
+
+ // 5. Create bias drift factor if an origin capture exists -> zero difference + covariance as integration of a Brownian noise
+ if (_cap_ikin_origin){
+ double dt_drift = _cap_ikin->getTimeStamp() - _cap_ikin_origin->getTimeStamp();
+ Eigen::Matrix3d cov_drift = Eigen::Matrix3d::Identity() * pow(params_ikin_->std_bp_drift, 2) * dt_drift;
+ FeatureBasePtr feat_diff = FeatureBase::emplace<FeatureBase>(_cap_ikin, "ComBiasDrift", Eigen::Vector3d::Zero(), cov_drift,
+ FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ StateBlockPtr sb_pbc1 = _cap_ikin_origin->getSensorIntrinsic();
+ StateBlockPtr sb_pbc2 = _cap_ikin->getSensorIntrinsic();
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ feat_diff, feat_diff, sb_pbc1, sb_pbc2, nullptr, _cap_ikin_origin
+ );
+ }
+
+ return true;
+}
+
+
+
+//////////////////////////
+// Covariance computations
+//////////////////////////
+
+Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& b_p_bc_unbiased,
+ const Eigen::Vector3d& b_omg_b_unbiased,
+ const Eigen::Matrix3d& Iq)
+{
+ Eigen::Matrix9d J; J.setZero();
+
+ Eigen::Matrix3d wx = skew(b_omg_b_unbiased);
+ Eigen::Matrix3d px = skew(b_p_bc_unbiased);
+ // Starting from top left, to the right and then next row
+ J.topLeftCorner<3,3>() = Eigen::Matrix3d::Identity();
+ // J.block<3,3>(0,3) = Matrix3d::Zero();
+ // J.topRightCorner<3,3>() = Matrix3d::Zero();
+ J.block<3,3>(3,0) = - wx;
+ J.block<3,3>(3,3) = - Eigen::Matrix3d::Identity();
+ J.block<3,3>(3,6) = px;
+ // J.bottomLeftCorner<3,3>() = Matrix3d::Zero();
+ // J.block<3,3>(6,3) = Matrix3d::Zero();
+ J.bottomRightCorner<3,3>() = -Iq;
+
+ return J;
+}
+
+
+Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
+ const Eigen::Matrix3d& Qv,
+ const Eigen::Matrix3d& Qw,
+ const Eigen::Vector3d& b_p_bc_unbiased,
+ const Eigen::Vector3d& b_omg_b_unbiased,
+ const Eigen::Matrix3d& Iq)
+{
+ Eigen::Matrix9d cov; cov.setZero();
+
+ Eigen::Matrix3d px = skew(b_p_bc_unbiased);
+ Eigen::Matrix3d wx = skew(b_omg_b_unbiased);
+ // Starting from top left, to the right and then next row
+ cov.topLeftCorner<3,3>() = Qp;
+ cov.block<3,3>(0,3) = Qp * wx;
+ // cov.topRightCorner<3,3>() = Matrix3d::Zero();
+ cov.block<3,3>(3,0) = cov.block<3,3>(0,3).transpose();
+ cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px;
+ cov.block<3,3>(3,6) = -px*Qw*Iq;
+ // cov.bottomLeftCorner<3,3>() = Matrix3d::Zero();
+ cov.block<3,3>(6,3) = Iq*Qw*px;
+ cov.bottomRightCorner<3,3>() = Iq*Qw*Iq;
+
+ return cov;
+}
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorInertialKinematics);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorInertialKinematics);
+} // namespace wolf
diff --git a/src/processor/processor_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..358ad06be77d987125dcd0341bbd10e62c6fdd78
--- /dev/null
+++ b/src/processor/processor_point_feet_nomove.cpp
@@ -0,0 +1,177 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_point_feet_nomove.cpp
+ *
+ * Created on: Oct 23, 2020
+ * \author: mfourmy
+ */
+
+#include "bodydynamics/processor/processor_point_feet_nomove.h"
+
+
+namespace wolf{
+
+
+inline ProcessorPointFeetNomove::ProcessorPointFeetNomove(ParamsProcessorPointFeetNomovePtr _params_pfnomove) :
+ ProcessorBase("ProcessorPointFeetNomove", 3, _params_pfnomove),
+ params_pfnomove_(std::make_shared<ParamsProcessorPointFeetNomove>(*_params_pfnomove))
+{
+
+}
+
+void ProcessorPointFeetNomove::configure(SensorBasePtr _sensor)
+{
+}
+
+void ProcessorPointFeetNomove::createFactorIfNecessary(){
+ auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
+
+ while (buffer_frame_.size() >= 2)
+ {
+ auto kf1_it = buffer_frame_.getContainer().begin();
+ auto kf2_it = std::next(kf1_it);
+ TimeStamp t1 = kf1_it->first;
+ TimeStamp t2 = kf2_it->first;
+ auto cap1_it = buffer_capture_.selectIterator(t1, getTimeTolerance());
+ auto cap2_it = buffer_capture_.selectIterator(t2, getTimeTolerance());
+
+ // check that the first 2 KF have corresponding captures in the capture buffer
+ // just quit and assume that you will someday have matching captures
+ if ( (cap1_it == buffer_capture_.getContainer().end()) or (cap2_it == buffer_capture_.getContainer().end()) )
+ {
+ return;
+ }
+ else
+ {
+ // store the initial contact/kinematic meas map
+ auto cap1 = std::static_pointer_cast<CapturePointFeetNomove>(cap1_it->second);
+ auto kin_incontact_from_t1 = cap1->kin_incontact_;
+ std::cout << kin_incontact_from_t1.size() << " ";
+
+
+ auto cap_it = std::next(cap1_it);
+ auto it_after_capt_t2 = std::next(cap2_it);
+
+ // loop throught the captures between t1 and t2
+ while (cap_it != it_after_capt_t2){
+ // for each new capture, filter the initial contact/kin meas map to track which feet stay in contact throughout
+ auto cap_pfeet = std::static_pointer_cast<CapturePointFeetNomove>(cap_it->second);
+ if (!cap_pfeet){
+
+ }
+
+ auto kin_incontact_current = cap_pfeet->kin_incontact_;
+ // std::cout << kin_incontact_current.size() << " ";
+
+ for (auto elt_it = kin_incontact_from_t1.cbegin(); elt_it != kin_incontact_from_t1.cend(); /* no increment */){
+ if (kin_incontact_current.find(elt_it->first) == kin_incontact_current.end()){
+ // The foot has lost contact with the ground since t1
+ elt_it = kin_incontact_from_t1.erase(elt_it); // erase and jump (from stackoverflow)
+ }
+ else{
+ elt_it++;
+ }
+ }
+ cap_it++;
+ }
+ // std::cout << std::endl;
+
+ if (!kin_incontact_from_t1.empty())
+ {
+ auto cap2 = std::static_pointer_cast<CapturePointFeetNomove>(cap2_it->second);
+ // link (kind of arbitrarily) factor from KF1 to KF2 with a feature and capture on KF2 to mimic capture motion
+ // however, this capture solely does not contain enough information to recreate the factor
+ cap2->link(kf2_it->second);
+ auto kin_incontact_t2 = cap2->kin_incontact_;
+
+ for (auto kin_pair1: kin_incontact_from_t1){
+ auto kin_pair2_it = kin_incontact_t2.find(kin_pair1.first);
+
+ Vector6d meas; meas << kin_pair1.second, kin_pair2_it->second;
+ FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, sensor_pfnm->getCovFootNomove());
+ FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
+ }
+ }
+
+ // Once the factors are created, remove kf1 and all the captures until ts 2 NOT INCLUDING the one at ts 2 since we need it for the next
+ // Note: erase by range does not include the end range iterator
+ buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), kf2_it); // !! works only if getContainer returns a non const reference
+ buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), cap2_it);
+ }
+ }
+}
+
+
+inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
+{
+ if (!_capture)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+
+}
+
+inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr)
+{
+ if (!_keyframe_ptr)
+ {
+ WOLF_ERROR("Received KF is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+}
+
+
+
+
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorPointFeetNomove);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorPointFeetNomove);
+} // namespace wolf
diff --git a/src/sensor/.blank b/src/sensor/.blank
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/sensor/sensor_force_torque.cpp b/src/sensor/sensor_force_torque.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76704952b7ce2d5971e1a181cd8a6f1ac797702b
--- /dev/null
+++ b/src/sensor/sensor_force_torque.cpp
@@ -0,0 +1,55 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "bodydynamics/sensor/sensor_force_torque.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+
+namespace wolf {
+
+SensorForceTorque::SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params) :
+ SensorBase("SensorForceTorque",
+ nullptr,
+ nullptr,
+ nullptr,
+ (Eigen::Matrix<double, 12, 1>() << _params->std_f,_params->std_f,_params->std_f,
+ _params->std_f,_params->std_f,_params->std_f,
+ _params->std_tau,_params->std_tau,_params->std_tau,
+ _params->std_tau,_params->std_tau,_params->std_tau).finished(),
+ false,
+ false,
+ true),
+ mass_(_params->mass),
+ std_f_(_params->std_f),
+ std_tau_(_params->std_tau)
+{
+ assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0");
+}
+
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorForceTorque)
+} // namespace wolf
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..33369c5f00e3d7a65383e28cb0d65d102bcc4de7
--- /dev/null
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -0,0 +1,56 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "bodydynamics/sensor/sensor_inertial_kinematics.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+
+namespace wolf {
+
+SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extrinsics, ParamsSensorInertialKinematicsPtr _params) :
+ SensorBase("SensorInertialKinematics",
+ nullptr, // no position
+ nullptr, // no orientation
+ std::make_shared<StateBlock>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
+ (Eigen::Vector6d() << _params->std_pbc,_params->std_pbc,_params->std_pbc,
+ _params->std_vbc,_params->std_vbc,_params->std_vbc).finished(),
+ false, false, true),
+ intr_ikin_(_params)
+{
+ assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0.");
+ assert(getIntrinsic()->getState().size() == 3 && "Wrong extrinsics size! Should be 3.");
+ setStateBlockDynamic('I', true);
+}
+
+SensorInertialKinematics::~SensorInertialKinematics()
+{
+ //
+}
+
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorInertialKinematics)
+} // namespace wolf
diff --git a/src/sensor/sensor_point_feet_nomove.cpp b/src/sensor/sensor_point_feet_nomove.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e5a48b7c058aa152ad4179404a061e7fe74dedd
--- /dev/null
+++ b/src/sensor/sensor_point_feet_nomove.cpp
@@ -0,0 +1,54 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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 "bodydynamics/sensor/sensor_point_feet_nomove.h"
+
+namespace wolf {
+
+SensorPointFeetNomove::SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPointFeetNomovePtr& _params) :
+ SensorBase("SensorPointFeetNomove",
+ nullptr,
+ nullptr,
+ nullptr,
+ (Eigen::Vector3d() << _params->std_foot_nomove_,_params->std_foot_nomove_,_params->std_foot_nomove_).finished(),
+ false,
+ false,
+ false)
+{
+ assert(_extrinsics_pq.size() == 0 && "Bad extrinsics vector size! Should be 0 since not used.");
+
+ cov_foot_nomove_ = pow(_params->std_foot_nomove_, 2)*Matrix3d::Identity();
+}
+
+
+SensorPointFeetNomove::~SensorPointFeetNomove()
+{
+ //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorPointFeetNomove);
+WOLF_REGISTER_SENSOR_AUTO(SensorPointFeetNomove);
+}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 268d8fa2e37f7c85dd1b4b63bf5f88615673fefc..60f23c1fe3f823d3c1c848365735928550d24095 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -22,17 +22,45 @@ add_subdirectory(gtest)
# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS} ${wolfIMU_INCLUDE_DIRS})
+include_directories(${GTEST_INCLUDE_DIRS})
################# ADD YOUR TESTS BELOW ####################
# #
# ==== IN ALPHABETICAL ORDER! ==== #
# #
+wolf_add_gtest(gtest_capture_inertial_kinematics gtest_capture_inertial_kinematics.cpp)
+target_link_libraries(gtest_capture_inertial_kinematics ${PLUGIN_NAME})
+
+wolf_add_gtest(gtest_capture_leg_odom gtest_capture_leg_odom.cpp)
+target_link_libraries(gtest_capture_leg_odom ${PLUGIN_NAME})
+
+wolf_add_gtest(gtest_feature_inertial_kinematics gtest_feature_inertial_kinematics.cpp)
+target_link_libraries(gtest_feature_inertial_kinematics ${PLUGIN_NAME})
+
wolf_add_gtest(gtest_factor_inertial_kinematics gtest_factor_inertial_kinematics.cpp)
-target_link_libraries(gtest_factor_inertial_kinematics ${PLUGIN_NAME} ${wolfIMU_LIBRARIES})
+target_link_libraries(gtest_factor_inertial_kinematics ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
-target_link_libraries(gtest_factor_force_torque ${PLUGIN_NAME} ${wolfIMU_LIBRARIES})
+target_link_libraries(gtest_factor_force_torque ${PLUGIN_NAME})
wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp)
-target_link_libraries(gtest_force_torque_delta_tools ${PLUGIN_NAME})
\ No newline at end of file
+target_link_libraries(gtest_force_torque_delta_tools ${PLUGIN_NAME})
+
+# wolf_add_gtest(gtest_feature_force_torque_preint gtest_feature_force_torque_preint.cpp)
+# target_link_libraries(gtest_feature_force_torque_preint ${PLUGIN_NAME})
+
+# wolf_add_gtest(gtest_processor_inertial_kinematics gtest_processor_inertial_kinematics.cpp)
+# target_link_libraries(gtest_processor_inertial_kinematics ${PLUGIN_NAME})
+
+# wolf_add_gtest(gtest_processor_force_torque_preint gtest_processor_force_torque_preint.cpp)
+# target_link_libraries(gtest_processor_force_torque_preint ${PLUGIN_NAME})
+
+wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
+target_link_libraries(gtest_processor_point_feet_nomove ${PLUGIN_NAME})
+
+wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
+target_link_libraries(gtest_sensor_force_torque ${PLUGIN_NAME})
+
+wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
+target_link_libraries(gtest_sensor_inertial_kinematics ${PLUGIN_NAME})
+
diff --git a/test/gtest_capture_inertial_kinematics.cpp b/test/gtest_capture_inertial_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4402ddec609e504f7f5fc63e345eb2eda6778980
--- /dev/null
+++ b/test/gtest_capture_inertial_kinematics.cpp
@@ -0,0 +1,57 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_capture_inertial_kinematics.cpp
+ *
+ * Created on: Feb 28, 2020
+ * \author: jsola
+ */
+
+
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+
+#include "core/utils/utils_gtest.h"
+#include <core/utils/logging.h>
+
+#include <Eigen/Dense>
+
+using namespace wolf;
+
+TEST(CaptureInertialKinematics, constructor_and_getters)
+{
+ SensorInertialKinematicsPtr sen;
+ Vector9d data = Vector9d::Random();
+ Matrix3d Iq = Matrix3d::Random();
+ Vector3d Lcm = Vector3d::Random();
+ Vector3d bias = Vector3d::Random();
+ CaptureInertialKinematicsPtr C = std::make_shared<CaptureInertialKinematics>(0, sen, data, Iq, Lcm, bias);
+
+ ASSERT_MATRIX_APPROX(C->getData(), data, 1e-20);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_capture_leg_odom.cpp b/test/gtest_capture_leg_odom.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b19d8db8b85ff86254e84c373cb23a4616a7833
--- /dev/null
+++ b/test/gtest_capture_leg_odom.cpp
@@ -0,0 +1,120 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_capture_leg_odom.cpp
+ *
+ * Created on: May 12, 2020
+ * \author: mfourmy
+ */
+
+
+#include "bodydynamics/capture/capture_leg_odom.h"
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+#include <Eigen/Dense>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+TEST(CaptureLegOdom, point_feet_relative_kin)
+{
+ TimeStamp ts(0.0);
+ double dt = 1;
+ // 2 point feet + wb
+ MatrixXd data_kin(3,5);
+ Vector3d bm_p_bml1; bm_p_bml1 << 0,0,0;
+ Vector3d b_p_bl1; b_p_bl1 << -1,0,0;
+ Vector3d bm_p_bml2; bm_p_bml2 << 0,0,0;
+ Vector3d b_p_bl2; b_p_bl2 << -1,0,0;
+ Vector3d i_omg_oi; i_omg_oi << 0,0,0;
+ data_kin.col(0) = bm_p_bml1;
+ data_kin.col(1) = b_p_bl1;
+ data_kin.col(2) = bm_p_bml2;
+ data_kin.col(3) = b_p_bl2;
+ data_kin.col(4) = i_omg_oi;
+
+ Matrix6d data_cov = Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
+
+ Vector3d bm_p_bmb_exp; bm_p_bmb_exp << 1, 0, 0;
+ ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
+}
+
+TEST(CaptureLegOdom, point_feet_differential_kin)
+{
+ TimeStamp ts(0.0);
+ double dt = 1;
+ // 2 point feet + wb
+ MatrixXd data_kin(10,3);
+ Vector3d i_omg_oi; i_omg_oi << 0,1,0;
+
+ // 1rst foot: only relative foot/base motion from J*dqa, no lever so no influence from angular vel
+ Vector3d b_p_bl1; b_p_bl1 << 0,0,0;
+ Vector4d b_qvec_l1; b_qvec_l1 << 0,0,0,1;
+ Vector3d l_v_bl1; l_v_bl1 << -1,0,0;
+
+ // 2nd feet: rigid rotation pivoting around the foot point
+ Vector3d b_p_bl2; b_p_bl2 << 0,0,-1;
+ Vector4d b_qvec_l2; b_qvec_l2 << 0,0,0,1;
+ Vector3d l_v_bl2; l_v_bl2 << 0,0,0;
+ data_kin.col(0) << b_p_bl1, b_qvec_l1, l_v_bl1;
+ data_kin.col(1) << b_p_bl2, b_qvec_l2, l_v_bl2;
+ data_kin.col(2) << i_omg_oi, 0,0,0,0,0,0,0;
+
+ Matrix6d data_cov = Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN);
+
+ Vector3d bm_p_bmb_exp; bm_p_bmb_exp << 1, 0, 0;
+ ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
+}
+
+TEST(CaptureLegOdom, humanoid_feet)
+{
+ TimeStamp ts(0.0);
+ double dt = 1;
+ // 1 humanoid feet + wb
+ MatrixXd data_kin(7,2);
+ // only one foot taken into account
+ Vector3d bm_p_bml1; bm_p_bml1 << 0,0,0;
+ Vector4d bm_q_bml1; bm_q_bml1 << 0,0,0,1;
+ Vector3d b_p_bl1; b_p_bl1 << -1,0,0;
+ Vector4d b_q_bl1; b_q_bl1 << 0,0,0,1;
+ data_kin.col(0) << bm_p_bml1, bm_q_bml1;
+ data_kin.col(1) << b_p_bl1, b_q_bl1;
+
+ Matrix6d data_cov = Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::HUMANOID_FEET_RELATIVE_KIN);
+
+ Vector3d bm_p_bmb_exp; bm_p_bmb_exp << 1, 0, 0;
+ ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 9eff4752348505f42b7f26484de945533e33802f..16bd8d88a91abebcf2c00cdabb7bd163f44e2046 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_inertial_kinematics.cpp
@@ -9,10 +30,10 @@
/*
-Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
+Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
Then, each test case is derived in a child class which defines the data processed by the different sensors and expected estimated test values.
-Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
-solve is done with a perturbated system.
+Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
+solve is done with a perturbated system.
Tests list:
@@ -32,25 +53,32 @@ FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
// WOLF
#include <core/problem/problem.h>
-#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/capture/capture_base.h>
#include <core/feature/feature_base.h>
#include <core/factor/factor_block_absolute.h>
-// IMU
-// #include "IMU/internal/config.h"
-// #include "IMU/capture/capture_IMU.h"
-// #include "IMU/processor/processor_IMU.h"
-// #include "IMU/sensor/sensor_IMU.h"
+// Imu
+// #include "imu/internal/config.h"
+// #include "imu/capture/capture_imu.h"
+// #include "imu/processor/processor_imu.h"
+// #include "imu/sensor/sensor_imu.h"
// BODYDYNAMICS
#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
#include "bodydynamics/feature/feature_inertial_kinematics.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
#include "bodydynamics/feature/feature_force_torque.h"
#include "bodydynamics/factor/factor_force_torque.h"
+// ODOM3d
+#include "core/capture/capture_pose.h"
+#include "core/feature/feature_pose.h"
+#include "core/factor/factor_relative_pose_3d.h"
+
#include <Eigen/Eigenvalues>
using namespace wolf;
@@ -59,8 +87,8 @@ using namespace std;
// SOME CONSTANTS
-const double Acc1 = 1.0;
-const double Acc2 = 2.5;
+const double Acc1 = 1;
+const double Acc2 = 3;
const Vector3d zero3 = Vector3d::Zero();
const Vector6d zero6 = Vector6d::Zero();
@@ -89,7 +117,7 @@ Matrix9d computeKinJac(const Vector3d& w_unb,
}
Matrix9d computeKinCov(const Matrix3d& Qp,
- const Matrix3d& Qv,
+ const Matrix3d& Qv,
const Matrix3d& Qw,
const Vector3d& w_unb,
const Vector3d& p_unb,
@@ -114,21 +142,24 @@ Matrix9d computeKinCov(const Matrix3d& Qp,
}
-void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
- if(!KF->getStateBlock(sb_name)->isFixed())
+void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
+ if(!KF->getStateBlock(sb_name)->isFixed())
{
- if (sb_name == "O")
+ if (sb_name == 'O')
{
- KF->getStateBlock("O")->setState(KF->getStateBlock("O")->getState().reverse());
+ double max_rad_pert = M_PI / 3;
+ Vector3d do_pert = max_rad_pert*Vector3d::Random();
+ Quaterniond curr_state_q(KF->getO()->getState().data());
+ KF->getStateBlock('O')->setState((curr_state_q * exp_q(do_pert)).coeffs());
}
- else
- {
+ else
+ {
VectorXd pert;
pert.resize(KF->getStateBlock(sb_name)->getSize());
- pert.setRandom();
+ pert.setRandom(); pert *= 0.5;
KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
}
- }
+ }
}
void perturbateAllIfUnFixed(const FrameBasePtr& KF)
@@ -138,15 +169,14 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
}
}
-FrameBasePtr createKFWithCDLBI(const ProblemPtr& problem, const TimeStamp& t, const Vector10d& x_origin,
- const Vector3d& c, const Vector3d& cd, const Vector3d& Lc, const Vector3d& bias_p, const Vector6d& bias_imu)
-{
- FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), "POV", 3, wolf::KEY, t, x_origin);
- StateBlockPtr sbc = make_shared<StateBlock>(c); KF->addStateBlock("C", sbc); problem->notifyStateBlock(sbc,ADD);
- StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock("D", sbd); problem->notifyStateBlock(sbd,ADD);
- StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock("L", sbL); problem->notifyStateBlock(sbL,ADD);
- StateBlockPtr sbb = make_shared<StateBlock>(bias_p); KF->addStateBlock("B", sbb); problem->notifyStateBlock(sbb,ADD);
- StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock("I", sbbimu); problem->notifyStateBlock(sbbimu,ADD);
+FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, VectorComposite x_origin,
+ Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
+{
+ FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
+ StateBlockPtr sbc = make_shared<StateBlock>(c); KF->addStateBlock('C', sbc, problem);
+ StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock('D', sbd, problem);
+ StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock('L', sbL, problem);
+ StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
return KF;
}
@@ -158,22 +188,24 @@ class FactorInertialKinematics_2KF : public testing::Test
wolf::TimeStamp tA_;
wolf::TimeStamp tB_;
ProblemPtr problem_;
- CeresManager* ceres_manager_;
- VectorXd x_origin_;
- MatrixXd P_origin_;
+ SolverCeresPtr solver_;
+ VectorComposite x_origin_; // initial state
+ VectorComposite s_origin_; // initial sigmas for prior
+ SensorInertialKinematicsPtr SIK_;
+ CaptureInertialKinematicsPtr CIKA_, CIKB_;
FrameBasePtr KFA_;
FrameBasePtr KFB_;
Matrix3d Qp_, Qv_, Qw_;
Vector3d bias_p_;
Vector6d bias_imu_;
- FeatureInertialKinematicsPtr feat_ikinA_;
- FeatureInertialKinematicsPtr feat_ikinB_;
- FeatureForceTorquePtr feat_ftAB_;
- // SensorIMUPtr sen_imu_;
- // ProcessorIMUPtr processor_imu_;
-
+ FeatureInertialKinematicsPtr FIKA_;
+ FeatureInertialKinematicsPtr FIKB_;
+ FeatureForceTorquePtr FFTAB_;
+ // SensorImuPtr sen_imu_;
+ // ProcessorImuPtr processor_imu_;
+
protected:
- virtual void SetUp()
+ void SetUp() override
{
std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
@@ -183,104 +215,98 @@ class FactorInertialKinematics_2KF : public testing::Test
// WOLF PROBLEM
problem_ = Problem::create("POV", 3);
+ VectorXd extr_ik(0);
+ ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
+ intr_ik->std_pbc = 0.1;
+ intr_ik->std_vbc = 0.1;
+ auto senik = problem_->installSensor("SensorInertialKinematics", "senIK", extr_ik, intr_ik);
+ SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
+
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e3;
- ceres_manager_ = new CeresManager(problem_, ceres_options);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ solver_->getSolverOptions().max_num_iterations = 1e3;
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
- // INSTALL IMU SENSOR
+ // INSTALL Imu SENSOR
// Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
- // SensorBasePtr sen_imu = problem_->installSensor("SensorIMU", "Main IMU Sensor", extrinsics, bodydynamics_root + "/demos/sensor_imu.yaml");
- // sen_imu_ = std::static_pointer_cast<SensorIMU>(sen_imu);
- // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorIMU", "IMU PROC", "Main IMU Sensor", bodydynamics_root + "/demos/processor_imu.yaml");
- // Vector6d data = Vector6d::Zero();
- // wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(0, sen_imu_, data, sen_imu_->getNoiseCov(), Vector6d::Zero());
+ // SensorBasePtr sen_imu = problem_->installSensor("SensorImu", "Main Imu Sensor", extrinsics, bodydynamics_root + "/demos/sensor_imu.yaml");
+ // sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu);
+ // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorImu", "Imu PROC", "Main Imu Sensor", bodydynamics_root + "/demos/processor_imu.yaml");
+ // Vector6d data = zero6;
+ // wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(0, sen_imu_, data, sen_imu_->getNoiseCov(), zero6);
// // sen_imu_->process(imu_ptr);
// ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
tA_.set(0);
- x_origin_.resize(10);
- x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
- P_origin_ = pow(1e-3, 2) * Matrix9d::Identity();
-
- // Set origin of the problem
- KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005);
- // Specific factor for origin velocity (not covered by setPrior)
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KFA_, "Vel0", tA_);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KFA_->getV());
+ x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+ s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
+ KFA_ = problem_->setPriorFactor(x_origin_, s_origin_, tA_);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_ = zero3;
bias_imu_ = zero6;
- StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("C", sbcA); problem_->notifyStateBlock(sbcA,ADD);
- StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("D", sbdA); problem_->notifyStateBlock(sbdA,ADD);
- StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("L", sbLA); problem_->notifyStateBlock(sbLA,ADD);
- StateBlockPtr sbbA = make_shared<StateBlock>(bias_p_); KFA_->addStateBlock("B", sbbA); problem_->notifyStateBlock(sbbA,ADD);
- StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock("I", sbbimuA); problem_->notifyStateBlock(sbbimuA,ADD);
-
+ StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
+ StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
+ StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
+ StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
+
// Fix the one we cannot estimate
// KFA_->getP()->fix();
// KFA_->getO()->fix();
// KFA_->getV()->fix();
- KFA_->getStateBlock("I")->fix();
- KFA_->getStateBlock("B")->fix();
+ KFA_->getStateBlock('I')->fix(); // Imu
// INERTIAL KINEMATICS FACTOR
// Measurements
- Vector3d pBC_measA = zero3;
+ Vector3d pBC_measA = zero3;
Vector3d vBC_measA = zero3;
- Vector3d w_measA = zero3;
+ Vector3d w_measA = zero3;
Vector9d meas_ikinA; meas_ikinA << pBC_measA, vBC_measA, w_measA;
-
// momentum parameters
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
+ Matrix3d Iq; Iq.setIdentity();
+ Vector3d Lq = zero3;
Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
Qw_ = pow(1e-2, 2)*Matrix3d::Identity();
- Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_measA-bias_p_, w_measA-bias_imu_.tail(3), Iq);
+ Eigen::Matrix9d Q_ikin_err = computeKinCov(Qp_, Qv_, Qw_, meas_ikinA.head<3>()-bias_p_, meas_ikinA.tail<3>()-bias_imu_.tail<3>(), Iq);
- // FACTOR INERTIAL KINEMATICS ON KFA
- CaptureBasePtr cap_ikinA = CaptureBase::emplace<CaptureBase>(KFA_, "CaptureInertialKinematics", tA_, nullptr);
- auto feat_ikinA = FeatureBase::emplace<FeatureInertialKinematics>(cap_ikinA, meas_ikinA, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- feat_ikinA_ = static_pointer_cast<FeatureInertialKinematics>(feat_ikinA); // !! auto feat_ikinA_ creates no compilation error but a segfault
- auto fac_inkinA = FactorBase::emplace<FactorInertialKinematics>(feat_ikinA_, feat_ikinA_, sbbimuA, nullptr, false);
+ CIKA_ = CaptureBase::emplace<CaptureInertialKinematics>(KFA_, tA_, SIK_, meas_ikinA, Iq, Lq, bias_p_);
+ CIKA_->getStateBlock('I')->fix(); // IK bias
+ FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ FactorBase::emplace<FactorInertialKinematics>(FIKA_, FIKA_, sbbimuA, nullptr, false);
// =============== NEW KFB WITH CORRESPONDING STATEBLOCKS
tB_.set(1);
- KFB_ = createKFWithCDLBI(problem_, tB_, x_origin_,
- zero3, zero3, zero3, bias_p_, bias_imu_);
+ KFB_ = createKFWithCDLI(problem_, tB_, x_origin_,
+ zero3, zero3, zero3, bias_imu_);
// Fix the one we cannot estimate
// KFB_->getP()->fix();
KFB_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
- KFB_->getStateBlock("I")->fix();
- KFB_->getStateBlock("B")->fix();
+ KFB_->getStateBlock('I')->fix(); // Imu
- // // ================ PROCESS IMU DATA
+ // // ================ PROCESS Imu DATA
// Vector6d data_imu;
// data_imu << -wolf::gravity(), 0,0,0;
- // CaptureIMUPtr cap_imu = std::make_shared<CaptureIMU>(tB_, sen_imu_, data_imu, sen_imu_->getNoiseCov(), bias_imu_); //set data on IMU (measure only gravity here)
+ // CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(tB_, sen_imu_, data_imu, sen_imu_->getNoiseCov(), bias_imu_); //set data on Imu (measure only gravity here)
// // process data in capture
// // sen_imu_->process(cap_imu);
// ================ FACTOR INERTIAL KINEMATICS ON KFB
- Vector3d pBC_measB = zero3;
+ Vector3d pBC_measB = zero3;
Vector3d vBC_measB = zero3;
- Vector3d w_measB = zero3;
+ Vector3d w_measB = zero3;
Vector9d meas_ikinB; meas_ikinB << pBC_measB, vBC_measB, w_measB;
- CaptureBasePtr cap_ikinB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureInertialKinematics", tB_, nullptr);
- auto feat_ikinB = FeatureBase::emplace<FeatureInertialKinematics>(cap_ikinB, meas_ikinB, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- feat_ikinB_ = static_pointer_cast<FeatureInertialKinematics>(feat_ikinB); // !! auto feat_ikinB_ creates no compilation error but a segfault
- auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(feat_ikinB_, feat_ikinB_, KFB_->getStateBlock("I"), nullptr, false);
+ CIKB_ = CaptureBase::emplace<CaptureInertialKinematics>(KFB_, tB_, SIK_, meas_ikinB, Iq, Lq, bias_p_);
+ CIKB_->getSensorIntrinsic()->fix(); // IK bias
+ FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -298,100 +324,175 @@ class FactorInertialKinematics_2KF : public testing::Test
Vector6d tau_meas; tau_meas << tau1, tau2;
Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
- Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
- Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
+ Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
+ Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
+ Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
CaptureBasePtr cap_ftB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureForceTorque", tB_, nullptr);
- feat_ftAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
- tB_ - tA_, mass_,
+ FFTAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
+ tB_ - tA_, mass_,
f_meas, tau_meas, pbc, kin_meas,
cov_f, cov_tau, cov_pbc);
- FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftAB_, feat_ftAB_, KFA_, nullptr, false);
+
+ FactorForceTorquePtr fac_ftAB = FactorBase::emplace<FactorForceTorque>(FFTAB_, FFTAB_, KFA_, CIKA_->getSensorIntrinsic(), nullptr, false);
+
+ }
+};
+
+
+class FactorInertialKinematics_2KF_foot1turned : public FactorInertialKinematics_2KF
+{
+ protected:
+ void SetUp() override
+ {
+ cout << "\n\n\nFactorInertialKinematics_2KF_foot1turned" << endl;
+ FactorInertialKinematics_2KF::SetUp();
+ problem_->print(3,false,true,true);
+ Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
+ Vector3d f1 = -mass_*wolf::gravity()/2;
+ f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
+ Vector3d f2 = -mass_*wolf::gravity()/2;
+ Vector6d fmeas; fmeas << f1, f2;
+ FFTAB_->setKinMeas(kin_meas);
+ FFTAB_->setForcesMeas(fmeas);
+ // problem_->print(3,false,true,true);
+ }
+};
+
+class FactorInertialKinematics_2KF_foot2turned : public FactorInertialKinematics_2KF
+{
+ protected:
+ void SetUp() override
+ {
+ FactorInertialKinematics_2KF::SetUp();
+ Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
+ Vector3d f1 = -mass_*wolf::gravity()/2;
+ Vector3d f2 = -mass_*wolf::gravity()/2;
+ f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
+ Vector6d fmeas; fmeas << f1, f2;
+ FFTAB_->setKinMeas(kin_meas);
+ FFTAB_->setForcesMeas(fmeas);
}
+};
- virtual void TearDown(){}
+class FactorInertialKinematics_2KF_singleContactFoot2turned : public FactorInertialKinematics_2KF
+{
+ protected:
+ void SetUp() override
+ {
+ FactorInertialKinematics_2KF::SetUp();
+ Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
+ Vector3d f1 = -mass_*wolf::gravity();
+ f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
+ Vector3d f2 = zero3;
+ Vector6d fmeas; fmeas << f1, f2;
+ FFTAB_->setKinMeas(kin_meas);
+ FFTAB_->setForcesMeas(fmeas);
+ }
+};
+
+class FactorInertialKinematics_2KF_singleContactFoot1turned : public FactorInertialKinematics_2KF
+{
+ protected:
+ void SetUp() override
+ {
+ FactorInertialKinematics_2KF::SetUp();
+ Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
+ Vector3d f1 = zero3;
+ Vector3d f2 = -mass_*wolf::gravity();
+ f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
+ Vector6d fmeas; fmeas << f1, f2;
+ FFTAB_->setKinMeas(kin_meas);
+ FFTAB_->setForcesMeas(fmeas);
+ }
};
+
+
class FactorInertialKinematics_2KF_ForceX : public FactorInertialKinematics_2KF
{
protected:
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
- feat_ftAB_->setForcesMeas(fmeas);
+ Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
+ Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
+ Vector6d fmeas; fmeas << f1, f2;
+ FFTAB_->setForcesMeas(fmeas);
}
};
class FactorInertialKinematics_2KF_ForceY : public FactorInertialKinematics_2KF
{
protected:
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
- feat_ftAB_->setForcesMeas(fmeas);
+ Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
+ Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
+ Vector6d fmeas; fmeas << f1, f2;
+ FFTAB_->setForcesMeas(fmeas);
}
};
class FactorInertialKinematics_2KF_ForceZ : public FactorInertialKinematics_2KF
{
protected:
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector6d fmeas; fmeas << f1, f2;
- feat_ftAB_->setForcesMeas(fmeas);
+ Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
+ Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
+ Vector6d fmeas; fmeas << f1, f2;
+ FFTAB_->setForcesMeas(fmeas);
}
};
class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_ForceX
{
- public:
+ public:
FrameBasePtr KFC_;
+ CaptureInertialKinematicsPtr CIKC_;
+ TimeStamp tC_;
protected:
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_2KF_ForceX::SetUp();
- TimeStamp tC;
- tC.set(2);
-
- std::cout << "\n\n\n" << feat_ftAB_->getForcesMeas().transpose() << endl;
- // WHY NOT SAME VALUE AS
+ tC_.set(2);
// KFB_->getStateBlock("O")->unfix();
-
- KFC_ = createKFWithCDLBI(problem_, tC, x_origin_,
- zero3, zero3, zero3, bias_p_, bias_imu_);
+
+ KFC_ = createKFWithCDLI(problem_, tC_, x_origin_,
+ zero3, zero3, zero3, bias_imu_);
// Fix the one we cannot estimate
// KFB_->getP()->fix();
- KFC_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
+ // KFC_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
- KFC_->getStateBlock("I")->fix();
- KFC_->getStateBlock("B")->fix();
+ KFC_->getStateBlock('I')->fix();
// ================ FACTOR INERTIAL KINEMATICS ON KFB
- Vector3d pBC_measC = zero3;
+ Vector3d pBC_measC = zero3;
Vector3d vBC_measC = zero3;
- Vector3d w_measC = zero3;
+ Vector3d w_measC = zero3;
Vector9d meas_ikinC; meas_ikinC << pBC_measC, vBC_measC, w_measC;
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
+ Matrix3d Iq; Iq.setIdentity();
+ Vector3d Lq = zero3;
+ Eigen::Matrix9d Q_ikin_errC = computeKinCov(Qp_, Qv_, Qw_, meas_ikinC.head<3>()-bias_p_, meas_ikinC.tail<3>()-bias_imu_.tail<3>(), Iq);
- Matrix9d QkinC = computeKinCov(Qp_, Qv_, Qw_, pBC_measC-bias_p_, w_measC-bias_imu_.tail(3), Iq);
- CaptureBasePtr cap_ikinB = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureInertialKinematics", tC, nullptr);
- auto feat_BC = FeatureBase::emplace<FeatureInertialKinematics>(cap_ikinB, meas_ikinC, QkinC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- auto feat_ikinBC = static_pointer_cast<FeatureInertialKinematics>(feat_BC); // !! auto feat_ikinB_ creates no compilation error but a segfault
- auto fac_inkinBC = FactorBase::emplace<FactorInertialKinematics>(feat_ikinBC, feat_ikinBC, KFC_->getStateBlock("I"), nullptr, false);
+ CIKC_ = CaptureBase::emplace<CaptureInertialKinematics>(KFC_, tC_, SIK_, meas_ikinC, Iq, Lq, bias_p_);
+ CIKC_->getStateBlock('I')->fix(); // IK bias
+ auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -409,160 +510,341 @@ class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_
Vector6d tau_meas; tau_meas << tau1, tau2;
Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
- Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
- Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
+ Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
+ Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
+ Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
- CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC, nullptr);
+ CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC_, nullptr);
auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(cap_ftC,
- tC - tB_, mass_,
+ tC_ - tB_, mass_,
f_meas, tau_meas, pbc, kin_meas,
cov_f, cov_tau, cov_pbc);
- FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, nullptr, false);
+ FactorForceTorquePtr fac_ftBC = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, CIKB_->getSensorIntrinsic(), nullptr, false);
}
};
+class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinematics_2KF_ForceX
+{
+ protected:
+ void SetUp() override
+ {
+ FactorInertialKinematics_2KF_ForceX::SetUp();
+
+ Vector7d pose_A_B; pose_A_B << (tB_-tA_)*Acc1/2,0,0, 0,0,0,1;
+ Matrix6d rel_pose_cov = Matrix6d::Identity();
+ rel_pose_cov.topLeftCorner(3, 3) *= pow(1e-3, 2);
+ rel_pose_cov.bottomRightCorner(3, 3) *= pow(M_TORAD*1e-3, 2);
+
+ CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KFB_, tB_, nullptr, pose_A_B, rel_pose_cov);
+ FeatureBasePtr ftr_pose_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, pose_A_B, rel_pose_cov);
+ FactorBase::emplace<FactorRelativePose3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false, TOP_MOTION);
+
+ // With Odom3d the bias on relative base COM position is observable, we unfix the KFB state block
+ CIKB_->getStateBlock('I')->unfix();
+ // However this test is not sufficient to observe the bias at KFA
+ // CIKA_->getStateBlock('I')->unfix(); // this is not observable
+ }
+};
+
+
+
+
+
+////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
// =============== TEST FONCTIONS ======================
////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////
TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
-{
- Vector3d c_exp = Vector3d::Zero();
- Vector3d cd_exp = Vector3d::Zero();
- Vector3d Lc_exp = Vector3d::Zero();
- Vector3d bp_exp = Vector3d::Zero();
+{
+ Vector3d c_exp = zero3;
+ Vector3d cd_exp = zero3;
+ Vector3d Lc_exp = zero3;
+ Vector3d bp_exp = zero3;
+
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ perturbateAllIfUnFixed(KFA_);
+ perturbateAllIfUnFixed(KFB_);
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+}
+
+TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
+{
+ Vector3d c_exp = zero3;
+ Vector3d cd_exp = zero3;
+ Vector3d Lc_exp = zero3;
+ Vector3d bp_exp = zero3;
+
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
- problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- std::cout << report << std::endl;
- problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), bp_exp, 1e-5);
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
+TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
+{
+ Vector3d c_exp = zero3;
+ Vector3d cd_exp = zero3;
+ Vector3d Lc_exp = zero3;
+ Vector3d bp_exp = zero3;
+
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ perturbateAllIfUnFixed(KFA_);
+ perturbateAllIfUnFixed(KFB_);
+
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+}
+
+
+TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
+{
+ Vector3d c_exp = zero3;
+ Vector3d cd_exp = zero3;
+ Vector3d Lc_exp = zero3;
+ Vector3d bp_exp = zero3;
+
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ perturbateAllIfUnFixed(KFA_);
+ perturbateAllIfUnFixed(KFB_);
+
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+}
+
+
+TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
+{
+ Vector3d c_exp = zero3;
+ Vector3d cd_exp = zero3;
+ Vector3d Lc_exp = zero3;
+ Vector3d bp_exp = zero3;
+
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ perturbateAllIfUnFixed(KFA_);
+ perturbateAllIfUnFixed(KFB_);
+
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+}
+
+
TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
-{
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+{
+ // problem_->print(4,true,true,true);
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
- problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- std::cout << report << std::endl;
- problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
-{
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+{
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
- problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- std::cout << report << std::endl;
- problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, Acc2/2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, Acc2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
-{
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+{
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
- problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- std::cout << report << std::endl;
- problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, 0, Acc1/2).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, 0, Acc1).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
-TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
-{
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+// TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
+// {
+// string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+// perturbateAllIfUnFixed(KFA_);
+// perturbateAllIfUnFixed(KFB_);
+// perturbateAllIfUnFixed(KFC_);
+
+// // problem_->print(4,true,true,true);
+// report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+// // std::cout << report << std::endl;
+// // problem_->print(4,true,true,true);
+
+// ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+// // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock('I')->getState(), zero3, 1e-5);
+// }
+
+
+TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
+{
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
- perturbateAllIfUnFixed(KFC_);
-
- problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- std::cout << report << std::endl;
- problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFC_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2 + Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
- ASSERT_MATRIX_APPROX(KFC_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
- ASSERT_MATRIX_APPROX(KFC_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
- ASSERT_MATRIX_APPROX(KFC_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+
+ // problem_->print(4,true,true,true);
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+ // problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 760db647769e9ad175f2c508284d94443187cbd4..45034d63e1e90ab4c86172a8ba81a88317cd6bb4 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_inertial_kinematics.cpp
@@ -9,10 +30,10 @@
/*
-Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
+Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
Then, each test case is derived in a child class which defines the data processed by the different sensors and expected estimated test values.
-Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
-solve is done with a perturbated system.
+Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
+solve is done with a perturbated system.
Tests list:
@@ -32,20 +53,23 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
// WOLF
#include <core/problem/problem.h>
-#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/capture/capture_base.h>
#include <core/feature/feature_base.h>
#include <core/factor/factor_block_absolute.h>
-// IMU
-#include "IMU/internal/config.h"
-#include "IMU/capture/capture_IMU.h"
-#include "IMU/processor/processor_IMU.h"
-#include "IMU/sensor/sensor_IMU.h"
+// Imu
+//#include "imu/internal/config.h"
+//#include "imu/capture/capture_imu.h"
+//#include "imu/processor/processor_imu.h"
+//#include "imu/sensor/sensor_imu.h"
// BODYDYNAMICS
#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
#include "bodydynamics/feature/feature_inertial_kinematics.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
@@ -55,382 +79,303 @@ using namespace wolf;
using namespace Eigen;
using namespace std;
-
-Matrix9d computeKinJac(const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
-{
- Matrix9d J; J.setZero();
-
- Matrix3d wx = skew(w_unb);
- Matrix3d px = skew(p_unb);
- // Starting from top left, to the right and then next row
- J.topLeftCorner<3,3>() = Matrix3d::Identity();
- // J.block<3,3>(0,3) = Matrix3d::Zero();
- // J.topRightCorner<3,3>() = Matrix3d::Zero();
- J.block<3,3>(3,0) = -wx;
- J.block<3,3>(3,3) = -Matrix3d::Identity();
- J.block<3,3>(3,6) = px;
- // J.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- // J.block<3,3>(6,3) = Matrix3d::Zero();
- J.bottomRightCorner<3,3>() = -Iq;
-
- return J;
-}
-
-Matrix9d computeKinCov(const Matrix3d& Qp,
- const Matrix3d& Qv,
- const Matrix3d& Qw,
- const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
-{
- Matrix9d cov; cov.setZero();
-
- Matrix3d wx = skew(w_unb);
- Matrix3d px = skew(p_unb);
- // Starting from top left, to the right and then next row
- cov.topLeftCorner<3,3>() = Qp;
- cov.block<3,3>(0,3) = Qp * wx;
- // cov.topRightCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(3,0) = cov.block<3,3>(0,3).transpose();
- cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px;
- cov.block<3,3>(3,6) = -px*Qw*Iq;
- // cov.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(6,3) = Iq*Qw*px;
- cov.bottomRightCorner<3,3>() = Iq*Qw*Iq;
-
- return cov;
-}
-
-
+const Vector3d ZERO3 = Vector3d::Zero();
+const Vector6d ZERO6 = Vector6d::Zero();
class FactorInertialKinematics_1KF : public testing::Test
{
public:
wolf::TimeStamp t_;
ProblemPtr problem_;
- // SensorIMUPtr sen_imu;
- CeresManager* ceres_manager_;
+ // SensorImuPtr sen_imu;
+ SolverCeresPtr solver_;
// ProcessorBasePtr processor;
- // ProcessorIMUPtr processor_imu;
- VectorXd x_origin_;
- MatrixXd P_origin_;
+ // ProcessorImuPtr processor_imu;
+ VectorComposite x_origin_; // initial state
+ VectorComposite s_origin_; // initial sigmas for prior
FrameBasePtr KF0_;
+ SensorInertialKinematicsPtr SIK_;
+ CaptureInertialKinematicsPtr CIK0_;
Eigen::Matrix3d Qp_, Qv_, Qw_;
- Eigen::Vector3d bias_p_, bias_imu_;
FeatureInertialKinematicsPtr feat_in_;
+ StateBlockPtr sbbimu_;
+ Vector3d bias_p_;
+ Vector6d bias_imu_;
- virtual void SetUp()
+ void SetUp() override
{
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
problem_ = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.max_num_iterations = 1e4;
- ceres_manager_ = new CeresManager(problem_, ceres_options);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
//===================================================== INITIALIZATION
- x_origin_.resize(10);
- x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
- P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
- t_.set(0);
-
-
- // Set origin of the problem
- KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
- // Specific factor for origin velocity (not covered by setPrior)
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+ x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+ s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
+ t_.set(0.0);
+ KF0_ = problem_->setPriorFactor(x_origin_, s_origin_, t_);
+ ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
+ SIK_ = std::make_shared<SensorInertialKinematics>(Eigen::VectorXd(0), intr_ik);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
- Vector3d zero3; zero3 << 0,0,0;
- Vector6d zero6; zero6 << zero3, zero3;
- Vector3d bias_p_ = zero3;
- Vector6d bias_imu_ = zero6;
- StateBlockPtr sbc = make_shared<StateBlock>(zero3);
- StateBlockPtr sbd = make_shared<StateBlock>(zero3);
- StateBlockPtr sbL = make_shared<StateBlock>(zero3);
+ bias_p_ = ZERO3;
+ bias_imu_ = ZERO6;
+ StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
+ StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
+ StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
- StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu_);
+ sbbimu_ = make_shared<StateBlock>(bias_imu_);
- KF0_->addStateBlock("C", sbc); problem_->notifyStateBlock(sbc,ADD);
- KF0_->addStateBlock("D", sbd); problem_->notifyStateBlock(sbd,ADD);
- KF0_->addStateBlock("L", sbL); problem_->notifyStateBlock(sbL,ADD);
- KF0_->addStateBlock("B", sbb); problem_->notifyStateBlock(sbb,ADD);
- KF0_->addStateBlock("I", sbbimu); problem_->notifyStateBlock(sbbimu,ADD);
+ KF0_->addStateBlock('C', sbc, problem_);
+ KF0_->addStateBlock('D', sbd, problem_);
+ KF0_->addStateBlock('L', sbL, problem_);
+ KF0_->addStateBlock('I', sbbimu_, problem_); // IMU bias
// Fix the one we cannot estimate
- // KF0_->getP()->fix();
- // KF0_->getO()->fix();
- // KF0_->getV()->fix();
- KF0_->getStateBlock("I")->fix();
+ KF0_->getStateBlock('I')->fix(); // Imu
// =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
// Measurements
- Vector3d pBC_meas = zero3;
- Vector3d vBC_meas = zero3;
- Vector3d w_meas = zero3;
+ Vector3d pBC_meas = ZERO3;
+ Vector3d vBC_meas = ZERO3;
+ Vector3d w_meas = ZERO3;
// momentum parameters
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
+ Matrix3d Iq; Iq.setIdentity();
+ Vector3d Lq = ZERO3;
Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
- // Qn.setZero();
- // Qn.topLeftCorner<3,3>() = Qp_;
- // Qn.block<3,3>(3,3) = Qv_;
- // Qn.bottomRightCorner<3,3>() = Qw_;
-
- Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
-
- // // Compute with the traditionnal method for comparison (unitest to extract later)
- // Matrix9d J = computeKinJac(pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
- // Matrix9d QkinFromJ = J * Qn * J.transpose();
-
- // =============== CREATE CAPURE/FEATURE/FACTOR
- Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
- CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(KF0_, "CaptureInertialKinematics", t_, nullptr);
- auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- feat_in_ = static_pointer_cast<FeatureInertialKinematics>(feat); // !! auto feat_in_ creates no compilation error but a segfault
- auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu, nullptr, false);
+
+ // =============== CREATE CAPURE
+ Vector9d meas_ikin0; meas_ikin0 << pBC_meas, vBC_meas, w_meas;
+ CIK0_ = CaptureBase::emplace<CaptureInertialKinematics>(KF0_, t_, SIK_, meas_ikin0, Iq, Lq, bias_p_);
}
- virtual void TearDown(){}
+ void TearDown() override{}
};
class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics_1KF
{
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- KF0_->getStateBlock("B")->fix();
- KF0_->getStateBlock("C")->unfix();
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
- Eigen::Vector3d pBC_meas = zero3;
- Eigen::Vector3d vBC_meas = zero3;
- Eigen::Vector3d w_meas = zero3;
+ Eigen::Vector3d pBC_meas = ZERO3;
+ Eigen::Vector3d vBC_meas = ZERO3;
+ Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
-
- Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
-
+ Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Vector3d Lq = ZERO3;
- feat_in_->setMeasurement(meas);
- feat_in_->setMeasurementCovariance(Qkin);
- feat_in_->setBIq(Iq);
- feat_in_->setBLcm(Lq);
+ // =============== CREATE FEATURE/FACTOR
+ Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1KF
{
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- KF0_->getStateBlock("B")->fix();
- KF0_->getStateBlock("C")->unfix();
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
- Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
- Eigen::Vector3d vBC_meas = zero3;
- Eigen::Vector3d w_meas = zero3;
+ Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
+ Eigen::Vector3d vBC_meas = ZERO3;
+ Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
+ Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Vector3d Lq = ZERO3;
- Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
-
- feat_in_->setMeasurement(meas);
- feat_in_->setMeasurementCovariance(Qkin);
- feat_in_->setBIq(Iq);
- feat_in_->setBLcm(Lq);
+ // =============== CREATE FEATURE/FACTOR
+ Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1KF
{
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_1KF::SetUp();
// Fix the COM position
- KF0_->getStateBlock("B")->unfix();
- KF0_->getStateBlock("C")->fix();
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
-
- Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
- Eigen::Vector3d vBC_meas = zero3;
- Eigen::Vector3d w_meas = zero3;
- bias_p_ << 1,0,0;
+ CIK0_->getStateBlock('I')->unfix();
+ KF0_->getStateBlock('C')->fix();
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
+
+ Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
+ Eigen::Vector3d vBC_meas = ZERO3;
+ Eigen::Vector3d w_meas = ZERO3;
+ bias_p_ << 1,0,0;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
-
- Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
+ Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Vector3d Lq = ZERO3;
- feat_in_->setMeasurement(meas);
- feat_in_->setMeasurementCovariance(Qkin);
- feat_in_->setBIq(Iq);
- feat_in_->setBLcm(Lq);
+ // =============== CREATE FEATURE/FACTOR
+ Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
{
- virtual void SetUp() override
+ void SetUp() override
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
- KF0_->getStateBlock("B")->fix();
- KF0_->getStateBlock("C")->unfix();
- bias_p_ = zero3;
- KF0_->getStateBlock("B")->setState(bias_p_);
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
+ bias_p_ = ZERO3;
+ CIK0_->getStateBlock('I')->setState(bias_p_);
- Eigen::Vector3d pBC_meas = zero3;
+ Eigen::Vector3d pBC_meas = ZERO3;
Eigen::Vector3d vBC_meas; vBC_meas << 1,0,0;
- Eigen::Vector3d w_meas = zero3;
+ Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
+ Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Vector3d Lq = ZERO3;
- Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
-
- feat_in_->setMeasurement(meas);
- feat_in_->setMeasurementCovariance(Qkin);
- feat_in_->setBIq(Iq);
- feat_in_->setBLcm(Lq);
+ // =============== CREATE FEATURE/FACTOR
+ Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
-class FactorInertialKinematics_IMU : public testing::Test
-{
- public:
- wolf::TimeStamp t_;
- ProblemPtr problem_;
- CeresManager* ceres_manager_;
- VectorXd x_origin_;
- MatrixXd P_origin_;
- FrameBasePtr KF0_;
- Eigen::Matrix3d Qp_, Qv_, Qw_;
- Eigen::Vector3d bias_p_, bias_imu_;
- FeatureInertialKinematicsPtr feat_in_;
- SensorIMUPtr sen_imu_;
- ProcessorIMUPtr processor_imu_;
-
- virtual void SetUp()
- {
- // using shared_ptr;
- // using make_shared;
- // using static_pointer_cast;
-
- std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- problem_ = Problem::create("POV", 3);
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.max_num_iterations = 1e4;
- ceres_manager_ = new CeresManager(problem_, ceres_options);
-
- //===================================================== INITIALIZATION
- x_origin_.resize(10);
- x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
- P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
- t_.set(0);
-
-
- // Set origin of the problem
- KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
- // Specific factor for origin velocity (not covered by setPrior)
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
-
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem_->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root + "/demos/sensor_imu.yaml");
- sen_imu_ = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", bodydynamics_root + "/demos/processor_imu.yaml");
- processor_imu_ = std::static_pointer_cast<ProcessorIMU>(proc);
-
-
- // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
- Vector3d zero3; zero3 << 0,0,0;
- Vector6d zero6; zero6 << zero3, zero3;
- Vector3d bias_p_ = zero3;
- Vector6d bias_imu_ = zero6;
- StateBlockPtr sbc = make_shared<StateBlock>(zero3);
- StateBlockPtr sbd = make_shared<StateBlock>(zero3);
- StateBlockPtr sbL = make_shared<StateBlock>(zero3);
- StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
-
- KF0_->addStateBlock("C", sbc); problem_->notifyStateBlock(sbc,ADD);
- KF0_->addStateBlock("D", sbd); problem_->notifyStateBlock(sbd,ADD);
- KF0_->addStateBlock("L", sbL); problem_->notifyStateBlock(sbL,ADD);
- KF0_->addStateBlock("B", sbb); problem_->notifyStateBlock(sbb,ADD);
-
- // Fix the one we cannot estimate
- // KF0_->getP()->fix();
- // KF0_->getO()->fix();
- // KF0_->getV()->fix();
-
- // =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
- // Measurements
- Vector3d pBC_meas = zero3;
- Vector3d vBC_meas = zero3;
- Vector3d w_meas = zero3;
-
- // momentum parameters
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
-
- Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
- Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
- Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
- // Qn.setZero();
- // Qn.topLeftCorner<3,3>() = Qp_;
- // Qn.block<3,3>(3,3) = Qv_;
- // Qn.bottomRightCorner<3,3>() = Qw_;
-
- Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
-
- // // Compute with the traditionnal method for comparison (unitest to extract later)
- // Matrix9d J = computeKinJac(pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
- // Matrix9d QkinFromJ = J * Qn * J.transpose();
-
- // =============== CREATE CAPURE/FEATURE/FACTOR
- Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
- CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(KF0_, "CaptureInertialKinematics", t_, nullptr);
- auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- feat_in_ = static_pointer_cast<FeatureInertialKinematics>(feat); // !! auto feat_in_ creates no compilation error but a segfault
- // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu, nullptr, false);
- }
-
- virtual void TearDown(){}
-};
+//class FactorInertialKinematic_imu : public testing::Test
+//{
+// public:
+// wolf::TimeStamp t_;
+// ProblemPtr problem_;
+// SolverCeres* solver_;
+// VectorXd x_origin_;
+// MatrixXd P_origin_;
+// FrameBasePtr KF0_;
+// Eigen::Matrix3d Qp_, Qv_, Qw_;
+// Eigen::Vector3d bias_p_, bias_imu_;
+// FeatureInertialKinematicsPtr feat_in_;
+// SensorImuPtr sen_imu_;
+// ProcessorImuPtr processor_imu_;
+//
+// virtual void SetUp()
+// {
+// // using shared_ptr;
+// // using make_shared;
+// // using static_pointer_cast;
+//
+// std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+//
+// //===================================================== SETTING PROBLEM
+// // WOLF PROBLEM
+// problem_ = Problem::create("POV", 3);
+//
+// // CERES WRAPPER
+// ceres::Solver::Options ceres_options;
+// // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+// // ceres_options.max_line_search_step_contraction = 1e-3;
+// // ceres_options.max_num_iterations = 1e4;
+// solver_ = new SolverCeres(problem_, ceres_options);
+//
+// //===================================================== INITIALIZATION
+// x_origin_.resize(10);
+// x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
+// P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+// t_.set(0);
+//
+//
+// // Set origin of the problem
+// KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
+// // Specific factor for origin velocity (not covered by setPrior)
+// CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
+// FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
+// FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+//
+//
+// // SENSOR + PROCESSOR Imu
+// SensorBasePtr sen0_ptr = problem_->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root + "/demos/sensor_imu.yaml");
+// sen_imu_ = std::static_pointer_cast<SensorImu>(sen0_ptr);
+// ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", bodydynamics_root + "/demos/processor_imu.yaml");
+// processor_imu_ = std::static_pointer_cast<ProcessorImu>(proc);
+//
+//
+// // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
+// Vector3d ZERO3; ZERO3 << 0,0,0;
+// Vector6d ZERO6; ZERO6 << ZERO3, ZERO3;
+// Vector3d bias_p_ = ZERO3;
+// Vector6d bias_imu_ = ZERO6;
+// StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
+// StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
+// StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
+// StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
+//
+// KF0_->addStateBlock("C", sbcproblem_);
+// KF0_->addStateBlock("D", sbdproblem_);
+// KF0_->addStateBlock("L", sbLproblem_);
+//
+// // Fix the one we cannot estimate
+// // KF0_->getP()->fix();
+// // KF0_->getO()->fix();
+// // KF0_->getV()->fix();
+//
+// // =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
+// // Measurements
+// Vector3d pBC_meas = ZERO3;
+// Vector3d vBC_meas = ZERO3;
+// Vector3d w_meas = ZERO3;
+//
+// // momentum parameters
+// Matrix3d Iq; Iq.setIdentity();
+// Vector3d Lq = ZERO3;
+//
+// Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
+// Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
+// Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
+// // Qn.setZero();
+// // Qn.topLeftCorner<3,3>() = Qp_;
+// // Qn.block<3,3>(3,3) = Qv_;
+// // Qn.bottomRightCorner<3,3>() = Qw_;
+//
+// Matrix9d Qkin = computeIKinCov(Qp_, Qv_, Qw_, pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
+//
+// // // Compute with the traditionnal method for comparison (unitest to extract later)
+// // Matrix9d J = computeIKinJac(pBC_meas-bias_p_, w_meas-bias_imu_.tail(3), Iq);
+// // Matrix9d QkinFromJ = J * Qn * J.transpose();
+//
+// // =============== CREATE CAPURE/FEATURE/FACTOR
+// Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
+// CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(KF0_, "CaptureInertialKinematics", t_, nullptr);
+// auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+// feat_in_ = static_pointer_cast<FeatureInertialKinematics>(feat); // !! auto feat_in_ creates no compilation error but a segfault
+// // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
+// }
+//
+// virtual void TearDown(){}
+//};
@@ -440,120 +385,66 @@ class FactorInertialKinematics_IMU : public testing::Test
TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt)
-{
- Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
- // Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
-
- Vector3d errP = Vector3d::Random();
- Vector3d errV = Vector3d::Random();
- Vector3d errC = Vector3d::Random();
- Vector3d errD = Vector3d::Random();
- Vector3d errL = Vector3d::Random();
- // Vector3d errBp = Vector3d::Random();
-
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- // throw std::invalid_argument( "received negative value" );
- KF0_->getP()->setState(KF0_->getP()->getState() + errP);
- KF0_->getO()->setState(KF0_->getO()->getState().reverse());
- KF0_->getV()->setState(KF0_->getV()->getState() + errV);
- KF0_->getStateBlock("C")->setState(KF0_->getStateBlock("C")->getState() + errC);
- KF0_->getStateBlock("D")->setState(KF0_->getStateBlock("D")->getState() + errD);
- KF0_->getStateBlock("L")->setState(KF0_->getStateBlock("L")->getState() + errL);
- // KF0_->getStateBlock("B")->setState(KF0_->getStateBlock("B")->getState() + errBp);
-
- problem_->print(3,0,1,1);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- std::cout << report << std::endl;
- problem_->print(3,0,1,1);
-
- ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.head(3), 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- // ASSERT_MATRIX_APPROX(KF0_->getStateBlock("B")->getState(), bp_exp, 1e-5);
+{
+ Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
+ // Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
+
+ KF0_->perturb();
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_1KF_1p_bpfix,ZeroMvt)
-{
- Eigen::Vector3d c_exp; c_exp << 1,0,0;
- Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
- // Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
-
- // Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
- Vector3d errC = Vector3d::Random();
- Vector3d errD = Vector3d::Random();
- Vector3d errL = Vector3d::Random();
- // Vector3d errBp = Vector3d::Random();
-
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- KF0_->getStateBlock("C")->setState(KF0_->getStateBlock("C")->getState() + errC);
- KF0_->getStateBlock("D")->setState(KF0_->getStateBlock("D")->getState() + errD);
- KF0_->getStateBlock("L")->setState(KF0_->getStateBlock("L")->getState() + errL);
- // KF0_->getStateBlock("B")->setState(KF0_->getStateBlock("B")->getState() + errBp);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- // ASSERT_MATRIX_APPROX(KF0_->getStateBlock("B")->getState(), bp_exp, 1e-5);
+{
+ Eigen::Vector3d c_exp; c_exp << 1,0,0;
+ Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
+ // Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
+
+ KF0_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt)
-{
- // Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d bp_exp; bp_exp << 1,0,0;
-
- // Vector3d errC = Vector3d::Random();
- Vector3d errD = Vector3d::Random();
- Vector3d errL = Vector3d::Random();
- Vector3d errBp = Vector3d::Random();
-
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- // KF0_->getStateBlock("C")->setState(KF0_->getStateBlock("C")->getState() + errC);
- KF0_->getStateBlock("D")->setState(KF0_->getStateBlock("D")->getState() + errD);
- KF0_->getStateBlock("L")->setState(KF0_->getStateBlock("L")->getState() + errL);
- KF0_->getStateBlock("B")->setState(KF0_->getStateBlock("B")->getState() + errBp);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- // ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("B")->getState(), bp_exp, 1e-5);
+{
+ // Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d bp_exp; bp_exp << 1,0,0;
+
+ KF0_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ // ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIK0_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_1KF_1v_bfix,ZeroMvt)
-{
- Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d cd_exp; cd_exp << 1,0,0;
- Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
- // Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
-
- Vector3d errC = Vector3d::Random();
- Vector3d errD = Vector3d::Random();
- Vector3d errL = Vector3d::Random();
- // Vector3d errBp = Vector3d::Random();
-
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- KF0_->getStateBlock("C")->setState(KF0_->getStateBlock("C")->getState() + errC);
- KF0_->getStateBlock("D")->setState(KF0_->getStateBlock("D")->getState() + errD);
- KF0_->getStateBlock("L")->setState(KF0_->getStateBlock("L")->getState() + errL);
- // KF0_->getStateBlock("B")->setState(KF0_->getStateBlock("B")->getState() + errBp);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // problem_->print(3,0,1,1);
-
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- // ASSERT_MATRIX_APPROX(KF0_->getStateBlock("B")->getState(), bp_exp, 1e-5);
+{
+ Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d cd_exp; cd_exp << 1,0,0;
+ Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
+ // Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
+
+ KF0_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
diff --git a/test/gtest_feature_inertial_kinematics.cpp b/test/gtest_feature_inertial_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..41d39701af5ac0ed184a8324ac88ba2faf51c6c1
--- /dev/null
+++ b/test/gtest_feature_inertial_kinematics.cpp
@@ -0,0 +1,88 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_feature_inertial_kinematics.cpp
+ *
+ * Created on: March 9, 2020
+ * \author: mfourmy
+ */
+
+
+#include "bodydynamics/feature/feature_inertial_kinematics.h"
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+#include <Eigen/Dense>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+TEST(FeatureInertialKinematics, jacobian_covariance_and_constructor)
+{
+ Vector3d pBC_meas = Vector3d::Random();
+ Vector3d vBC_meas = Vector3d::Random();
+ Vector3d w_meas = Vector3d::Random();
+
+ Eigen::Matrix3d Qp = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qv = pow(2e-2, 2)*Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qw = pow(3e-2, 2)*Eigen::Matrix3d::Identity();
+
+ // momentum parameters
+ Matrix3d Iq = Matrix3d::Identity();
+ Vector3d Lq = Vector3d::Random();
+ Vector9d meas_ikin; meas_ikin << pBC_meas, vBC_meas, w_meas;
+
+ // direct computation of error covariance (analytical formula of first order propagation)
+ Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp, Qv, Qw, pBC_meas, w_meas, Iq);
+
+ // covariance from the first order propagation (using jacobian analytical formula)
+ Matrix9d J_err_noise = computeIKinJac(pBC_meas, w_meas, Iq);
+ Eigen::Matrix9d Q_meas = Eigen::Matrix9d::Zero();
+ Q_meas.block<3,3>(0,0) = Qp;
+ Q_meas.block<3,3>(3,3) = Qv;
+ Q_meas.block<3,3>(6,6) = Qw;
+ Eigen::Matrix9d Q_ikin_err_from_Jc = J_err_noise * Q_meas * J_err_noise.transpose();
+ FeatureInertialKinematicsPtr feat_ptr = std::make_shared<FeatureInertialKinematics>(meas_ikin, Q_ikin_err, Iq, Lq);
+
+ // Test if analytic covariance matches with the explicit jacobian propagation formula
+ ASSERT_MATRIX_APPROX(Q_ikin_err, Q_ikin_err_from_Jc, 1e-3);
+
+ // Simple constructor tests
+ ASSERT_MATRIX_APPROX(Iq, feat_ptr->getBIq(), 1e-3);
+ ASSERT_MATRIX_APPROX(Lq, feat_ptr->getBLcm(), 1e-3);
+ Iq = 2*Matrix3d::Identity();
+ Lq = Vector3d::Random();
+ feat_ptr->setBIq(Iq);
+ feat_ptr->setBLcm(Lq);
+ ASSERT_MATRIX_APPROX(Iq, feat_ptr->getBIq(), 1e-3);
+ ASSERT_MATRIX_APPROX(Lq, feat_ptr->getBLcm(), 1e-3);
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_force_torque_delta_tools.cpp b/test/gtest_force_torque_delta_tools.cpp
index bb7397182a2d9779a46feee1ae53f5f5633d777b..e27553d54600d3538c6b87671ecfa276ace51182 100644
--- a/test/gtest_force_torque_delta_tools.cpp
+++ b/test/gtest_force_torque_delta_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_force_torque_delta_tools.cpp
*
@@ -12,7 +33,7 @@ using namespace Eigen;
using namespace wolf;
using namespace bodydynamics;
-TEST(IMU_tools, identity)
+TEST(FT_tools, identity)
{
VectorXd id_true;
id_true.setZero(13);
@@ -22,7 +43,7 @@ TEST(IMU_tools, identity)
ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
}
-TEST(IMU_tools, identity_split)
+TEST(FT_tools, identity_split)
{
VectorXd c(3), cd(3), Lc(3), qv(4);
Quaterniond q;
@@ -40,7 +61,7 @@ TEST(IMU_tools, identity_split)
ASSERT_MATRIX_APPROX(qv, (Vector4d()<<0,0,0,1).finished(), 1e-10);
}
-TEST(IMU_tools, inverse)
+TEST(FT_tools, inverse)
{
VectorXd d(13), id(13), iid(13), iiid(13), I(13);
Vector4d qv;
@@ -62,240 +83,280 @@ TEST(IMU_tools, inverse)
ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
}
-// TEST(IMU_tools, compose_between)
-// {
-// VectorXd dx1(10), dx2(10), dx3(10);
-// Matrix<double, 10, 1> d1, d2, d3;
-// Vector4d qv;
-// double dt = 0.1;
-
-// qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
-// dx1 << 0, 1, 2, qv, 7, 8, 9;
-// d1 = dx1;
-// qv = (Vector4d() << 6, 5, 4, 3).finished().normalized();
-// dx2 << 9, 8, 7, qv, 2, 1, 0;
-// d2 = dx2;
-
-// // 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);
-
-// // minus(d1, d3) should recover delta_2
-// VectorXd diffx(10);
-// Matrix<double,10,1> diff;
-// 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);
-// }
-
-// TEST(IMU_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(IMU_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(FT_tools, compose_between)
+{
+ VectorXd dx1(13), dx2(13), dx3(13);
+ Matrix<double, 13, 1> d1, d2, d3;
+ double dt = 0.1;
-// TEST(IMU_tools, plus)
-// {
-// VectorXd d1(10), d2(10), d3(10);
-// VectorXd err(9);
-// Vector4d qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
-// d1 << 0, 1, 2, qv, 7, 8, 9;
-// err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09;
-
-// d3.head(3) = d1.head(3) + err.head(3);
-// d3.segment(3,4) = (Quaterniond(qv.data()) * exp_q(err.segment(3,3))).coeffs();
-// d3.tail(3) = d1.tail(3) + err.tail(3);
-
-// plus(d1, err, d2);
-// ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(9), 1e-10);
-// }
+ dx1 << Vector9d::Random(), Vector4d::Random().normalized();
+ d1 = dx1;
+ dx2 << Vector9d::Random(), Vector4d::Random().normalized();
+ d2 = dx2;
+
+ // 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);
+
+ // // minus(d1, d3) should recover delta_2
+ VectorXd diffx(13);
+ Matrix<double,13,1> diff;
+ 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);
+}
-// TEST(IMU_tools, diff)
-// {
-// VectorXd d1(10), d2(10);
-// Vector4d qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
-// d1 << 0, 1, 2, qv, 7, 8, 9;
-// d2 = d1;
+TEST(FT_tools, compose_between_with_state)
+{
+ VectorXd x(13), d(13), x2(13), x3(13), d2(13), d3(13);
+ double dt = 0.1;
-// VectorXd err(9);
-// diff(d1, d2, err);
-// ASSERT_MATRIX_APPROX(err, VectorXd::Zero(9), 1e-10);
-// ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXd::Zero(9), 1e-10);
+ x << Vector9d::Random(), Vector4d::Random().normalized();
+ d << Vector9d::Random(), Vector4d::Random().normalized();
-// VectorXd d3(10);
-// d3.setRandom(); d3.segment(3,4).normalize();
-// err.head(3) = d3.head(3) - d1.head(3);
-// err.segment(3,3) = log_q(Quaterniond(d1.data()+3).conjugate()*Quaterniond(d3.data()+3));
-// err.tail(3) = d3.tail(3) - d1.tail(3);
+ composeOverState(x, d, dt, x2);
+ x3 = composeOverState(x, d, dt);
+ ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
-// ASSERT_MATRIX_APPROX(err, diff(d1, d3), 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);
-// TEST(IMU_tools, compose_jacobians)
-// {
-// VectorXd d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
-// VectorXd t1(9), t2(9); // tangents
-// Matrix<double, 9, 9> J1_a, J2_a, J1_n, J2_n;
-// Vector4d qv1, qv2;
-// double dt = 0.1;
-// double dx = 1e-6;
-// qv1 = (Vector4d() << 3, 4, 5, 6).finished().normalized();
-// d1 << 0, 1, 2, qv1, 7, 8, 9;
-// qv2 = (Vector4d() << 1, 2, 3, 4).finished().normalized();
-// d2 << 9, 8, 7, qv2, 2, 1, 0;
-
-// // analytical jacobians
-// compose(d1, d2, dt, d3, J1_a, J2_a);
+ // (x + d) - x = d
+ ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
+}
-// // 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 = 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 = 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
-// }
+TEST(FT_tools, lift_retract)
+{
+ VectorXd d_min(12); d_min << Vector9d::Random(), .1*Vector3d::Random(); // use angles in the ball theta < pi
+
+ // transform to delta
+ VectorXd delta = exp_FT(d_min);
+
+ // expected delta
+ Vector3d dc = d_min.head(3);
+ Vector3d dcd = d_min.segment<3>(3);
+ Vector3d dLc = d_min.segment<3>(6);
+ Quaterniond dq = v2q(d_min.segment<3>(9));
+ VectorXd delta_true(13); delta_true << dc, dcd, dLc, dq.coeffs();
+ ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
+
+ // transform to a new tangent -- should be the original tangent
+ VectorXd d_from_delta = log_FT(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_FT(d_from_delta);
+ ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
+}
-// // 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(FT_tools, plus)
+{
+ VectorXd d1(13), d2(13), d3(13);
+ VectorXd err(12);
+ Vector4d qv = Vector4d::Random().normalized();
+ d1 << Vector9d::Random(), qv;
+ err << Matrix<double, 12, 1>::Random();
+
+ d3.head<3>() = d1.head<3>() + err.head<3>();
+ d3.segment<3>(3) = d1.segment<3>(3) + err.segment<3>(3);
+ d3.segment<3>(6) = d1.segment<3>(6) + err.segment<3>(6);
+ d3.tail<4>() = (Quaterniond(qv.data()) * exp_q(err.tail<3>())).coeffs();
+
+ plus(d1, err, d2);
+ ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(12), 1e-10);
+}
-// TEST(IMU_tools, diff_jacobians)
-// {
-// VectorXd d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
-// VectorXd t1(9), t2(9); // tangents
-// Matrix<double, 9, 9> J1_a, J2_a, J1_n, J2_n;
-// Vector4d qv1, qv2;
-// double dx = 1e-6;
-// qv1 = (Vector4d() << 3, 4, 5, 6).finished().normalized();
-// d1 << 0, 1, 2, qv1, 7, 8, 9;
-// qv2 = (Vector4d() << 1, 2, 3, 4).finished().normalized();
-// d2 << 9, 8, 7, qv2, 2, 1, 0;
-
-// // analytical jacobians
-// diff(d1, d2, d3, J1_a, J2_a);
+TEST(FT_tools, diff)
+{
+ VectorXd d1(13), d2(13);
+ VectorXd err(12);
+ Vector4d qv = Vector4d::Random().normalized();
+ d1 << Vector9d::Random(), qv;
+ d2 = d1;
+
+ diff(d1, d2, err);
+ ASSERT_MATRIX_APPROX(err, VectorXd::Zero(12), 1e-10);
+ ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXd::Zero(12), 1e-10);
+
+ VectorXd d3(13);
+ d3.setRandom(); d3.tail<4>().normalize();
+ err.head<3>() = d3.head<3>() - d1.head<3>();
+ err.segment<3>(3) = d3.segment<3>(3) - d1.segment<3>(3);
+ err.segment<3>(6) = d3.segment<3>(6) - d1.segment<3>(6);
+ err.tail<3>() = log_q(Quaterniond(d1.data()+9).conjugate()*Quaterniond(d3.data()+9));
+
+ ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
+}
-// // 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
-// }
+TEST(FT_tools, compose_jacobians)
+{
+ VectorXd d1(13), d2(13), d3(13), d1_pert(13), d2_pert(13), d3_pert(13); // deltas
+ VectorXd t1(12), t2(12); // tangent elements
+ Matrix<double, 12, 12> J1_a, J2_a, J1_n, J2_n;
+ Vector4d qv1, qv2;
+ double dt = 0.1;
+ double dx = 1e-6;
+ qv1 = Vector4d::Random().normalized();
+ d1 << Vector9d::Random(), qv1;
+ qv2 = Vector4d::Random().normalized();
+ d2 << Vector9d::Random(), qv2;
+
+ // analytical jacobians
+ compose(d1, d2, dt, d3, J1_a, J2_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<12; i++)
+ {
+ t1 . setZero();
+ t1(i) = dx;
+
+ // Jac wrt first delta
+ d1_pert = 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 = 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);
+}
-// // 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(FT_tools, diff_jacobians)
+{
+ VectorXd d1(13), d2(13), diff3(12), d1_pert(13), d2_pert(13), diff3_pert(12); // deltas
+ VectorXd t1(12), t2(12); // tangents
+ Matrix<double, 12, 12> J1_a, J2_a, J1_n, J2_n;
+ Vector4d qv1, qv2;
+ double dx = 1e-6;
+ qv1 = Vector4d::Random().normalized();
+ d1 << Vector9d::Random(), qv1;
+ qv2 = Vector4d::Random().normalized();
+ d2 << Vector9d::Random(), qv2;
+
+ // analytical jacobians
+ diff(d1, d2, diff3, J1_a, J2_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<12; i++)
+ {
+ t1 . setZero();
+ t1(i) = dx;
+
+ // Jac wrt first delta
+ d1_pert = plus(d1, t1); // (d1 + t1)
+ diff3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
+ t2 = diff3_pert - diff3; // { 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)
+ diff3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
+ t2 = diff3_pert - diff3; // { (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(IMU_tools, body2delta_jacobians)
-// {
-// VectorXd delta(10), delta_pert(10); // deltas
-// VectorXd body(6), pert(6);
-// VectorXd tang(9); // tangents
-// Matrix<double, 9, 6> J_a, J_n; // analytic and numerical jacs
-// Vector4d qv;;
-// double dt = 0.1;
-// double dx = 1e-6;
-// qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
-// delta << 0, 1, 2, qv, 7, 8, 9;
-// body << 1, 2, 3, 3, 2, 1;
+TEST(FT_tools, body2delta_jacobians)
+{
+ VectorXd delta(13), delta_pert(13); // deltas
+ VectorXd body(32), pert(32), body_pert(32);
+ VectorXd tang(12); // tangents
+ Matrix<double, 12, 30> J_a, J_n; // analytic and numerical jacs
+ double dt = 0.1;
+ double mass = 1;
+ double dx = 1e-6;
+ Vector4d qv = Vector4d::Random().normalized();
+ delta << Vector9d::Random(), qv;
+ Vector4d bql1 = Vector4d::Random().normalized();
+ Vector4d bql2 = Vector4d::Random().normalized();
+ body << Matrix<double, 24, 1>::Random(), bql1, bql2; //
+
+ // analytical jacobians
+ body2delta(body, dt, mass, 2, 6, delta, J_a);
+
+ // numerical jacobians
+ J_n.setZero();
+ for (unsigned int i = 0; i<18; i++)
+ {
+ pert . setZero();
+ pert(i) = dx;
+
+ // Jac wrt first delta
+ body_pert = body + pert;
+ delta_pert = body2delta(body_pert, dt, mass, 2, 6); // 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.block(0,0,12,18), J_n.block(0,0,12,18), 1e-4); // only compare the implemented parts -> kinematics uncertainty not considered here
+ ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+}
-// // analytical jacobians
-// body2delta(body, dt, delta, J_a);
-// // numerical jacobians
-// for (unsigned int i = 0; i<6; i++)
-// {
-// pert . setZero();
-// pert(i) = dx;
+TEST(FT_tools, debiasData_jacobians)
+{
+ VectorXd data(32), bias(6), bias_pert(6), pert(6), body(32), body_pert(32), tang(32);
+ Matrix<double, 30, 6> J_a, J_n; // analytic and numerical jacs
+ double dx = 1e-6;
+ Vector4d bql1 = Vector4d::Random().normalized();
+ Vector4d bql2 = Vector4d::Random().normalized();
+ data << Matrix<double, 24, 1>::Random(), bql1, bql2; //
+ bias << Vector6d::Random();
+
+ // analytical jacobians
+ debiasData(data, bias, 2, 6, body, J_a);
+
+ // numerical jacobian
+ J_n.setZero();
+ for (unsigned int i = 0; i<6; i++)
+ {
+ pert . setZero();
+ pert(i) = dx;
+
+ // Jac wrt first delta
+ bias_pert = bias + pert;
+ debiasData(data, bias_pert, 2, 6, body_pert);
+ // The next line is actually a cheating:
+ // we know that the quaternions at the end of the body vector are not influenced by the
+ // bias perturbation so we just take the euclidean difference of the 2 vectors and remove the last 2 elements
+ // to get the tangent space vector
+ tang = (body_pert - body).segment<30>(0);
+
+ J_n.col(i) = tang/dx;
+ }
+
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+}
-// // 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);
-// }
// TEST(motion2data, zero)
// {
@@ -395,7 +456,7 @@ TEST(IMU_tools, inverse)
// * 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_real: the real bias of the Imu
// * bias_preint: the bias used for Delta pre-integration
// * Output:
// * return: the preintegrated delta
@@ -404,8 +465,8 @@ TEST(IMU_tools, inverse)
// {
// VectorXd data(6);
// VectorXd body(6);
-// VectorXd delta(10);
-// VectorXd Delta(10), Delta_plus(10);
+// VectorXd delta(13);
+// VectorXd Delta(13), Delta_plus(13);
// Delta << 0,0,0, 0,0,0,1, 0,0,0;
// Quaterniond q(q0);
// for (int n = 0; n<N; n++)
@@ -425,7 +486,7 @@ TEST(IMU_tools, inverse)
// * 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_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
@@ -435,10 +496,10 @@ TEST(IMU_tools, inverse)
// {
// VectorXd data(6);
// VectorXd body(6);
-// VectorXd delta(10);
+// VectorXd delta(13);
// 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);
+// VectorXd Delta(13), Delta_plus(13);
// Quaterniond q;
// Delta << 0,0,0, 0,0,0,1, 0,0,0;
@@ -450,12 +511,12 @@ TEST(IMU_tools, inverse)
// data = motion2data(motion, q, bias_real);
// q = q*exp_q(motion.tail(3)*dt);
// // Motion::integrateOneStep()
-// { // IMU::computeCurrentDelta
+// { // Imu::computeCurrentDelta
// body = data - bias_preint;
// body2delta(body, dt, delta, J_d_d);
// J_d_b = - J_d_d;
// }
-// { // IMU::deltaPlusDelta
+// { // Imu::deltaPlusDelta
// compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
// }
// // Motion:: jac calib
@@ -466,9 +527,9 @@ TEST(IMU_tools, inverse)
// return Delta;
// }
-// TEST(IMU_tools, integral_jacobian_bias)
+// TEST(FT_tools, integral_jacobian_bias)
// {
-// VectorXd Delta(10), Delta_pert(10); // deltas
+// VectorXd Delta(13), Delta_pert(13); // deltas
// VectorXd bias_real(6), pert(6), bias_pert(6), bias_preint(6);
// VectorXd body(6), data(6), motion(6);
// VectorXd tang(9); // tangents
@@ -501,9 +562,9 @@ TEST(IMU_tools, inverse)
// ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
// }
-// TEST(IMU_tools, delta_correction)
+// TEST(FT_tools, delta_correction)
// {
-// VectorXd Delta(10), Delta_preint(10), Delta_corr; // deltas
+// VectorXd Delta(13), Delta_preint(13), Delta_corr; // deltas
// VectorXd bias(6), pert(6), bias_preint(6);
// VectorXd body(6), motion(6);
// VectorXd tang(9); // tangents
@@ -538,11 +599,11 @@ TEST(IMU_tools, inverse)
// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
// }
-// TEST(IMU_tools, full_delta_residual)
+// TEST(FT_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 x1(13), x2(13);
+// VectorXd Delta(13), Delta_preint(13), Delta_corr(13);
+// VectorXd Delta_real(9), Delta_err(9), Delta_diff(13), Delta_exp(13);
// VectorXd bias(6), pert(6), bias_preint(6), bias_null(6);
// VectorXd body(6), motion(6);
// VectorXd tang(9); // tangents
@@ -625,7 +686,7 @@ TEST(IMU_tools, inverse)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
+// ::testing::GTEST_FLAG(filter) = "FT_tools.delta_correction";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3465ea6187358cffe667af6a2dd326d279e31e47
--- /dev/null
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -0,0 +1,803 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_force_torque_preint.cpp
+ *
+ * Created on: March 20, 2020
+ * \author: Mederic Fourmy
+ */
+
+// debug
+#include <iostream>
+#include <fstream>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+
+// #include <core/ceres_wrapper/qr_manager.h>
+
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/factor/factor_relative_pose_3d.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_force_torque_preint.h"
+
+#include <Eigen/Eigenvalues>
+
+using namespace wolf;
+using namespace Eigen;
+using namespace std;
+
+const Vector3d ZERO3 = Vector3d::Zero();
+const Vector6d ZERO6 = Vector6d::Zero();
+const Vector3d ONES3 = Vector3d::Ones();
+
+const Vector3d BIAS_PBC_SMAL = {0.01, 0.02, 0.03};
+
+const double ACC = 1;
+const double MASS = 1;
+const double DT = 1.0;
+const Vector3d PBCX = {-0.1, 0, 0}; // X axis offset
+const Vector3d PBCY = {0, -0.1, 0}; // Y axis offset
+const Vector3d PBCZ = {0, 0, -0.1}; // Z axis offset
+
+
+class ProcessorForceTorquePreintImuOdom3dIkin2KF : public testing::Test
+{
+public:
+ wolf::TimeStamp t0_;
+ ProblemPtr problem_;
+ SensorImuPtr sen_imu_;
+ SolverCeresPtr solver_;
+ SensorInertialKinematicsPtr sen_ikin_;
+ SensorForceTorquePtr sen_ft_;
+ ProcessorImuPtr proc_imu_;
+ ProcessorInertialKinematicsPtr proc_inkin_;
+ ProcessorForceTorquePreintPtr proc_ftpreint_;
+ FrameBasePtr KF1_;
+
+ void SetUp() override
+ {
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ problem_ = Problem::create("POVCDL", 3);
+
+ // CERES WRAPPER
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR INERTIAL KINEMATICS
+ ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
+ intr_ik->std_pbc = 0.1;
+ intr_ik->std_vbc = 0.1;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
+ // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ sen_ikin_ = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
+
+ ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->std_bp_drift = 1e-2;
+ ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
+ // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml");
+ proc_inkin_ = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
+
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/test/sensor_imu.yaml");
+ sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem_->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
+ proc_imu_ = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
+
+ // SENSOR + PROCESSOR FT PREINT
+ ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
+ intr_ft->std_f = 0.001;
+ intr_ft->std_tau = 0.001;
+ intr_ft->mass = MASS;
+ SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
+ // SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
+ sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
+ ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
+ params_sen_ft->sensor_ikin_name = "SenIK";
+ params_sen_ft->sensor_angvel_name = "SenImu";
+ params_sen_ft->nbc = 2;
+ params_sen_ft->dimc = 6;
+ params_sen_ft->time_tolerance = 0.0005;
+ params_sen_ft->unmeasured_perturbation_std = 1e-4;
+ params_sen_ft->max_time_span = 1000;
+ params_sen_ft->max_buff_length = 500;
+ params_sen_ft->dist_traveled = 20000.0;
+ params_sen_ft->angle_turned = 1000;
+ params_sen_ft->voting_active = true;
+ ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft_, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
+ proc_ftpreint_ = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
+ }
+
+ void TearDown() override {}
+};
+
+
+class Cst2KFZeroMotion : public ProcessorForceTorquePreintImuOdom3dIkin2KF
+{
+public:
+ FrameBasePtr KF2_;
+
+ // Unitary data
+ Vector3d acc_meas_;
+ Vector3d w_meas_;
+ Vector3d pbc_;
+ Vector3d pbc_meas_;
+ Vector3d vBC_meas_;
+ Vector3d f1_;
+ Vector3d tau1_;
+ Vector3d pbl1_;
+ Vector4d bql1_;
+ Vector3d f2_;
+ Vector3d tau2_;
+ Vector3d pbl2_;
+ Vector4d bql2_;
+
+ Matrix6d cov_f_;
+ Matrix6d cov_tau_;
+ Matrix3d cov_pbc_;
+ Matrix3d cov_wb_;
+
+ // Aggregated data to construct Captures
+ Vector6d acc_gyr_meas_;
+ Matrix6d acc_gyr_cov_;
+ Vector9d meas_ikin_;
+ MatrixXd cap_ftp_data_;
+ MatrixXd Qftp_;
+ Matrix3d Iq_;
+ Vector3d Lq_;
+ Vector3d bias_pbc_;
+ Vector7d prev_pose_curr_;
+
+ VectorXd x_origin_;
+
+
+
+ Cst2KFZeroMotion()
+ {
+ bias_pbc_ = ZERO3;
+ }
+
+ virtual void setOriginState()
+ {
+ x_origin_.resize(19);
+ x_origin_ << ZERO3, 0, 0, 0, 1, ZERO3, ZERO3, ZERO3, ZERO3;
+ }
+
+ void SetUp() override
+ {
+ ProcessorForceTorquePreintImuOdom3dIkin2KF::SetUp();
+ t0_.set(0.0);
+ TimeStamp t1; t1.set(1*DT);
+ TimeStamp t2; t2.set(2*DT);
+ TimeStamp t3; t3.set(3*DT);
+ TimeStamp t4; t4.set(3*DT);
+
+ // SETPRIOR RETRO-ENGINEERING
+ // We are not using setPrior because of processors initial captures problems so we have to
+ // - Manually set problem prior
+ // - call setOrigin on processors MotionProvider
+ setOriginState();
+ MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18,18);
+ KF1_ = problem_->emplaceFrame( t0_, x_origin_);
+ // Prior pose factor
+ CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1_, t0_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
+ pose_prior_capture->emplaceFeatureAndFactor();
+ ///////////////////////////////////////////////////
+ // Prior velocity factor
+ CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1_, "Vel0", t0_);
+ FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.segment<3>(7), P_origin_.block<3, 3>(6, 6));
+ FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1_->getV(), nullptr, false);
+
+ initializeData1();
+
+ // Special trick to make things work in the IMU + IKin + FTPreint case
+ // 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0
+ // 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
+ // 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
+ // 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
+ proc_inkin_->keyFrameCallback(KF1_, 0.5);
+ proc_imu_->setOrigin(KF1_);
+ auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0_, sen_ikin_, meas_ikin_, Iq_, Lq_);
+ CIKin0->process();
+ proc_ftpreint_->setOrigin(KF1_);
+
+ // T1
+ CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
+ CImu1->process();
+ auto CIKin1 = std::make_shared<CaptureInertialKinematics>(t1, sen_ikin_, meas_ikin_, Iq_, Lq_);
+ CIKin1->process();
+ auto CFTpreint1 = std::make_shared<CaptureForceTorquePreint>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_);
+ CFTpreint1->process();
+
+
+ changeForData2();
+
+
+ // T2
+ CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(t2, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
+ CImu2->process();
+ auto CIKin2 = std::make_shared<CaptureInertialKinematics>(t2, sen_ikin_, meas_ikin_, Iq_, Lq_);
+ CIKin2->process();
+ auto CFTpreint2 = std::make_shared<CaptureForceTorquePreint>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_);
+ CFTpreint2->process();
+
+ changeForData3();
+
+ // T3
+ CaptureImuPtr CImu3 = std::make_shared<CaptureImu>(t3, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
+ CImu3->process();
+ CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin_, Iq_, Lq_);
+ CIKin3->process();
+ auto CFTpreint3 = std::make_shared<CaptureForceTorquePreint>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
+ CFTpreint3->process();
+
+ // T4, just here to make sure the KF at t3 is created
+ CaptureImuPtr CImu4 = std::make_shared<CaptureImu>(t4, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
+ CImu4->process();
+ CaptureInertialKinematicsPtr CIKin4 = std::make_shared<CaptureInertialKinematics>(t4, sen_ikin_, meas_ikin_, Iq_, Lq_);
+ CIKin4->process();
+ auto CFTpreint4 = std::make_shared<CaptureForceTorquePreint>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
+ CFTpreint4->process();
+
+ /////////////////////////////////////////////
+ // Add an Odom3D to make the biases observable
+ setOdomData();
+ Matrix6d rel_pose_cov = 1e-6 * Matrix6d::Identity();
+
+ KF2_ = problem_->getTrajectory()->getLastFrame();
+ CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF2_, KF2_->getTimeStamp(), nullptr, prev_pose_curr_, rel_pose_cov);
+ FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_curr_, rel_pose_cov);
+ FactorBase::emplace<FactorRelativePose3d>(ftr_odom3d_base, ftr_odom3d_base, KF1_, nullptr, false, TOP_MOTION);
+
+ ////////////////////////////////////////////
+ // Add absolute factor on Imu biases to simulate a fix()
+ Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1_, "bi0", t0_);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
+
+ // Add loose absolute factor on initial bp bias since dynamical trajectories
+ // able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
+ Matrix3d Q_bp = 1e-1 * Matrix3d::Identity();
+ CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_);
+ FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_, Q_bp);
+ // FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ + (Vector3d()<<0.1,0.1,0.1).finished(), Q_bp);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic(),nullptr,false);
+
+ }
+
+ void buildDataVectors(){
+ pbc_meas_ = pbc_ + bias_pbc_;
+
+ acc_gyr_meas_ << acc_meas_, w_meas_;
+ acc_gyr_cov_ = 1e-3 * Matrix6d::Identity();
+ meas_ikin_ << pbc_meas_, vBC_meas_, w_meas_;
+
+ Vector6d f_meas; f_meas << f1_, f2_;
+ Vector6d tau_meas; tau_meas << tau1_, tau2_;
+ Matrix<double, 14, 1> kin_meas;
+ kin_meas << pbl1_, pbl2_, bql1_, bql2_;
+ cap_ftp_data_.resize(32,1);
+ cap_ftp_data_ << f_meas, tau_meas, pbc_meas_, acc_gyr_meas_.tail<3>(), kin_meas;
+
+ Qftp_ = Matrix<double, 18, 18>::Identity();
+ Qftp_.block<6, 6>(0, 0) = cov_f_;
+ Qftp_.block<6, 6>(6, 6) = cov_tau_;
+ Qftp_.block<3, 3>(12, 12) = cov_pbc_;
+ Qftp_.block<3, 3>(15, 15) = cov_wb_;
+ }
+
+ void setZeroMotionData()
+ {
+ // IMU CAPTURE DATA ZERO MOTION
+ acc_meas_ = -gravity();
+ w_meas_ = ZERO3;
+
+ // INERTIAL KINEMATICS CAPTURE DATA ZERO MOTION
+ pbc_ = ZERO3;
+ vBC_meas_ = ZERO3;
+ // momentum parameters
+ Iq_.setIdentity();
+ Lq_.setZero();
+
+ // FORCE TORQUE CAPTURE DATA ZERO MOTION
+ f1_ << -MASS * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ tau1_ << ZERO3;
+ pbl1_ << ZERO3;
+ bql1_ << ZERO3, 1;
+ f2_ << -MASS * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ tau2_ << ZERO3;
+ pbl2_ << ZERO3;
+ bql2_ << ZERO3, 1;
+
+ cov_f_ = 1e-4 * Matrix6d::Identity();
+ cov_tau_ = 1e-4 * Matrix6d::Identity();
+ cov_pbc_ = 1e-4 * Matrix3d::Identity();
+ cov_wb_ = 1e-4 * Matrix3d::Identity();
+ }
+
+ virtual void initializeData1()
+ {
+ setZeroMotionData();
+ buildDataVectors();
+ }
+
+ virtual void changeForData2()
+ {
+ // DO NOTHING IN BASE CLASS
+ }
+
+ virtual void changeForData3()
+ {
+ // DO NOTHING IN BASE CLASS
+ }
+
+ virtual void setOdomData()
+ {
+ prev_pose_curr_ << ZERO6, 1;
+ }
+
+};
+
+class Cst2KFZeroMotionBias : public Cst2KFZeroMotion
+{
+public:
+ void SetUp() override
+ {
+ bias_pbc_ = BIAS_PBC_SMAL;
+ Cst2KFZeroMotion::SetUp();
+ }
+};
+
+class Cst2KFXaxisLinearMotion : public Cst2KFZeroMotion
+{
+public:
+ void initializeData1() override
+ {
+ setZeroMotionData();
+ acc_meas_[0] += ACC;
+ f1_[0] += sen_ft_->getMass() * ACC / 2;
+ f2_[0] += sen_ft_->getMass() * ACC / 2;
+ // taus according to static Euler eq are zero since no lever
+
+ buildDataVectors();
+ }
+
+ void setOdomData() override
+ {
+ prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ }
+};
+
+class Cst2KFXaxisLinearMotionPbc : public Cst2KFZeroMotion
+{
+public:
+
+ void SetUp() override
+ {
+ Cst2KFZeroMotion::SetUp();
+ }
+
+ void setOriginState() override
+ {
+ Cst2KFZeroMotion::setOriginState();
+ x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
+ }
+
+ void initializeData1() override
+ {
+ setZeroMotionData();
+ pbc_ = PBCX + PBCY + PBCZ;
+ acc_meas_[0] += ACC;
+ f1_[0] += sen_ft_->getMass() * ACC / 2;
+ f2_[0] += sen_ft_->getMass() * ACC / 2;
+ // taus according to static Euler eq
+ tau1_ = -(pbl1_- pbc_).cross(f1_); // torque at C due to f1
+ tau2_ = -(pbl2_- pbc_).cross(f2_); // torque at C due to f2
+
+ buildDataVectors();
+ }
+
+ void setOdomData() override
+ {
+ prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ }
+};
+
+class Cst2KFXaxisLinearMotionPbcBias : public Cst2KFZeroMotion
+{
+public:
+ void SetUp() override
+ {
+ bias_pbc_ = BIAS_PBC_SMAL;
+ Cst2KFZeroMotion::SetUp();
+ }
+
+ void setOriginState() override
+ {
+ Cst2KFZeroMotion::setOriginState();
+ x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
+ }
+
+ void initializeData1() override
+ {
+ setZeroMotionData();
+ pbc_ = PBCX + PBCY + PBCZ;
+ acc_meas_[0] += ACC;
+ f1_[0] += sen_ft_->getMass() * ACC / 2;
+ f2_[0] += sen_ft_->getMass() * ACC / 2;
+ // taus according to static Euler eq
+ tau1_ = -(pbl1_- pbc_).cross(f1_); // torque at C due to f1
+ tau2_ = -(pbl2_- pbc_).cross(f2_); // torque at C due to f2
+
+ buildDataVectors();
+ }
+
+ void setOdomData() override
+ {
+ prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ }
+};
+class Cst2KFXaxisLinearMotionPbcBiasPQbl : public Cst2KFZeroMotion
+{
+public:
+ void SetUp() override
+ {
+ bias_pbc_ = BIAS_PBC_SMAL;
+ Cst2KFZeroMotion::SetUp();
+ }
+
+ void setOriginState() override
+ {
+ Cst2KFZeroMotion::setOriginState();
+ x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
+ }
+
+ void initializeData1() override
+ {
+ setZeroMotionData();
+ pbc_ = PBCX + PBCY + PBCZ;
+ acc_meas_[0] += ACC;
+ f1_[0] += sen_ft_->getMass() * ACC / 2;
+ f2_[0] += sen_ft_->getMass() * ACC / 2;
+ pbl1_ << 0.1, 0.1, 0.1;
+ pbl2_ << 0.1, 0.1, 0.1;
+ bql1_ << 1, 0, 0, 0;
+ bql2_ << 1, 0, 0, 0;
+ Quaterniond quat_bl1(bql1_);
+ Quaterniond quat_bl2(bql2_);
+ f1_ = quat_bl1.inverse() * f1_;
+ f2_ = quat_bl2.inverse() * f2_;
+
+ // taus according to static Euler eq
+ tau1_ = -(quat_bl1.inverse()*(pbl1_- pbc_)).cross(f1_); // torque at C due to f1
+ tau2_ = -(quat_bl2.inverse()*(pbl2_- pbc_)).cross(f2_); // torque at C due to f2
+
+ buildDataVectors();
+ }
+
+ void setOdomData() override
+ {
+ prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ }
+};
+
+class Cst2KFXaxisRotationPureTorque : public Cst2KFZeroMotion
+{
+public:
+ void SetUp() override
+ {
+ Cst2KFZeroMotion::SetUp();
+ }
+
+ void initializeData1() override
+ {
+ setZeroMotionData();
+
+ // taus according to static Euler eq
+ tau1_ << M_PI/3, 0, 0; // with Identity rotational inertial matrix, rotation of PI/2 rad.s-2
+ w_meas_ << M_PI/3, 0, 0;
+
+ buildDataVectors();
+ }
+
+ void setOdomData() override
+ {
+ prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ }
+};
+
+//////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////
+///////////////////////////// TESTS //////////////////////////////////
+//////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////
+
+TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
+{
+ // // TEST FIRST PURE INTEGRATION
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // // COM position on Z axis is not observable with this problem
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // // COM position on Z axis is not observable with this problem
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // THEN SOLVE
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
+}
+
+TEST_F(Cst2KFZeroMotionBias, ZeroMotionBias)
+{
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // problem_->print(4,true,true,true);
+ // std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // Bias value on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+}
+
+TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
+{
+
+ Vector3d posi_diff = ZERO3;
+ posi_diff[0] = 0.5*ACC * pow(3*DT,2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3*DT;
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // // COM position on Z axis is not observable with this problem
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
+ // // COM position on Z axis is not observable with this problem
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // problem_->print(4, true, true, true);
+ // std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // Bias value on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
+}
+
+TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
+{
+ Vector3d posi_diff = ZERO3;
+ posi_diff[0] = 0.5*ACC * pow(3*DT,2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3*DT;
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // // COM position on Z axis is not observable with this problem
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
+ // // COM position on Z axis is not observable with this problem
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // Bias value on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
+}
+
+TEST_F(Cst2KFXaxisLinearMotionPbcBias, XLinearMotionBias)
+{
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // problem_->print(4, true, true, true);
+ // std::cout << report << std::endl;
+
+ Vector3d posi_diff = ZERO3;
+ posi_diff[0] = 0.5*ACC * pow(3*DT,2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3*DT;
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // Bias value on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+}
+
+TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
+{
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ // problem_->print(4, true, true, true);
+ // std::cout << report << std::endl;
+
+ Vector3d posi_diff = ZERO3;
+ posi_diff[0] = 0.5*ACC * pow(3*DT,2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3*DT;
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
+ // COM position on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ // Bias value on Z axis is not observable with this problem
+ ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+}
+
+
+// TEST_F(Cst2KFXaxisRotationPureTorque, RotationOnlyNoSolve)
+// {
+
+// Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
+// Vector4d q_diff; q_diff << 1,0,0,0;
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('O')->getState(), q_diff, 1e-6);
+// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), Lc_diff, 1e-6);
+// }
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bee3fb3c6127bdd35969b3d8bc3eb5d2659bd286
--- /dev/null
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -0,0 +1,236 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_inertial_kinematics.cpp
+ *
+ * Created on: March 9, 2020
+ * \author: Mederic Fourmy
+ */
+
+// debug
+#include <iostream>
+#include <fstream>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_block_absolute.h>
+
+// Imu
+#include "imu/internal/config.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/sensor/sensor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+
+#include <Eigen/Eigenvalues>
+
+using namespace wolf;
+using namespace Eigen;
+using namespace std;
+
+const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
+const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
+
+
+class FactorInertialKinematics_2KF : public testing::Test
+{
+ public:
+ wolf::TimeStamp t_;
+ ProblemPtr problem_;
+ SensorImuPtr sen_imu_;
+ SolverCeresPtr solver_;
+ SensorInertialKinematicsPtr sen_ikin_;
+ VectorXd x_origin_;
+ MatrixXd P_origin_;
+ FrameBasePtr KF0_;
+
+ void SetUp() override
+ {
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ problem_ = Problem::create("POVCDL", 3);
+
+ // CERES WRAPPER
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
+
+ //===================================================== INITIALIZATION
+ t_.set(0.0);
+
+ // SENSOR + PROCESSOR INERTIAL KINEMATICS
+ ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
+ intr_ik->std_pbc = 0.1;
+ intr_ik->std_vbc = 0.1;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
+ // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/test/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ sen_ikin_ = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
+
+
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root_dir + "/test/sensor_imu.yaml");
+ sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
+ // auto processor_imu = std::static_pointer_cast<ProcessorImu>(proc);
+
+
+
+ ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->std_bp_drift = 0.0000001;
+ ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
+ // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml");
+ // auto proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
+
+ // Set origin of the problem
+ x_origin_.resize(19);
+ x_origin_ << ZERO3, 0,0,0,1, ZERO3, ZERO3, ZERO3, ZERO3;
+ VectorXd std_vec = pow(1e-3, 2)*VectorXd::Ones(18);
+ VectorComposite prior(x_origin_, "POVCDL", {3,4,3,3,3,3});
+ VectorComposite prior_std(std_vec, "POVCDL", {3,3,3,3,3,3});
+ problem_->setPriorFactor(prior, prior_std, t_);
+ KF0_ = problem_->getTrajectory()->getLastFrame();
+
+ // Set origin of processor Imu
+ std::static_pointer_cast<ProcessorImu>(proc)->setOrigin(KF0_);
+
+ ////////////////////////////////////////////
+ // Add absolute factor on Imu biases to simulate a fix()
+ Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF0_, "bi0", t_);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
+
+ }
+
+ void TearDown() override{}
+};
+
+TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
+{
+ TimeStamp t0; t0.set(0);
+ TimeStamp t1; t1.set(1);
+ TimeStamp t2; t2.set(2);
+ TimeStamp t3; t3.set(3);
+
+ // PROCESS ONE Imu CAPTURE
+ Vector6d acc_gyr_meas;
+ acc_gyr_meas << -gravity(), 0,0,0;
+ acc_gyr_meas[0] = 1;
+ Matrix6d acc_gyr_cov = 1e-3*Matrix6d::Identity();
+
+ // PROCESS ONE INERTIAL KINEMATICS CAPTURE
+ Vector3d pBC_meas = Vector3d::Zero();
+ Vector3d vBC_meas = Vector3d::Zero();
+ Vector3d w_meas = Vector3d::Zero();
+ // momentum parameters
+ Matrix3d Iq; Iq.setIdentity();
+ Vector3d Lq = Vector3d::Zero();
+ Vector9d meas_ikin0; meas_ikin0 << pBC_meas, vBC_meas, w_meas;
+
+ // sen_imu_->getIntrinsic()->fix();
+
+ // T0
+ CaptureImuPtr CImu0 = std::make_shared<CaptureImu>(t0, sen_imu_, acc_gyr_meas, acc_gyr_cov);
+ sen_imu_->process(CImu0);
+ auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin_, meas_ikin0, Iq, Lq);
+ sen_ikin_->process(CIKin0);
+
+ CIKin0->getSensorIntrinsic()->fix();
+
+ // T1
+ CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas, acc_gyr_cov);
+ sen_imu_->process(CImu1);
+ // CImu1->getSensorIntrinsic()->fix();
+
+ auto CIKin1 = std::make_shared<CaptureInertialKinematics>(t1, sen_ikin_, meas_ikin0, Iq, Lq);
+ sen_ikin_->process(CIKin1);
+ CIKin1->getSensorIntrinsic()->fix();
+
+
+ // T2
+ CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(t2, sen_imu_, acc_gyr_meas, acc_gyr_cov);
+ // CImu2->getSensorIntrinsic()->fix();
+ sen_imu_->process(CImu2);
+
+ auto CIKin2 = std::make_shared<CaptureInertialKinematics>(t2, sen_ikin_, meas_ikin0, Iq, Lq);
+ sen_ikin_->process(CIKin2);
+ CIKin2->getSensorIntrinsic()->fix();
+
+ // T3
+ CaptureImuPtr CImu3 = std::make_shared<CaptureImu>(t3, sen_imu_, acc_gyr_meas, acc_gyr_cov);
+ // CImu3->getSensorIntrinsic()->fix();
+ sen_imu_->process(CImu3);
+
+ CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin0, Iq, Lq);
+ sen_ikin_->process(CIKin3);
+ CIKin3->getSensorIntrinsic()->fix();
+
+ KF0_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::cout << report << std::endl;
+ problem_->print(4,true,true,true);
+
+ Vector3d pdiff; pdiff << 4.5, 0, 0;
+ Vector3d vdiff; vdiff << 3, 0, 0;
+
+ FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
+
+
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('P')->getState(), pdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('V')->getState(), vdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('C')->getState(), pdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('D')->getState(), vdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('L')->getState(), ZERO3, 1e-6);
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_point_feet_nomove.cpp b/test/gtest_processor_point_feet_nomove.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a246f3a1292c1b96a74ffeecb7c1ec65bc66471
--- /dev/null
+++ b/test/gtest_processor_point_feet_nomove.cpp
@@ -0,0 +1,199 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have 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_point_feet_nomove.cpp
+ *
+ * Created on: October 28, 2020
+ * \author: Mederic Fourmy
+ */
+
+// debug
+#include <iostream>
+#include <fstream>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/capture/capture_base.h>
+// #include <core/ceres_wrapper/solver_ceres.h>
+// #include <core/math/rotations.h>
+// #include <core/capture/capture_pose.h>
+// #include <core/feature/feature_base.h>
+// #include <core/factor/factor_block_absolute.h>
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/capture/capture_point_feet_nomove.h"
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
+#include "bodydynamics/processor/processor_point_feet_nomove.h"
+
+#include <Eigen/Eigenvalues>
+
+using namespace wolf;
+using namespace Eigen;
+using namespace std;
+
+const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
+// const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
+
+
+class PointFeetCaptures : public testing::Test
+{
+ public:
+ wolf::TimeStamp t_;
+ ProblemPtr problem_;
+ SensorPointFeetNomovePtr sen_pfnm_;
+ ProcessorBasePtr proc_pfnm_base_;
+ VectorXd x_origin_;
+ // MatrixXd P_origin_;
+ FrameBasePtr KF0_;
+
+ CapturePointFeetNomovePtr CPF0_;
+ CapturePointFeetNomovePtr CPF1_;
+ CapturePointFeetNomovePtr CPF2_;
+ CapturePointFeetNomovePtr CPF3_;
+ CapturePointFeetNomovePtr CPF4_;
+ CapturePointFeetNomovePtr CPF5_;
+ CapturePointFeetNomovePtr CPF6_;
+
+ void SetUp() override
+ {
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ problem_ = Problem::create("PO", 3);
+
+ // CERES WRAPPER
+ // solver_ = std::make_shared<SolverCeres>(problem_);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
+
+ //===================================================== INITIALIZATION
+
+ // SENSOR + PROCESSOR POINT FEET NOMOVE
+ ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+ intr_pfn->std_foot_nomove_ = 0.1;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_pfnm_base = problem_->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+ sen_pfnm_ = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+ proc_pfnm_base_ = problem_->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm_, params_pfnm);
+
+ // Set origin of the problem
+ x_origin_.resize(7);
+ x_origin_ << 0,0,0, 0,0,0,1;
+ VectorXd std_vec = pow(1e-3, 2)*VectorXd::Ones(6);
+ VectorComposite prior(x_origin_, "PO", {3,4});
+ VectorComposite prior_std(std_vec, "PO", {3,3});
+ problem_->setPriorFactor(prior, prior_std, 0.0);
+ KF0_ = problem_->getTrajectory()->getLastFrame();
+
+ // Simulate captures with 4 point feet
+ std::unordered_map<int, Vector3d> kin_incontact_A({
+ {1, Vector3d::Zero()},
+ {2, Vector3d::Zero()},
+ {3, Vector3d::Zero()},
+ {4, Vector3d::Zero()}
+ });
+
+ CPF0_ = std::make_shared<CapturePointFeetNomove>(0.0, sen_pfnm_, kin_incontact_A);
+ std::cout << CPF0_->getName() << std::endl;
+ CPF1_ = std::make_shared<CapturePointFeetNomove>(1.0, sen_pfnm_, kin_incontact_A);
+ CPF2_ = std::make_shared<CapturePointFeetNomove>(2.0, sen_pfnm_, kin_incontact_A);
+
+ // contact of the 2nd foot lost
+ std::unordered_map<int, Eigen::Vector3d> kin_incontact_B({
+ {1, Vector3d::Ones()},
+ {3, Vector3d::Ones()},
+ {4, Vector3d::Ones()}
+ });
+ CPF3_ = std::make_shared<CapturePointFeetNomove>(3.0, sen_pfnm_, kin_incontact_B);
+ CPF4_ = std::make_shared<CapturePointFeetNomove>(4.0, sen_pfnm_, kin_incontact_B);
+
+ // contact of the 3rd foot lost
+ std::unordered_map<int, Eigen::Vector3d> kin_incontact_C({
+ {1, 2*Vector3d::Ones()},
+ {4, 2*Vector3d::Ones()}
+ });
+ CPF5_ = std::make_shared<CapturePointFeetNomove>(5.0, sen_pfnm_, kin_incontact_C);
+ CPF6_ = std::make_shared<CapturePointFeetNomove>(6.0, sen_pfnm_, kin_incontact_C);
+
+ }
+
+ void TearDown() override{}
+};
+
+TEST_F(PointFeetCaptures, process_all_capture_first)
+{
+
+ CPF0_->process();
+ CPF1_->process();
+ CPF2_->process();
+ CPF3_->process();
+ CPF4_->process();
+ CPF5_->process();
+ CPF6_->process(); // one last capture to create the factor
+
+ // factor creation due to keyFrameCallback
+ FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_);
+ proc_pfnm_base_->keyFrameCallback(KF1);
+
+ FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_);
+ proc_pfnm_base_->keyFrameCallback(KF2);
+
+ problem_->print(4,1,1,1);
+}
+
+
+TEST_F(PointFeetCaptures, process_all_capture_last)
+{
+ // First, get the key frames from the keyFrameCallback
+ FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_);
+ proc_pfnm_base_->keyFrameCallback(KF1);
+
+ FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_);
+ proc_pfnm_base_->keyFrameCallback(KF2);
+
+ // Then process the captures
+ CPF0_->process();
+ CPF1_->process();
+ CPF2_->process();
+ CPF3_->process();
+ CPF4_->process();
+ CPF5_->process();
+ CPF6_->process(); // one last capture to create the factor
+
+ problem_->print(4,1,1,1);
+}
+
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_sensor_force_torque.cpp b/test/gtest_sensor_force_torque.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..470b78a2f10e92f44e604b5c267de4497e882000
--- /dev/null
+++ b/test/gtest_sensor_force_torque.cpp
@@ -0,0 +1,57 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file gtest_sensor_force_torque.cpp
+ *
+ * Created on: Feb 26, 2020
+ * \author: jsola
+ */
+
+
+#include "bodydynamics/sensor/sensor_force_torque.h"
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+#include <Eigen/Dense>
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(SensorForceTorque, constructor_and_getters)
+{
+ ParamsSensorForceTorquePtr intr = std::make_shared<ParamsSensorForceTorque>();
+ intr->std_f = 1;
+ intr->std_tau = 2;
+ VectorXd extr(0);
+ SensorForceTorque S(extr, intr);
+
+ ASSERT_EQ(S.getForceNoiseStd(), 1);
+ ASSERT_EQ(S.getTorqueNoiseStd(), 2);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_sensor_inertial_kinematics.cpp b/test/gtest_sensor_inertial_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc981d726049df6bb6145fe8d9ae3c6708a4499a
--- /dev/null
+++ b/test/gtest_sensor_inertial_kinematics.cpp
@@ -0,0 +1,57 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file gtest_sensor_inertial_kinematics.cpp
+ *
+ * Created on: Feb 28, 2020
+ * \author: jsola
+ */
+
+
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+#include <Eigen/Dense>
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(SensorInertialKinematics, constructor_and_getters)
+{
+ ParamsSensorInertialKinematicsPtr intr = std::make_shared<ParamsSensorInertialKinematics>();
+ intr->std_pbc = 1;
+ intr->std_vbc = 2;
+ VectorXd extr(0);
+ SensorInertialKinematics S(extr, intr);
+
+ ASSERT_EQ(S.getPbcNoiseStd(), 1);
+ ASSERT_EQ(S.getVbcNoiseStd(), 2);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/demos/processor_imu.yaml b/test/processor_imu.yaml
similarity index 51%
rename from demos/processor_imu.yaml
rename to test/processor_imu.yaml
index 3a7f2adf26c62e430549401406c49dc8a52f55fa..468ed456e8fff451dc495eeb2e48b1eabaf104be 100644
--- a/demos/processor_imu.yaml
+++ b/test/processor_imu.yaml
@@ -1,11 +1,10 @@
-type: "ProcessorIMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00000001
keyframe_vote:
- max_time_span: 1.0 # seconds
+ max_time_span: 2.1 # seconds
max_buff_length: 500 # motion deltas
- dist_traveled: 2.0 # meters
- angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: false
- voting_aux_active: false
\ No newline at end of file
+ dist_traveled: 20000.0 # meters
+ angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg)
+ voting_active: true
\ No newline at end of file
diff --git a/test/sensor_imu.yaml b/test/sensor_imu.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..124d55651a1b410e825aaa87b565719561f8e252
--- /dev/null
+++ b/test/sensor_imu.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main Imu Sensor" # This is ignored. The name provided to the SensorFactory prevails
+motion_variances:
+ a_noise: 0.03 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+ w_noise: 0.002 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ ab_initial_stdev: 0.00 # m/s2 - initial bias
+ wb_initial_stdev: 0.0 # rad/sec - initial bias
+ ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
\ No newline at end of file
diff --git a/test/utils_gtest.h b/test/utils_gtest.h
deleted file mode 100644
index 9cb2166e8429cd236f77c502891c14880e57ec30..0000000000000000000000000000000000000000
--- a/test/utils_gtest.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/**
- * \file utils_gtest.h
- * \brief Some utils for gtest
- * \author Jeremie Deray
- * Created on: 26/09/2016
- * Eigen macros extension by: Joan Sola on 26/04/2017
- */
-
-#ifndef WOLF_UTILS_GTEST_H
-#define WOLF_UTILS_GTEST_H
-
-#include <gtest/gtest.h>
-
-// Macros for testing equalities and inequalities.
-//
-// * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
-// * {ASSERT|EXPECT}_NE(v1, v2): Tests that v1 != v2
-// * {ASSERT|EXPECT}_LT(v1, v2): Tests that v1 < v2
-// * {ASSERT|EXPECT}_LE(v1, v2): Tests that v1 <= v2
-// * {ASSERT|EXPECT}_GT(v1, v2): Tests that v1 > v2
-// * {ASSERT|EXPECT}_GE(v1, v2): Tests that v1 >= v2
-//
-// C String Comparisons. All tests treat NULL and any non-NULL string
-// as different. Two NULLs are equal.
-//
-// * {ASSERT|EXPECT}_STREQ(s1, s2): Tests that s1 == s2
-// * {ASSERT|EXPECT}_STRNE(s1, s2): Tests that s1 != s2
-// * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case
-// * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case
-//
-// Macros for comparing floating-point numbers.
-//
-// * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual):
-// Tests that two float values are almost equal.
-// * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual):
-// Tests that two double values are almost equal.
-// * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error):
-// Tests that v1 and v2 are within the given distance to each other.
-//
-// These predicate format functions work on floating-point values, and
-// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g.
-//
-// EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0);
-//
-// Macros that execute statement and check that it doesn't generate new fatal
-// failures in the current thread.
-//
-// * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
-
-// http://stackoverflow.com/a/29155677
-
-namespace testing
-{
-namespace internal
-{
-enum GTestColor
-{
- COLOR_DEFAULT,
- COLOR_RED,
- COLOR_GREEN,
- COLOR_YELLOW
-};
-
-extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
-
-#define PRINTF(...) \
- do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\
- "[ ] "); \
- testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \
- while(0)
-
-#define PRINT_TEST_FINISHED \
-{ \
- const ::testing::TestInfo* const test_info = \
- ::testing::UnitTest::GetInstance()->current_test_info(); \
- PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\
- append(" of test ").append(test_info->name()).append(".\n").c_str()); \
-}
-
-// C++ stream interface
-class TestCout : public std::stringstream
-{
-public:
- ~TestCout()
- {
- PRINTF("%s\n", str().c_str());
- }
-};
-
-/* Usage :
-
-TEST(Test, Foo)
-{
- // the following works but prints default stream
- EXPECT_TRUE(false) << "Testing Stream.";
-
- // or you can play with AINSI color code
- EXPECT_TRUE(false) << "\033[1;31m" << "Testing Stream.";
-
- // or use the above defined macros
-
- PRINTF("Hello world");
-
- // or
-
- TEST_COUT << "Hello world";
-}
-
-*/
-#define TEST_COUT testing::internal::TestCout()
-
-} // namespace internal
-
-/* Macros related to testing Eigen classes:
- */
-#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- return (lhs - rhs).isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- return (lhs - rhs).isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-
-#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- MatrixXs er = lhs - rhs; \
- er(2) = pi2pi((Scalar)er(2)); \
- return er.isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- MatrixXs er = lhs - rhs; \
- er(2) = pi2pi((Scalar)er(2)); \
- return er.isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-} // namespace testing
-
-#endif /* WOLF_UTILS_GTEST_H */