diff --git a/.gitignore b/.gitignore index 94d4918bdec0d0d02df589c529d938e6c87d72d4..77906100d4ae252720e456622103b13d8c6371e5 100644 --- a/.gitignore +++ b/.gitignore @@ -32,4 +32,7 @@ src/examples/map_apriltag_save.yaml \.vscode/ build_release/ .clangd -wolf.found +wolfcore.found +/wolf.found +.ccls* +compile_commands.json diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index dbf139b13a33b4b14ec3125cca2621f6c9e47381..1adcb74ceb72d69e5108c5fb639a45c7d2a0e1ee 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -67,7 +67,10 @@ wolf_build_and_test: - 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 .. + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON .. - make -j$(nproc) - ctest -j$(nproc) + # run demos + - ../bin/hello_wolf + - ../bin/hello_wolf_autoconf - make install diff --git a/CMakeLists.txt b/CMakeLists.txt index 9112af5f16d8a84404e203361c568563ccb88dee..246dffbc6f04c9054d91d8f09fb0daadf5052d98 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,6 @@ # Pre-requisites about cmake itself +#Start WOLF build +MESSAGE("Starting WOLF CMakeLists ...") CMAKE_MINIMUM_REQUIRED(VERSION 2.6) if(COMMAND cmake_policy) @@ -10,32 +12,16 @@ SET(CMAKE_MACOSX_RPATH 1) # The project name -PROJECT(wolf) - -#string(COMPARE EQUAL "${CMAKE_BINARY_DIR}" "" result) -#IF(result) -# SET(CMAKE_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR}) -#ENDIF() -#message(STATUS "Binary path : " ${CMAKE_BINARY_DIR}) -# - -#SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin) -#SET(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib) -# -# We'll set the install prefix only is it's empty -# which shouldn't be the case ... -#string(COMPARE EQUAL "${CMAKE_INSTALL_PREFIX}" "" result) -#IF(result) - # This path is actually default on linux -# SET(CMAKE_INSTALL_PREFIX /usr/local) -#ENDIF() -#message(STATUS "Installation path : " ${CMAKE_INSTALL_PREFIX}) +PROJECT(core) +set(PLUGIN_NAME "wolf${PROJECT_NAME}") +# Paths SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin) SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) SET(CMAKE_INSTALL_PREFIX /usr/local) SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE) +# Build type IF (NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE "DEBUG") ENDIF (NOT CMAKE_BUILD_TYPE) @@ -64,12 +50,14 @@ if(UNIX) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers") endif(UNIX) + +# Options IF(NOT BUILD_TESTS) OPTION(BUILD_TESTS "Build Unit tests" ON) ENDIF(NOT BUILD_TESTS) IF(NOT BUILD_DEMOS) - OPTION(BUILD_DEMOS "Build Demos" OFF) + OPTION(BUILD_DEMOS "Build Demos" ON) ENDIF(NOT BUILD_DEMOS) IF(NOT BUILD_DOC) @@ -80,10 +68,7 @@ IF(PROFILING) add_definitions(-DPROFILING) message("Compiling with profiling options...") ENDIF(PROFILING) -############# -## Testing ## -############# -# + if(BUILD_TESTS) # Enables testing for this directory and below. # Note that ctest expects to find a test file in the build directory root. @@ -92,18 +77,6 @@ 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() @@ -118,12 +91,19 @@ IF(BUILD_DEMOS OR BUILD_TESTS) ENDIF(BUILD_DEMOS OR BUILD_TESTS) -#find dependencies. +#START_SRC -------------------------------------------------------------------------------------------------------------------------------- +#CMAKE modules + +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules") +MESSAGE(STATUS "Cmake modules at: " ${CMAKE_MODULE_PATH}) + + +#find dependencies. FIND_PACKAGE(Threads REQUIRED) -FIND_PACKAGE(Ceres REQUIRED) #Ceres is not required +FIND_PACKAGE(Ceres REQUIRED) #Ceres is required FIND_PACKAGE(Eigen3 3.3 REQUIRED) if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3) @@ -160,8 +140,10 @@ ELSE (SPDLOG_INCLUDE_DIR) MESSAGE(FATAL_ERROR "Could not find spdlog") ENDIF (SPDLOG_INCLUDE_DIR) -INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS}) -INCLUDE_DIRECTORIES("include") + +# Includes +INCLUDE_DIRECTORIES("include") # In this same project +INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR}) INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) @@ -291,6 +273,7 @@ SET(HDRS_TREE_MANAGER include/core/tree_manager/factory_tree_manager.h include/core/tree_manager/tree_manager_base.h include/core/tree_manager/tree_manager_sliding_window.h + include/core/tree_manager/tree_manager_sliding_window_dual_rate.h ) SET(HDRS_YAML include/core/yaml/parser_yaml.h @@ -380,6 +363,7 @@ SET(SRCS_SOLVER ) SET(SRCS_TREE_MANAGER src/tree_manager/tree_manager_sliding_window.cpp + src/tree_manager/tree_manager_sliding_window_dual_rate.cpp ) SET(SRCS_YAML src/yaml/parser_yaml.cpp @@ -392,16 +376,17 @@ SET(SRCS_YAML IF (Ceres_FOUND) SET(HDRS_WRAPPER #ceres_wrapper/qr_manager.h - include/core/ceres_wrapper/ceres_manager.h include/core/ceres_wrapper/cost_function_wrapper.h include/core/ceres_wrapper/create_numeric_diff_cost_function.h include/core/ceres_wrapper/local_parametrization_wrapper.h + include/core/ceres_wrapper/iteration_update_callback.h + include/core/ceres_wrapper/solver_ceres.h include/core/solver/solver_manager.h include/core/solver_suitesparse/sparse_utils.h ) SET(SRCS_WRAPPER #ceres_wrapper/qr_manager.cpp - src/ceres_wrapper/ceres_manager.cpp + src/ceres_wrapper/solver_ceres.cpp src/ceres_wrapper/local_parametrization_wrapper.cpp src/solver/solver_manager.cpp ) @@ -420,7 +405,7 @@ IF (Suitesparse_FOUND) ENDIF(Suitesparse_FOUND) # create the shared library -ADD_LIBRARY(${PROJECT_NAME} +ADD_LIBRARY(${PLUGIN_NAME} SHARED ${SRCS} ${SRCS_BASE} @@ -449,35 +434,30 @@ ADD_LIBRARY(${PROJECT_NAME} # ==================== if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") message(STATUS "Using C++ compiler clang") - target_compile_options(${PROJECT_NAME} PRIVATE -Winconsistent-missing-override) + 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(${PROJECT_NAME} PRIVATE -Wsuggest-override) + target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override) # using GCC endif() #Link the created libraries #============================================================= -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl) -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY}) +TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT} dl) +TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY}) IF (Ceres_FOUND) - TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES}) + TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES}) ENDIF(Ceres_FOUND) -# IF (GLOG_FOUND) -# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY}) -# ENDIF (GLOG_FOUND) -#check if this is done correctly - IF(BUILD_TESTS) - MESSAGE("Building tests.") + MESSAGE(STATUS "Will build tests.") add_subdirectory(test) ENDIF(BUILD_TESTS) IF(BUILD_DEMOS) #Build demos - MESSAGE("Building demos.") + MESSAGE(STATUS "Will build demos.") ADD_SUBDIRECTORY(demos) ENDIF(BUILD_DEMOS) @@ -485,12 +465,12 @@ ENDIF(BUILD_DEMOS) #install library #============================================================= -INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets +INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets RUNTIME DESTINATION bin LIBRARY DESTINATION lib/iri-algorithms ARCHIVE DESTINATION lib/iri-algorithms) -install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME}) +install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME}) #install headers INSTALL(FILES ${HDRS_CAPTURE} DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture) @@ -535,13 +515,13 @@ INSTALL(FILES ${HDRS_WRAPPER} INSTALL(FILES ${HDRS_YAML} DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml) -FILE(WRITE ${PROJECT_NAME}.found "") -INSTALL(FILES ${PROJECT_NAME}.found +FILE(WRITE ${PLUGIN_NAME}.found "") +INSTALL(FILES ${PLUGIN_NAME}.found DESTINATION include/iri-algorithms/wolf/plugin_core) #install Find*.cmake -configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake" - "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY) +configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" + "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" @ONLY) configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake" "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY) @@ -549,14 +529,15 @@ configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake" INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h" DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal) -INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}") -INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}") +INSTALL(FILES "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}") +INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}") INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/") -export(PACKAGE ${PROJECT_NAME}) +export(PACKAGE ${PLUGIN_NAME}) #-END_SRC -------------------------------------------------------------------------------------------------------------------------------- + FIND_PACKAGE(Doxygen) FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/) @@ -599,17 +580,4 @@ ELSE(UNIX) ) ENDIF(UNIX) -IF (UNIX) - SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}") - SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev") - SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...") - SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}) - SET(CPACK_GENERATOR "DEB") - SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu") - SET(CPACK_SET_DESTDIR "ON") # Necessary because of the absolute install paths - INCLUDE(CPack) -ELSE(UNIX) - ADD_CUSTOM_COMMAND( - COMMENT "packaging only implemented in unix" - TARGET uninstall) -ENDIF(UNIX) + diff --git a/README.md b/README.md index ad5473fec99242beacf3889f0818e3f4cd93caa7..c1e960469ccdcaf2b9918905b7e10536c9e9af59 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ WOLF - Windowed Localization Frames =================================== -For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/). \ No newline at end of file +For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/). \ No newline at end of file diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake index d44ea8903d4c5a72af468d603dc1d8e5a6bbf542..9e969786089ca8ea3801be8b084c51a5782f09b5 100644 --- a/cmake_modules/FindEigen3.cmake +++ b/cmake_modules/FindEigen3.cmake @@ -1,263 +1,97 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2015 Google Inc. All rights reserved. -# http://ceres-solver.org/ +# - Try to find Eigen3 lib # -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. # -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Google Inc. nor the names of its contributors may be -# used to endorse or promote products derived from this software without -# specific prior written permission. +# Once done this will define # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version # -# Author: alexs.mac@gmail.com (Alex Stewart) +# This module reads hints about search locations from +# the following enviroment variables: # +# EIGEN3_ROOT +# EIGEN3_ROOT_DIR + +# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org> +# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr> +# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # search first if an Eigen3Config.cmake is available in the system, + # if successful this would set EIGEN3_INCLUDE_DIR and the rest of + # the script will work as usual + find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) + + if(NOT EIGEN3_INCLUDE_DIR) + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + HINTS + ENV EIGEN3_ROOT + ENV EIGEN3_ROOT_DIR + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + endif(NOT EIGEN3_INCLUDE_DIR) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) -# FindEigen.cmake - Find Eigen library, version >= 3. -# -# This module defines the following variables: -# -# EIGEN_FOUND: TRUE iff Eigen is found. -# EIGEN_INCLUDE_DIRS: Include directories for Eigen. -# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h -# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 -# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 -# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 -# FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION: True iff the version of Eigen -# found was built & installed / -# exported as a CMake package. -# -# The following variables control the behaviour of this module: -# -# EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then -# then prefer using an exported CMake configuration -# generated by Eigen over searching for the -# Eigen components manually. Otherwise (FALSE) -# ignore any exported Eigen CMake configurations and -# always perform a manual search for the components. -# Default: TRUE iff user does not define this variable -# before we are called, and does NOT specify -# EIGEN_INCLUDE_DIR_HINTS, otherwise FALSE. -# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to -# search for eigen includes, e.g: /timbuktu/eigen3. -# -# The following variables are also defined by this module, but in line with -# CMake recommended FindPackage() module style should NOT be referenced directly -# by callers (use the plural variables detailed above instead). These variables -# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which -# are NOT re-called (i.e. search for library is not repeated) if these variables -# are set with valid values _in the CMake cache_. This means that if these -# variables are set directly in the cache, either by the user in the CMake GUI, -# or by the user passing -DVAR=VALUE directives to CMake when called (which -# explicitly defines a cache variable), then they will be used verbatim, -# bypassing the HINTS variables and other hard-coded search locations. -# -# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the -# include directory of any dependencies. - -# Called if we failed to find Eigen or any of it's required dependencies, -# unsets all public (designed to be used externally) variables and reports -# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument. -macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) - unset(EIGEN_FOUND) - unset(EIGEN_INCLUDE_DIRS) - unset(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION) - # Make results of search visible in the CMake GUI if Eigen has not - # been found so that user does not have to toggle to advanced view. - mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) - # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() - # use the camelcase library name, not uppercase. - if (Eigen_FIND_QUIETLY) - message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - elseif (Eigen_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - else() - # Neither QUIETLY nor REQUIRED, use no priority which emits a message - # but continues configuration and allows generation. - message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - endif () - return() -endmacro(EIGEN_REPORT_NOT_FOUND) - -# Protect against any alternative find_package scripts for this library having -# been called previously (in a client project) which set EIGEN_FOUND, but not -# the other variables we require / set here which could cause the search logic -# here to fail. -unset(EIGEN_FOUND) - -# ----------------------------------------------------------------- -# By default, if the user has expressed no preference for using an exported -# Eigen CMake configuration over performing a search for the installed -# components, and has not specified any hints for the search locations, then -# prefer an exported configuration if available. -if (NOT DEFINED EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION - AND NOT EIGEN_INCLUDE_DIR_HINTS) - message(STATUS "No preference for use of exported Eigen CMake configuration " - "set, and no hints for include directory provided. " - "Defaulting to preferring an installed/exported Eigen CMake configuration " - "if available.") - set(EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION TRUE) -endif() - -if (EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION) - # Try to find an exported CMake configuration for Eigen. - # - # We search twice, s/t we can invert the ordering of precedence used by - # find_package() for exported package build directories, and installed - # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) - # respectively in [1]. - # - # By default, exported build directories are (in theory) detected first, and - # this is usually the case on Windows. However, on OS X & Linux, the install - # path (/usr/local) is typically present in the PATH environment variable - # which is checked in item 4) in [1] (i.e. before both of the above, unless - # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed - # packages are usually detected in preference to exported package build - # directories. - # - # To ensure a more consistent response across all OSs, and as users usually - # want to prefer an installed version of a package over a locally built one - # where both exist (esp. as the exported build directory might be removed - # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which - # means any build directories exported by the user are ignored, and thus - # installed directories are preferred. If this fails to find the package - # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any - # exported build directories will now be detected. - # - # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which - # is item 5) in [1]), to not preferentially use projects that were built - # recently with the CMake GUI to ensure that we always prefer an installed - # version if available. - # - # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package - find_package(Eigen3 QUIET - NO_MODULE - NO_CMAKE_PACKAGE_REGISTRY - NO_CMAKE_BUILDS_PATH) - if (EIGEN3_FOUND) - message(STATUS "Found installed version of Eigen: ${Eigen3_DIR}") - else() - # Failed to find an installed version of Eigen, repeat search allowing - # exported build directories. - message(STATUS "Failed to find installed Eigen CMake configuration, " - "searching for Eigen build directories exported with CMake.") - # Again pass NO_CMAKE_BUILDS_PATH, as we know that Eigen is exported and - # do not want to treat projects built with the CMake GUI preferentially. - find_package(Eigen3 QUIET - NO_MODULE - NO_CMAKE_BUILDS_PATH) - if (EIGEN3_FOUND) - message(STATUS "Found exported Eigen build directory: ${Eigen3_DIR}") - endif() - endif() - if (EIGEN3_FOUND) - set(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION TRUE) - set(EIGEN_FOUND ${EIGEN3_FOUND}) - set(EIGEN_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}" CACHE STRING - "Eigen include directory" FORCE) - else() - message(STATUS "Failed to find an installed/exported CMake configuration " - "for Eigen, will perform search for installed Eigen components.") - endif() -endif() - -if (NOT EIGEN_FOUND) - # Search user-installed locations first, so that we prefer user installs - # to system installs where both exist. - list(APPEND EIGEN_CHECK_INCLUDE_DIRS - /usr/local/include - /usr/local/homebrew/include # Mac OS X - /opt/local/var/macports/software # Mac OS X. - /opt/local/include - /usr/include) - # Additional suffixes to try appending to each search path. - list(APPEND EIGEN_CHECK_PATH_SUFFIXES - eigen3 # Default root directory for Eigen. - Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 - Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 - - # Search supplied hint directories first if supplied. - find_path(EIGEN_INCLUDE_DIR - NAMES Eigen/Core - HINTS ${EIGEN_INCLUDE_DIR_HINTS} - PATHS ${EIGEN_CHECK_INCLUDE_DIRS} - PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) - - if (NOT EIGEN_INCLUDE_DIR OR - NOT EXISTS ${EIGEN_INCLUDE_DIR}) - eigen_report_not_found( - "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " - "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") - endif (NOT EIGEN_INCLUDE_DIR OR - NOT EXISTS ${EIGEN_INCLUDE_DIR}) - - # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets - # if called. - set(EIGEN_FOUND TRUE) -endif() - -# Extract Eigen version from Eigen/src/Core/util/Macros.h -if (EIGEN_INCLUDE_DIR) - set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) - if (NOT EXISTS ${EIGEN_VERSION_FILE}) - eigen_report_not_found( - "Could not find file: ${EIGEN_VERSION_FILE} " - "containing version information in Eigen install located at: " - "${EIGEN_INCLUDE_DIR}.") - else (NOT EXISTS ${EIGEN_VERSION_FILE}) - file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) - - string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" - EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" - EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") - - string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" - EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" - EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") - - string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" - EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" - EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") - - # This is on a single line s/t CMake does not interpret it as a list of - # elements and insert ';' separators which would result in 3.;2.;0 nonsense. - set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") - endif (NOT EXISTS ${EIGEN_VERSION_FILE}) -endif (EIGEN_INCLUDE_DIR) - -# Set standard CMake FindPackage variables if found. -if (EIGEN_FOUND) - set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) -endif (EIGEN_FOUND) - -# Handle REQUIRED / QUIET optional arguments and version. -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Eigen - REQUIRED_VARS EIGEN_INCLUDE_DIRS - VERSION_VAR EIGEN_VERSION) - -# Only mark internal variables as advanced if we found Eigen, otherwise -# leave it visible in the standard GUI for the user to set manually. -if (EIGEN_FOUND) - mark_as_advanced(FORCE EIGEN_INCLUDE_DIR - Eigen3_DIR) # Autogenerated by find_package(Eigen3) -endif (EIGEN_FOUND) \ No newline at end of file diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake deleted file mode 100644 index 74ede89df7d20ffdfe00d7cd669bce317fc501a9..0000000000000000000000000000000000000000 --- a/cmake_modules/wolfConfig.cmake +++ /dev/null @@ -1,99 +0,0 @@ -#edit the following line to add the librarie's header files -FIND_PATH( - wolf_INCLUDE_DIRS - NAMES wolf.found - PATHS /usr/local/include/iri-algorithms/wolf/plugin_core) -IF(wolf_INCLUDE_DIRS) - MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}") -ELSE(wolf_INCLUDE_DIRS) - MESSAGE("Couldn't find wolf include dirs") -ENDIF(wolf_INCLUDE_DIRS) - -FIND_LIBRARY( - wolf_LIBRARIES - NAMES libwolf.so libwolf.dylib - PATHS /usr/local/lib/iri-algorithms) -IF(wolf_LIBRARIES) - MESSAGE("Found wolf lib: ${wolf_LIBRARIES}") -ELSE(wolf_LIBRARIES) - MESSAGE("Couldn't find wolf lib") -ENDIF(wolf_LIBRARIES) - -IF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES) - SET(wolf_FOUND TRUE) - ELSE(wolf_INCLUDE_DIRS AND wolf_LIBRARIES) - set(wolf_FOUND FALSE) -ENDIF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES) - -IF (wolf_FOUND) - IF (NOT wolf_FIND_QUIETLY) - MESSAGE(STATUS "Found wolf: ${wolf_LIBRARIES}") - ENDIF (NOT wolf_FIND_QUIETLY) -ELSE (wolf_FOUND) - IF (wolf_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find wolf") - ENDIF (wolf_FIND_REQUIRED) -ENDIF (wolf_FOUND) - - -macro(wolf_report_not_found REASON_MSG) - set(wolf_FOUND FALSE) - unset(wolf_INCLUDE_DIRS) - unset(wolf_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 (wolf_FIND_QUIETLY) - message(STATUS "Failed to find wolf- " ${REASON_MSG} ${ARGN}) - else (wolf_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN}) - else() - # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error - # that prevents generation, but continues configuration. - message(SEND_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN}) - endif () - return() -endmacro(wolf_report_not_found) - -if(NOT wolf_FOUND) - wolf_report_not_found("Something went wrong while setting up wolf.") -endif(NOT wolf_FOUND) -# Set the include directories for wolf (itself). -set(wolf_FOUND TRUE) - -# Now we gather all the required dependencies for Wolf - -get_filename_component(WOLF_CURRENT_CONFIG_DIR - "${CMAKE_CURRENT_LIST_FILE}" PATH) - -SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH}) -LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLF_CURRENT_CONFIG_DIR}) - -FIND_PACKAGE(Threads REQUIRED) -list(APPEND wolf_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) - -FIND_PACKAGE(Ceres REQUIRED) -list(APPEND wolf_INCLUDE_DIRS ${CERES_INCLUDE_DIRS}) -list(APPEND wolf_LIBRARIES ${CERES_LIBRARIES}) - -# YAML with yaml-cpp -FIND_PACKAGE(YamlCpp REQUIRED) -list(APPEND wolf_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS}) -list(APPEND wolf_LIBRARIES ${YAMLCPP_LIBRARY}) - -#Eigen -# FIND_PACKAGE(Eigen3 REQUIRED) -# list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) - -if(NOT Eigen3_FOUND) - FIND_PACKAGE(Eigen3 REQUIRED) -endif() -if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3) - message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3") -endif() -list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) - -SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH}) diff --git a/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake new file mode 100644 index 0000000000000000000000000000000000000000..b4df7e15bab20bc8d1090ffbd5428ac7a3ab5cd6 --- /dev/null +++ b/cmake_modules/wolfcoreConfig.cmake @@ -0,0 +1,99 @@ +#edit the following line to add the librarie's header files +FIND_PATH( + wolfcore_INCLUDE_DIRS + NAMES wolfcore.found + PATHS /usr/local/include/iri-algorithms/wolf/plugin_core) +IF(wolfcore_INCLUDE_DIRS) + MESSAGE("Found wolf core include dirs: ${wolfcore_INCLUDE_DIRS}") +ELSE(wolfcore_INCLUDE_DIRS) + MESSAGE("Couldn't find wolf core include dirs") +ENDIF(wolfcore_INCLUDE_DIRS) + +FIND_LIBRARY( + wolfcore_LIBRARIES + NAMES libwolfcore.so libwolfcore.dylib + PATHS /usr/local/lib/iri-algorithms) +IF(wolfcore_LIBRARIES) + MESSAGE("Found wolf core lib: ${wolfcore_LIBRARIES}") +ELSE(wolfcore_LIBRARIES) + MESSAGE("Couldn't find wolf core lib") +ENDIF(wolfcore_LIBRARIES) + +IF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES) + SET(wolfcore_FOUND TRUE) + ELSE(wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES) + set(wolfcore_FOUND FALSE) +ENDIF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES) + +IF (wolfcore_FOUND) + IF (NOT wolfcore_FIND_QUIETLY) + MESSAGE(STATUS "Found wolf core: ${wolfcore_LIBRARIES}") + ENDIF (NOT wolfcore_FIND_QUIETLY) +ELSE (wolfcore_FOUND) + IF (wolfcore_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find wolf core") + ENDIF (wolfcore_FIND_REQUIRED) +ENDIF (wolfcore_FOUND) + + +macro(wolfcore_report_not_found REASON_MSG) + set(wolfcore_FOUND FALSE) + unset(wolfcore_INCLUDE_DIRS) + unset(wolfcore_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 (wolfcore_FIND_QUIETLY) + message(STATUS "Failed to find wolf core - " ${REASON_MSG} ${ARGN}) + elseif (wolfcore_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error + # that prevents generation, but continues configuration. + message(SEND_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(wolfcore_report_not_found) + +if(NOT wolfcore_FOUND) + wolfcore_report_not_found("Something went wrong while setting up wolf.") +else (NOT wolfcore_FOUND) + # Set the include directories for wolf core (itself). + set(wolfcore_FOUND TRUE) + + # Now we gather all the required dependencies for Wolf + + get_filename_component(WOLFCORE_CURRENT_CONFIG_DIR + "${CMAKE_CURRENT_LIST_FILE}" PATH) + + SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH}) + LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLFCORE_CURRENT_CONFIG_DIR}) + + FIND_PACKAGE(Threads REQUIRED) + list(APPEND wolfcore_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) + + FIND_PACKAGE(Ceres REQUIRED) + list(APPEND wolfcore_INCLUDE_DIRS ${CERES_INCLUDE_DIRS}) + list(APPEND wolfcore_LIBRARIES ${CERES_LIBRARIES}) + + # YAML with yaml-cpp + FIND_PACKAGE(YamlCpp REQUIRED) + list(APPEND wolfcore_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS}) + list(APPEND wolfcore_LIBRARIES ${YAMLCPP_LIBRARY}) + + #Eigen + # FIND_PACKAGE(Eigen3 REQUIRED) + # list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) + + if(NOT Eigen3_FOUND) + FIND_PACKAGE(Eigen3 REQUIRED) + endif() + if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3) + message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3") + endif() + list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +endif(NOT wolfcore_FOUND) +SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH}) diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp index 6325f785bd2cbfab046f31b73e419c01adf3d7ff..0fd8d42cc5280016d7473e23cb2624615221fda7 100644 --- a/demos/demo_analytic_autodiff_factor.cpp +++ b/demos/demo_analytic_autodiff_factor.cpp @@ -10,10 +10,10 @@ #include <cmath> #include <queue> +#include "../include/core/ceres_wrapper/solver_ceres.h" //Wolf includes #include "wolf_manager.h" #include "core/capture/capture_void.h" -#include "core/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -62,12 +62,14 @@ int main(int argc, char** argv) SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2); // Ceres wrapper - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, ceres_options); - CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, ceres_options); + SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options); + SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options); + ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_manager_autodiff.getSolverOptions().max_line_search_step_contraction = 1e-3; + ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4; + ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_manager_analytic.getSolverOptions().max_line_search_step_contraction = 1e-3; + ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4; // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); @@ -272,8 +274,8 @@ int main(int argc, char** argv) printf("\nError opening file\n"); // PRIOR - FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front(); - FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front(); + FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front(); + FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front(); CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01); CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01); first_frame_autodiff->addCapture(initial_covariance_autodiff); diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp index f19095d3400f9c08b9633e6486398a0d059bc707..386d803138f33b89bee401aceda047fe2199f104 100644 --- a/demos/demo_wolf_imported_graph.cpp +++ b/demos/demo_wolf_imported_graph.cpp @@ -10,10 +10,10 @@ #include <cmath> #include <queue> +#include "../include/core/ceres_wrapper/solver_ceres.h" //Wolf includes #include "wolf_manager.h" #include "core/capture/capture_void.h" -#include "core/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -72,8 +72,8 @@ int main(int argc, char** argv) ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; - CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options); - CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun, ceres_options); + SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options); + SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options); // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); @@ -130,8 +130,8 @@ int main(int argc, char** argv) bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store @@ -301,8 +301,8 @@ int main(int argc, char** argv) printf("\nError opening file\n"); // PRIOR - FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front(); - FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front(); + FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front(); + FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front(); CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01); CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01); first_frame_full->addCapture(initial_covariance_full); diff --git a/demos/hello_wolf/CMakeLists.txt b/demos/hello_wolf/CMakeLists.txt index bf223af61a3358cfc458d8360c1b07e7ab1aff01..81291a479cd3e524ba77b3676982ed0e140d965b 100644 --- a/demos/hello_wolf/CMakeLists.txt +++ b/demos/hello_wolf/CMakeLists.txt @@ -22,17 +22,17 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} add_library(hellowolf SHARED ${SRCS_HELLOWOLF}) -TARGET_LINK_LIBRARIES(hellowolf ${PROJECT_NAME}) +TARGET_LINK_LIBRARIES(hellowolf ${PLUGIN_NAME}) ADD_EXECUTABLE(hello_wolf hello_wolf.cpp) -TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME} hellowolf) +TARGET_LINK_LIBRARIES(hello_wolf ${PLUGIN_NAME} hellowolf) ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp) -#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME}) -TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME} hellowolf) +#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME}) +TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME} hellowolf) #add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2d.cpp ../src/processor/processor_odom_2d.cpp) -#TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf) +#TARGET_LINK_LIBRARIES(sensor_odom ${PLUGIN_NAME} hellowolf) # #add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp) -#TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME} hellowolf) +#TARGET_LINK_LIBRARIES(range_bearing ${PLUGIN_NAME} hellowolf) diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index e51f7a33658bf1c3afb9f2d20e8118825886ebf6..2b2d03d43f1a7d315efb0ae07a734ec022f9e4c6 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -14,13 +14,11 @@ // wolf core includes +#include "../../include/core/ceres_wrapper/solver_ceres.h" #include "core/common/wolf.h" #include "core/sensor/sensor_odom_2d.h" #include "core/processor/processor_odom_2d.h" #include "core/capture/capture_odom_2d.h" -#include "core/ceres_wrapper/ceres_manager.h" - -// hello wolf local includes #include "sensor_range_bearing.h" #include "processor_range_bearing.h" #include "capture_range_bearing.h" @@ -106,12 +104,11 @@ int main() // Wolf problem and solver ProblemPtr problem = Problem::create("PO", 2); - ceres::Solver::Options options; - options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations - CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); + SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem); + ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations // sensor odometer 2d - ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>(); + ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>(); SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", Vector3d(0,0,0), intrinsics_odo); // processor odometer 2d @@ -235,13 +232,12 @@ int main() // GET COVARIANCES of all states WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - for (auto& kf : problem->getTrajectory()->getFrameList()) - if (kf->isKeyOrAux()) - { - Eigen::MatrixXd cov; - kf->getCovariance(cov); - WOLF_TRACE("KF", kf->id(), "_cov = \n", cov); - } + for (auto& kf : *problem->getTrajectory()) + { + Eigen::MatrixXd cov; + kf->getCovariance(cov); + WOLF_TRACE("KF", kf->id(), "_cov = \n", cov); + } for (auto& lmk : problem->getMap()->getLandmarkList()) { Eigen::MatrixXd cov; diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index fbe33c3afd885a4ccafdb72a10bad23d19cf507a..edeb6b1f04f1f673ad3af9b94493b4e67e46ab9f 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -7,13 +7,12 @@ // wolf core includes +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/solver/factory_solver.h" #include "core/common/wolf.h" #include "core/capture/capture_odom_2d.h" #include "core/processor/processor_motion.h" #include "core/yaml/parser_yaml.h" -#include "core/ceres_wrapper/ceres_manager.h" - -// hello wolf local includes #include "capture_range_bearing.h" @@ -112,9 +111,10 @@ int main() problem->print(4,0,1,0); // Solver. Configure a Ceres solver - ceres::Solver::Options options; - options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations - CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); + // ceres::Solver::Options options; + // options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations + // SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options); + SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server); // recover sensor pointers and other stuff for later use (access by sensor name) SensorBasePtr sensor_odo = problem->getSensor("sen odom"); @@ -218,13 +218,12 @@ int main() // GET COVARIANCES of all states WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - for (auto& kf : problem->getTrajectory()->getFrameList()) - if (kf->isKeyOrAux()) - { - Eigen::MatrixXd cov; - kf->getCovariance(cov); - WOLF_TRACE("KF", kf->id(), "_cov = \n", cov); - } + for (auto& kf : *problem->getTrajectory()) + { + Eigen::MatrixXd cov; + kf->getCovariance(cov); + WOLF_TRACE("KF", kf->id(), "_cov = \n", cov); + } for (auto& lmk : problem->getMap()->getLandmarkList()) { Eigen::MatrixXd cov; diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp index 08e77f76c8d7d2ec70c9b1ac6689e724a67e5c7c..7df38e73367b7b45aff81cdbf5f00099d64aa990 100644 --- a/demos/hello_wolf/processor_range_bearing.cpp +++ b/demos/hello_wolf/processor_range_bearing.cpp @@ -46,7 +46,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) if (!kf) { // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below) - kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp()); + kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp()); assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!"); } diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h index bdcefb3a7141627523a6f2111307fc776abbadbb..f086da9071e4956e3c7ef1ab2d59bff81326f594 100644 --- a/demos/hello_wolf/processor_range_bearing.h +++ b/demos/hello_wolf/processor_range_bearing.h @@ -33,7 +33,7 @@ struct ParamsProcessorRangeBearing : public ParamsProcessorBase } std::string print() const { - return "\n" + ParamsProcessorBase::print(); + return ParamsProcessorBase::print(); } }; diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h index 59c217fe0f3f9f5f70973dca8a2ef4b060a6696a..70684f77951f2681d8c2741d3c5c5b09fc503a12 100644 --- a/demos/hello_wolf/sensor_range_bearing.h +++ b/demos/hello_wolf/sensor_range_bearing.h @@ -31,7 +31,7 @@ struct ParamsSensorRangeBearing : public ParamsSensorBase } std::string print() const { - return "" + ParamsSensorBase::print() + "\n" + return ParamsSensorBase::print() + "\n" + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n" + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n"; } diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml index f2e4f8db31f97ef8059b4d59483a747456e6d19c..99f31be90ac5f4302127c0213c768e41ac9e9b80 100644 --- a/demos/hello_wolf/yaml/hello_wolf_config.yaml +++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml @@ -2,24 +2,31 @@ config: problem: - tree_manager: - type: "TreeManagerSlidingWindow" - n_key_frames: -1 - fix_first_key_frame: false - viral_remove_empty_parent: true - frame_structure: "PO" # keyframes have position and orientation dimension: 2 # space is 2d + frame_structure: "PO" # keyframes have position and orientation + prior: mode: "factor" - # state: [0,0,0] $state: P: [0,0] O: [0] - # cov: [[3,3],.01,0,0,0,.01,0,0,0,.01] $sigma: P: [0.31, 0.31] O: [0.31] time_tolerance: 0.1 + + tree_manager: + type: "TreeManagerSlidingWindow" + n_frames: -1 + fix_first_frame: false + viral_remove_empty_parent: true + + solver: + max_num_iterations: 100 + verbose: 0 + period: 0.2 + update_immediately: false + n_threads: 1 sensors: diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml index 7e345ef211f52897b17267065e28feec0bf4b143..328e7783982f12fe4991fbee9270af3878d1ff30 100644 --- a/demos/hello_wolf/yaml/processor_odom_2d.yaml +++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml @@ -1,4 +1,4 @@ -type: "ProcessorOdom2d" +#type: "ProcessorOdom2d" time_tolerance: 0.1 unmeasured_perturbation_std: 0.001 diff --git a/demos/hello_wolf/yaml/processor_range_bearing.yaml b/demos/hello_wolf/yaml/processor_range_bearing.yaml index 3c257678c9a73eeb3b415e976b55eeddb3a79edd..49a55d90dbb31b7fb18023eba9e71f7c1c03f633 100644 --- a/demos/hello_wolf/yaml/processor_range_bearing.yaml +++ b/demos/hello_wolf/yaml/processor_range_bearing.yaml @@ -1,4 +1,4 @@ -type: "ProcessorRangeBearing" +#type: "ProcessorRangeBearing" time_tolerance: 0.1 diff --git a/demos/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml index 65ac28219471ad65180538e526521d7866391a01..0f724c780c41c708b0b5a63a0e429cbd3319cb1e 100644 --- a/demos/hello_wolf/yaml/sensor_odom_2d.yaml +++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml @@ -1,4 +1,4 @@ -type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error. +#type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error. k_disp_to_disp: 0.1 # m^2 / m k_rot_to_rot: 0.1 # rad^2 / rad diff --git a/demos/hello_wolf/yaml/sensor_range_bearing.yaml b/demos/hello_wolf/yaml/sensor_range_bearing.yaml index d1625337dfc0b0d323d662a727c1ac298117a455..7f81091db16c74fb479684a171256a91dd425f66 100644 --- a/demos/hello_wolf/yaml/sensor_range_bearing.yaml +++ b/demos/hello_wolf/yaml/sensor_range_bearing.yaml @@ -1,4 +1,4 @@ -type: "SensorRangeBearing" +#type: "SensorRangeBearing" noise_range_metres_std: 0.1 noise_bearing_degrees_std: 0.5 diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp index 8084a74093ba94602ce19dd413deb12c24cfa047..82aab6219874c3575dc14fdb24af6d2cc0e2d07a 100644 --- a/demos/solver/test_iQR_wolf2.cpp +++ b/demos/solver/test_iQR_wolf2.cpp @@ -35,9 +35,8 @@ //Ceres includes #include "glog/logging.h" -#include "core/ceres_wrapper/ceres_manager.h" -//laser_scan_utils +#include "../../include/core/ceres_wrapper/solver_ceres.h" #include "iri-algorithms/laser_scan_utils/corner_detector.h" #include "iri-algorithms/laser_scan_utils/entities.h" @@ -181,7 +180,7 @@ int main(int argc, char *argv[]) // ceres_options.minimizer_progress_to_stdout = false; // ceres_options.line_search_direction_type = ceres::LBFGS; // ceres_options.max_num_iterations = 100; - CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblem(), ceres_options); + SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options); std::ofstream log_file, landmark_file; //output file // Own solver @@ -364,8 +363,8 @@ int main(int argc, char *argv[]) // Vehicle poses std::cout << "Vehicle poses..." << std::endl; int i = 0; - Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3); - for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++) + Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().size() * 3); + for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().end(); frame_it++) { if (complex_angle) state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1)); diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index c10374ae82ae1a8f495e6adc3bea5b7f167b548b..18a09ac3e24f36c3cef6b11c87dfc861e17d14be 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -82,9 +82,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s // State blocks - const StateStructure& getStructure() const; - StateBlockPtr getStateBlock(const std::string& _key) const; - StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); } + StateBlockPtr getStateBlock(const char& _key) const; StateBlockPtr getSensorP() const; StateBlockPtr getSensorO() const; StateBlockPtr getSensorIntrinsic() const; @@ -138,17 +136,17 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al inline StateBlockPtr CaptureBase::getSensorP() const { - return getStateBlock("P"); + return getStateBlock('P'); } inline StateBlockPtr CaptureBase::getSensorO() const { - return getStateBlock("O"); + return getStateBlock('O'); } inline StateBlockPtr CaptureBase::getSensorIntrinsic() const { - return getStateBlock("I"); + return getStateBlock('I'); } inline unsigned int CaptureBase::id() const @@ -158,7 +156,8 @@ inline unsigned int CaptureBase::id() const inline FrameBasePtr CaptureBase::getFrame() const { - return frame_ptr_.lock(); + if(frame_ptr_.expired()) return nullptr; + else return frame_ptr_.lock(); } inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr) diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h deleted file mode 100644 index dd88e5db6bf3b7ac38a925a64ba66c089dbb09ec..0000000000000000000000000000000000000000 --- a/include/core/ceres_wrapper/ceres_manager.h +++ /dev/null @@ -1,139 +0,0 @@ -#ifndef CERES_MANAGER_H_ -#define CERES_MANAGER_H_ - -//Ceres includes -#include "ceres/jet.h" -#include "ceres/ceres.h" -#include "glog/logging.h" - -//wolf includes -#include "core/solver/solver_manager.h" -#include "core/utils/params_server.h" -#include "core/ceres_wrapper/cost_function_wrapper.h" -#include "core/ceres_wrapper/local_parametrization_wrapper.h" -#include "core/ceres_wrapper/create_numeric_diff_cost_function.h" - -namespace ceres { -typedef std::shared_ptr<CostFunction> CostFunctionPtr; -} - -namespace wolf { - -WOLF_PTR_TYPEDEFS(CeresManager); - -struct CeresParams : public SolverParams -{ - CeresParams() = default; - - CeresParams(std::string _unique_name, CeresParams& params) - { - // - } - - ~CeresParams() override = default; -}; -/** \brief Ceres manager for WOLF - * - */ - -class CeresManager : public SolverManager -{ - protected: - - std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_; - std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_; - - std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_; - - ceres::Solver::Options ceres_options_; - ceres::Solver::Summary summary_; - std::unique_ptr<ceres::Problem> ceres_problem_; - std::unique_ptr<ceres::Covariance> covariance_; - - public: - - CeresManager(const ProblemPtr& _wolf_problem, - const ceres::Solver::Options& _ceres_options - = ceres::Solver::Options()); - - ~CeresManager() override; - - static SolverManagerPtr create(const ProblemPtr& _wolf_problem, const ParamsServer& _server); - - ceres::Solver::Summary getSummary(); - - std::unique_ptr<ceres::Problem>& getCeresProblem(); - - void computeCovariances(CovarianceBlocksToBeComputed _blocks - = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; - - void computeCovariances(const std::vector<StateBlockPtr>& st_list) override; - - bool hasConverged() override; - - SizeStd iterations() override; - - double initialCost() override; - - double finalCost() override; - - ceres::Solver::Options& getSolverOptions(); - - - const Eigen::SparseMatrixd computeHessian() const; - - protected: - - bool checkDerived(std::string prefix="") const override; - - std::string solveDerived(const ReportVerbosity report_level) override; - - void addFactorDerived(const FactorBasePtr& fac_ptr) override; - - void removeFactorDerived(const FactorBasePtr& fac_ptr) override; - - void addStateBlockDerived(const StateBlockPtr& state_ptr) override; - - void removeStateBlockDerived(const StateBlockPtr& state_ptr) override; - - void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override; - - void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override; - - ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr); - - bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override; - - bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override; -}; - -inline ceres::Solver::Summary CeresManager::getSummary() -{ - return summary_; -} - -inline std::unique_ptr<ceres::Problem>& CeresManager::getCeresProblem() -{ - return ceres_problem_; -} - -inline ceres::Solver::Options& CeresManager::getSolverOptions() -{ - return ceres_options_; -} - -inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const -{ - return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() - && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end(); -} - -inline bool CeresManager::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) -{ - return state_blocks_local_param_.find(state_ptr) != state_blocks_local_param_.end() - && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)); -} - -} // namespace wolf - -#endif diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h new file mode 100644 index 0000000000000000000000000000000000000000..1717be546ab820d24e13985227dd19753530440e --- /dev/null +++ b/include/core/ceres_wrapper/iteration_update_callback.h @@ -0,0 +1,60 @@ +/* + * iteration_callback.h + * + * Created on: Jun 15, 2020 + * Author: joanvallve + */ + +#ifndef INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_ +#define INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_ + +#include "core/problem/problem.h" +#include "ceres/ceres.h" + +namespace wolf { + +class IterationUpdateCallback : public ceres::IterationCallback +{ + public: + explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false) + : problem_(_problem) + , min_num_iterations_(min_num_iterations) + , initial_cost_(0) + , verbose_(verbose) + {} + + ~IterationUpdateCallback() {} + + ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override + { + if (summary.iteration == 0) + initial_cost_ = summary.cost; + + else if (summary.iteration >= min_num_iterations_ and + (problem_->getStateBlockNotificationMapSize() != 0 or + problem_->getFactorNotificationMapSize() != 0)) + { + if (summary.cost >= initial_cost_) + { + WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", summary.iteration); + return ceres::SOLVER_ABORT; + } + else + { + WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", summary.iteration); + return ceres::SOLVER_TERMINATE_SUCCESSFULLY; + } + } + return ceres::SOLVER_CONTINUE; + } + + private: + ProblemPtr problem_; + int min_num_iterations_; + double initial_cost_; + bool verbose_; +}; + +} + +#endif /* INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_ */ diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h new file mode 100644 index 0000000000000000000000000000000000000000..0673207bc47e1873084e3f4d711628a67b322f3c --- /dev/null +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -0,0 +1,214 @@ +#ifndef CERES_MANAGER_H_ +#define CERES_MANAGER_H_ + +//Ceres includes +#include "ceres/jet.h" +#include "ceres/ceres.h" +#include "glog/logging.h" + +//wolf includes +#include "core/solver/solver_manager.h" +#include "core/utils/params_server.h" + +namespace ceres { +typedef std::shared_ptr<CostFunction> CostFunctionPtr; +} + +namespace wolf { + +WOLF_PTR_TYPEDEFS(SolverCeres); +WOLF_PTR_TYPEDEFS(LocalParametrizationWrapper); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres); + +struct ParamsCeres : public ParamsSolver +{ + bool update_immediately = false; + int min_num_iterations = 1; + ceres::Solver::Options solver_options; + ceres::Problem::Options problem_options; + ceres::Covariance::Options covariance_options; + + ParamsCeres() : + ParamsSolver() + { + loadHardcodedValues(); + } + + ParamsCeres(std::string _unique_name, const ParamsServer& _server) : + ParamsSolver(_unique_name, _server) + { + loadHardcodedValues(); + + // stop solver whenever the problem is updated (via ceres::iterationCallback) + update_immediately = _server.getParam<bool>(prefix + "update_immediately"); + if (update_immediately) + min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations"); + + // ceres solver options + solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations"); + solver_options.num_threads = _server.getParam<int>(prefix + "n_threads"); + covariance_options.num_threads = _server.getParam<int>(prefix + "n_threads"); + } + + void loadHardcodedValues() + { + solver_options = ceres::Solver::Options(); + solver_options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH; + solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; //ceres::DOGLEG; + problem_options = ceres::Problem::Options(); + covariance_options = ceres::Covariance::Options(); + problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP; + problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + + #if CERES_VERSION_MINOR >= 13 + covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD; + covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; + #elif CERES_VERSION_MINOR >= 10 + covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD; + #else + covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD; + #endif + covariance_options.apply_loss_function = false; + } + + ~ParamsCeres() override = default; +}; + +class SolverCeres : public SolverManager +{ + protected: + + std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_; + std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_; + + // this map is only for handling automatic destruction of localParametrizationWrapper objects + std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_; + + ceres::Solver::Summary summary_; + std::unique_ptr<ceres::Problem> ceres_problem_; + std::unique_ptr<ceres::Covariance> covariance_; + + ParamsCeresPtr params_ceres_; + + public: + + SolverCeres(const ProblemPtr& _wolf_problem); + + SolverCeres(const ProblemPtr& _wolf_problem, + const ParamsCeresPtr& _params); + + WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres); + + ~SolverCeres() override; + + ceres::Solver::Summary getSummary(); + + std::unique_ptr<ceres::Problem>& getCeresProblem(); + + bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks + = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; + + bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override; + + bool hasConverged() override; + + SizeStd iterations() override; + + double initialCost() override; + + double finalCost() override; + + ceres::Solver::Options& getSolverOptions(); + + const Eigen::SparseMatrixd computeHessian() const; + + virtual int numStateBlocksDerived() const override; + + virtual int numFactorsDerived() const override; + + protected: + + bool checkDerived(std::string prefix="") const override; + + std::string solveDerived(const ReportVerbosity report_level) override; + + void addFactorDerived(const FactorBasePtr& fac_ptr) override; + + void removeFactorDerived(const FactorBasePtr& fac_ptr) override; + + void addStateBlockDerived(const StateBlockPtr& state_ptr) override; + + void removeStateBlockDerived(const StateBlockPtr& state_ptr) override; + + void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override; + + void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override; + + ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr); + + bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override; + + bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override; + + bool isStateBlockFixedDerived(const StateBlockPtr& st) override; + + bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) override; + + bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override; +}; + +inline ceres::Solver::Summary SolverCeres::getSummary() +{ + return summary_; +} + +inline std::unique_ptr<ceres::Problem>& SolverCeres::getCeresProblem() +{ + return ceres_problem_; +} + +inline ceres::Solver::Options& SolverCeres::getSolverOptions() +{ + return params_ceres_->solver_options; +} + +inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const +{ + return fac_2_residual_idx_.count(fac_ptr) == 1 and + fac_2_costfunction_.count(fac_ptr) == 1; +} + +inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const +{ + if (state_blocks_.count(state_ptr) == 0) + return false; + return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)); +} + +inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st) +{ + if (state_blocks_.count(st) == 0) + return false; + return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st)); +}; + +inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const +{ + return state_blocks_local_param_.count(st) == 1; +}; + +inline int SolverCeres::numStateBlocksDerived() const +{ + return ceres_problem_->NumParameterBlocks(); +} + +inline int SolverCeres::numFactorsDerived() const +{ + return ceres_problem_->NumResidualBlocks(); +}; + +} // namespace wolf + +#endif diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h index 80f6516415e28ddce47ab272415604014ffb0a66..b9a4886c290fb9c1dcb4889d55b6d1523c42df78 100644 --- a/include/core/common/params_base.h +++ b/include/core/common/params_base.h @@ -13,10 +13,7 @@ namespace wolf { } virtual ~ParamsBase() = default; - std::string print() const - { - return ""; - } + virtual std::string print() const = 0; }; } #endif diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h index 229e10f95dd8f7506846fe75536a85c6604ae521..97f9a50ff1eb1b0a345461af2fbdce2b81b32491 100644 --- a/include/core/common/wolf.h +++ b/include/core/common/wolf.h @@ -23,6 +23,7 @@ #include <list> #include <map> #include <memory> // shared_ptr and weak_ptr +#include <set> // System specifics #include <sys/stat.h> @@ -230,10 +231,14 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorBase); // Trajectory WOLF_PTR_TYPEDEFS(TrajectoryBase); + // - Frame WOLF_PTR_TYPEDEFS(FrameBase); WOLF_LIST_TYPEDEFS(FrameBase); +class TimeStamp; +typedef std::map<TimeStamp, FrameBasePtr> FrameMap; + // - Capture WOLF_PTR_TYPEDEFS(CaptureBase); WOLF_LIST_TYPEDEFS(CaptureBase); diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index 1d4f01a35e5a1541ec6b147ab3155d268a7f9a9c..c600f8a2433a024f63d2af21fe39105c761e7300 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -4,6 +4,8 @@ // Forward declarations for node templates namespace wolf{ class FeatureBase; +class TrajectoryIter; +class TrajectoryRevIter; } //Wolf includes diff --git a/include/core/factor/factor_odom_2d_analytic.h b/include/core/factor/factor_odom_2d_analytic.h index dc1437e4fe441c8c9a6e9406097ac757f56d8c03..c98a521dfdc638d250de0406c616b51469df6390 100644 --- a/include/core/factor/factor_odom_2d_analytic.h +++ b/include/core/factor/factor_odom_2d_analytic.h @@ -35,15 +35,6 @@ class FactorOdom2dAnalytic : public FactorRelative2dAnalytic return std::string("MOTION"); } - public: -// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, -// const NodeBasePtr& _correspondant_ptr, -// const ProcessorBasePtr& _processor_ptr = nullptr) -// { -// return std::make_shared<FactorOdom2dAnalytic>(_feature_ptr, -// std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr); -// } - }; } // namespace wolf diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 91a94d274b36d0023a0ba267583f4b67726d8665..c4e423947d5ce6c1c00d2f70f6c510dea38e1608 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -1,4 +1,5 @@ + #ifndef FRAME_BASE_H_ #define FRAME_BASE_H_ @@ -7,15 +8,6 @@ namespace wolf{ class TrajectoryBase; class CaptureBase; class StateBlock; - -/** \brief Enumeration of frame types - */ -typedef enum -{ - KEY = 2, ///< key frame. It plays at optimizations (estimated). - AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated). - NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization. -} FrameType; } //Wolf includes @@ -44,7 +36,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha protected: unsigned int frame_id_; - FrameType type_; ///< type of frame. Either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h) TimeStamp time_stamp_; ///< frame time stamp public: @@ -57,39 +48,34 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha * \param _o_ptr StateBlock pointer to the orientation (default: nullptr) * \param _v_ptr StateBlock pointer to the velocity (default: nullptr). **/ - FrameBase(const FrameType & _tp, - const TimeStamp& _ts, + FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr); - FrameBase(const FrameType & _tp, - const TimeStamp& _ts, + FrameBase(const TimeStamp& _ts, const std::string _frame_structure, const VectorComposite& _state); - FrameBase(const FrameType & _tp, - const TimeStamp& _ts, + FrameBase(const TimeStamp& _ts, const std::string _frame_structure, const std::list<VectorXd>& _vectors); ~FrameBase() override; + + // Add and remove from WOLF tree --------------------------------- + template<typename classType, typename... T> + static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all); + + void link(TrajectoryBasePtr); + void link(ProblemPtr _prb); + virtual void remove(bool viral_remove_empty_parent=false); // Frame properties ----------------------------------------------- public: unsigned int id() const; - // get type - bool isKey() const; - bool isAux() const; - bool isKeyOrAux() const; - - // set type - void setNonEstimated(); - void setKey(); - void setAux(); - // Frame values ------------------------------------------------ public: void setTimeStamp(const TimeStamp& _ts); @@ -99,7 +85,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha // State blocks ---------------------------------------------------- public: using HasStateBlocks::addStateBlock; - StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb); + StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb); StateBlockPtr getV() const; protected: @@ -124,15 +110,14 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const; FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const; - void getFactorList(FactorBasePtrList& _fac_list) const; + unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; bool isConstrainedBy(const FactorBasePtr& _factor) const; - void link(TrajectoryBasePtr); - template<typename classType, typename... T> - static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all); + + // Debug and info ------------------------------------------------------- virtual void printHeader(int depth, // bool constr_by, // bool metric, // @@ -184,20 +169,11 @@ inline unsigned int FrameBase::id() const return frame_id_; } -inline bool FrameBase::isKey() const -{ - return (type_ == KEY); -} - -inline bool FrameBase::isAux() const -{ - return (type_ == AUXILIARY); -} - -inline bool FrameBase::isKeyOrAux() const -{ - return (type_ == KEY || type_ == AUXILIARY); -} +//inline bool FrameBase::isKey() const +//{ +// return true; +//// return (type_ == KEY); +//} inline void FrameBase::getTimeStamp(TimeStamp& _ts) const { @@ -211,7 +187,7 @@ inline TimeStamp FrameBase::getTimeStamp() const inline StateBlockPtr FrameBase::getV() const { - return getStateBlock("V"); + return getStateBlock('V'); } inline TrajectoryBasePtr FrameBase::getTrajectory() const @@ -234,13 +210,10 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const return constrained_by_list_; } -inline StateBlockPtr FrameBase::addStateBlock(const std::string& _sb_type, +inline StateBlockPtr FrameBase::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb) { - if (isKeyOrAux()) - HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem()); - else - HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr); + HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem()); return _sb; } diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h index 4ac32d2ded2923f212937cfa88a0994fb0dca5b4..ffd8ac8725e49ea921b32140042eb0936cf5d467 100644 --- a/include/core/math/SE2.h +++ b/include/core/math/SE2.h @@ -36,6 +36,12 @@ inline Eigen::Matrix<T, 2, 2> exp(const T theta) namespace SE2{ +template<class T> +inline Eigen::Matrix<T, 2, 2> skew(const T& t) +{ + return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished(); +} + /** \brief Compute [1]_x * t = (-t.y ; t.x) */ template<class D> @@ -69,8 +75,8 @@ inline Eigen::Matrix2d V_helper(const T theta) inline VectorComposite identity() { VectorComposite v; - v["P"] = Vector2d::Zero(); - v["O"] = Vector1d::Zero(); + v['P'] = Vector2d::Zero(); + v['O'] = Vector1d::Zero(); return v; } @@ -138,12 +144,12 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D inline void exp(const VectorComposite& _tau, VectorComposite& _delta) { // [1] eq. 156 - const auto &p = _tau.at("P"); - const auto &theta = pi2pi(_tau.at("O")(0)); + const auto &p = _tau.at('P'); + const auto &theta = pi2pi(_tau.at('O')(0)); Matrix2d V = V_helper(theta); - _delta["P"] = V * p; - _delta["O"] = Matrix1d(theta); + _delta['P'] = V * p; + _delta['O'] = Matrix1d(theta); } inline VectorComposite exp(const VectorComposite& tau) @@ -158,22 +164,22 @@ inline VectorComposite exp(const VectorComposite& tau) inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau) { // [1] eq. 156 - const auto &p = _tau.at("P"); - const auto &theta = pi2pi(_tau.at("O")(0)); + const auto &p = _tau.at('P'); + const auto &theta = pi2pi(_tau.at('O')(0)); Matrix2d V = V_helper(theta); - _delta["P"] = V * p; - _delta["O"] = Matrix1d(theta); + _delta['P'] = V * p; + _delta['O'] = Matrix1d(theta); // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks: // J_Vp_p = V: see V_helper, eq. 158 // J_Vp_theta: see fcn helper // J_theta_theta = 1; eq. 126 _J_delta_tau.clear(); - _J_delta_tau.emplace("P", "P", V); - _J_delta_tau.emplace("P", "O", J_Vp_theta(p, theta)); - _J_delta_tau.emplace("O", "P", RowVector2d(0.0, 0.0)); - _J_delta_tau.emplace("O", "O", Matrix1d(1)); + _J_delta_tau.emplace('P', 'P', V); + _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta)); + _J_delta_tau.emplace('O', 'P', RowVector2d(0.0, 0.0)); + _J_delta_tau.emplace('O', 'O', Matrix1d(1)); } template<class D1, class D2, class D3> @@ -247,18 +253,18 @@ inline void compose(const VectorComposite& _x1, const VectorComposite& _x2, VectorComposite& _c) { - const auto& p1 = _x1.at("P"); - const auto& a1 = _x1.at("O")(0); // angle + const auto& p1 = _x1.at('P'); + const auto& a1 = _x1.at('O')(0); // angle const auto& R1 = SO2::exp(a1); - const auto& p2 = _x2.at("P"); - const auto& a2 = _x2.at("O")(0); // angle + const auto& p2 = _x2.at('P'); + const auto& a2 = _x2.at('O')(0); // angle // tc = t1 + R1 t2 - _c["P"] = p1 + R1 * p2; + _c['P'] = p1 + R1 * p2; // Rc = R1 R2 --> theta = theta1 + theta2 - _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ; + _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ; } inline void compose(const VectorComposite& _x1, @@ -267,12 +273,12 @@ inline void compose(const VectorComposite& _x1, MatrixComposite& _J_c_x1, MatrixComposite& _J_c_x2) { - const auto& p1 = _x1.at("P"); - const auto& a1 = _x1.at("O")(0); // angle + const auto& p1 = _x1.at('P'); + const auto& a1 = _x1.at('O')(0); // angle Matrix2d R1 = SO2::exp(a1); // need temporary - const auto& p2 = _x2.at("P"); - const auto& a2 = _x2.at("O")(0); // angle + const auto& p2 = _x2.at('P'); + const auto& a2 = _x2.at('O')(0); // angle /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130) * @@ -302,25 +308,25 @@ inline void compose(const VectorComposite& _x1, */ _J_c_x1.clear(); - _J_c_x1.emplace("P", "P", Matrix2d::Identity()); - _J_c_x1.emplace("P", "O", MatrixXd(R1 * skewed(p2)) ); - _J_c_x1.emplace("O", "P", RowVector2d(0,0)); - _J_c_x1.emplace("O", "O", Matrix1d(1)); + _J_c_x1.emplace('P', 'P', Matrix2d::Identity()); + _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)) ); + _J_c_x1.emplace('O', 'P', RowVector2d(0,0)); + _J_c_x1.emplace('O', 'O', Matrix1d(1)); _J_c_x2.clear(); - _J_c_x2.emplace("P", "P", R1); - _J_c_x2.emplace("P", "O", Vector2d(0,0)); - _J_c_x2.emplace("O", "P", RowVector2d(0,0)); - _J_c_x2.emplace("O", "O", Matrix1d(1)); + _J_c_x2.emplace('P', 'P', R1); + _J_c_x2.emplace('P', 'O', Vector2d(0,0)); + _J_c_x2.emplace('O', 'P', RowVector2d(0,0)); + _J_c_x2.emplace('O', 'O', Matrix1d(1)); // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'. // tc = t1 + R1 t2 - _c["P"] = p1 + R1 * p2; + _c['P'] = p1 + R1 * p2; // Rc = R1 R2 --> theta = theta1 + theta2 - _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ; + _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ; } @@ -366,15 +372,15 @@ inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1, inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res) { - plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O")); + plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O')); } inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau) { VectorComposite res; - res["P"] = Vector2d(); - res["O"] = Vector1d(); + res['P'] = Vector2d(); + res['O'] = Vector1d(); plus(x, tau, res); diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h index 4b000889c59320e8c7e819725c415dd19308d077..685eefabaccf1d0585ec7de7d73c9d4ba2e1ff8d 100644 --- a/include/core/math/SE3.h +++ b/include/core/math/SE3.h @@ -67,8 +67,8 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q) inline void identity(VectorComposite& v) { v.clear(); - v["P"] = Vector3d::Zero(); - v["O"] = Quaterniond::Identity().coeffs(); + v['P'] = Vector3d::Zero(); + v['O'] = Quaterniond::Identity().coeffs(); } template<typename T = double> @@ -207,7 +207,7 @@ inline void compose(const MatrixBase<D1>& d1, inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c) { - compose(x1.at("P"), x1.at("O"), x2.at("P"), x2.at("O"), c["P"], c["O"]); + compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']); } inline void compose(const VectorComposite& x1, @@ -218,20 +218,20 @@ inline void compose(const VectorComposite& x1, { // Some useful temporaries - const auto R1 = q2R(x1.at("O")); //q1.matrix(); // make temporary - const auto& R2 = q2R(x2.at("O")); //q2.matrix(); // do not make temporary, only reference + const auto R1 = q2R(x1.at('O')); //q1.matrix(); // make temporary + const auto& R2 = q2R(x2.at('O')); //q2.matrix(); // do not make temporary, only reference // Jac wrt first delta - J_c_x1.emplace("P", "P", Matrix3d::Identity() ) ; - J_c_x1.emplace("P", "O", - R1 * skew(x2.at("P")) ) ; - J_c_x1.emplace("O", "P", Matrix3d::Zero() ) ; - J_c_x1.emplace("O", "O", R2.transpose() ) ; + J_c_x1.emplace('P', 'P', Matrix3d::Identity() ) ; + J_c_x1.emplace('P', 'O', - R1 * skew(x2.at('P')) ) ; + J_c_x1.emplace('O', 'P', Matrix3d::Zero() ) ; + J_c_x1.emplace('O', 'O', R2.transpose() ) ; // Jac wrt second delta - J_c_x2.emplace("P", "P", R1 ); - J_c_x2.emplace("P", "O", Matrix3d::Zero() ); - J_c_x2.emplace("O", "P", Matrix3d::Zero() ); - J_c_x2.emplace("O", "O", Matrix3d::Identity() ); + J_c_x2.emplace('P', 'P', R1 ); + J_c_x2.emplace('P', 'O', Matrix3d::Zero() ); + J_c_x2.emplace('O', 'P', Matrix3d::Zero() ); + J_c_x2.emplace('O', 'O', Matrix3d::Identity() ); // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable compose(x1, x2, c); @@ -325,15 +325,15 @@ inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_i inline VectorComposite exp(const VectorComposite& tau) { - const auto& p = tau.at("P"); - const auto& theta = tau.at("O"); + const auto& p = tau.at('P'); + const auto& theta = tau.at('O'); Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145) VectorComposite res; - res["P"] = V * p ; - res["O"] = exp_q(theta).coeffs() ; + res['P'] = V * p ; + res['O'] = exp_q(theta).coeffs() ; return res; } @@ -392,15 +392,15 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res) { - plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O")); + plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O')); } inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau) { VectorComposite res; - res["P"] = Vector3d(); - res["O"] = Vector4d(); + res['P'] = Vector3d::Zero(); + res['O'] = Vector4d::Zero(); plus(x, tau, res); diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h index 4721c3def15ec0f5cdf8371387daf989402dcc12..45cbdf3e1366fab06820ca54fad65cfa7e3af5fd 100644 --- a/include/core/math/covariance.h +++ b/include/core/math/covariance.h @@ -22,13 +22,15 @@ inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, template <typename T, int N, int RC> inline bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS) { - Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M); + Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly); if (eigensolver.info() == Eigen::Success) { // All eigenvalues must be >= 0: return (eigensolver.eigenvalues().array() >= eps).all(); } + else + std::cout << "eigen decomposition failed" << std::endl; return false; } diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index f64f629056e3c5c52abc41818f049dc14b876438..5ab5f053fee0c4559a4f8fd473dee43777f54197 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -69,7 +69,6 @@ class Problem : public std::enable_shared_from_this<Problem> PriorOptionsPtr prior_options_; private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !! - Problem(const std::string& _frame_structure); // USE create() below !! Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !! void setup(); @@ -83,7 +82,7 @@ class Problem : public std::enable_shared_from_this<Problem> // Properties ----------------------------------------- public: SizeEigen getDim() const; - StateStructure getFrameStructure() const; + const StateStructure& getFrameStructure() const; protected: void appendToStructure(const StateStructure& _structure); @@ -206,7 +205,7 @@ class Problem : public std::enable_shared_from_this<Problem> const double &_time_tol); /** \brief Emplace frame from string frame_structure, dimension and vector - * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED + * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * \param _frame_structure String indicating the frame structure. * \param _dim variable indicating the dimension of the problem @@ -217,14 +216,13 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim, // - const Eigen::VectorXd& _frame_state); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim, // + const Eigen::VectorXd& _frame_state); /** \brief Emplace frame from string frame_structure and state - * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED + * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * \param _frame_structure String indicating the frame structure. * \param _frame_state State vector; must match the size and format of the chosen frame structure @@ -236,13 +234,12 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const VectorComposite& _frame_state); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const VectorComposite& _frame_state); /** \brief Emplace frame from state - * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED + * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * \param _frame_state State; must be part of the problem's frame structure * @@ -254,12 +251,11 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const VectorComposite& _frame_state); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + const VectorComposite& _frame_state); /** \brief Emplace frame from string frame_structure and dimension - * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED + * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * \param _frame_structure String indicating the frame structure. * \param _dim variable indicating the dimension of the problem @@ -272,13 +268,12 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim); /** \brief Emplace frame from state vector - * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED + * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * \param _frame_state State vector; must match the size and format of the chosen frame structure * @@ -290,12 +285,11 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const Eigen::VectorXd& _frame_state); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + const Eigen::VectorXd& _frame_state); /** \brief Emplace frame, guess all values - * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED + * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * * - The structure is taken from Problem @@ -307,15 +301,11 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp); // Frame getters FrameBasePtr getLastFrame( ) const; - FrameBasePtr getLastKeyFrame( ) const; - FrameBasePtr getLastKeyOrAuxFrame( ) const; - FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const; - FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const; + FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const; /** \brief Give the permission to a processor to create a new key Frame * @@ -333,22 +323,6 @@ class Problem : public std::enable_shared_from_this<Problem> ProcessorBasePtr _processor_ptr, // const double& _time_tolerance); - /** \brief Give the permission to a processor to create a new auxiliary Frame - * - * This should implement a black list of processors that have forbidden auxiliary frame creation. - * - This decision is made at problem level, not at processor configuration level. - * - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors. - */ - bool permitAuxFrame(ProcessorBasePtr _processor_ptr) const; - - /** \brief New auxiliary frame callback - * - * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion. - */ - void auxFrameCallback(FrameBasePtr _frame_ptr, // - ProcessorBasePtr _processor_ptr, // - const double& _time_tolerance); - // State getters TimeStamp getTimeStamp ( ) const; VectorComposite getState ( const StateStructure& _structure = "" ) const; @@ -380,8 +354,7 @@ class Problem : public std::enable_shared_from_this<Problem> bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const; bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const; bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const; - bool getLastKeyFrameCovariance(Eigen::MatrixXd& _covariance) const; - bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const; + bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const; bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const; diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h index 09d7bc68dee5511c28ee456bb42467e89168bbb4..dc0487046e5013626ebc02288a00915a7df58b24 100644 --- a/include/core/processor/is_motion.h +++ b/include/core/processor/is_motion.h @@ -22,6 +22,7 @@ class IsMotion { public: + IsMotion(const StateStructure& _structure); virtual ~IsMotion(); // Queries to the processor: @@ -29,15 +30,33 @@ class IsMotion virtual VectorComposite getState(const StateStructure& _structure = "") const = 0; virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0; - std::string getStateStructure(){return state_structure_;}; - void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;}; + VectorComposite getOdometry ( ) const; + private: + void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;} + + public: + const StateStructure& getStateStructure ( ) { return state_structure_; }; + void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; }; void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr); - + protected: - std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) + StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) + VectorComposite odometry_; }; +inline IsMotion::IsMotion(const StateStructure& _structure) : + state_structure_(_structure) +{ + // +} + + +inline wolf::VectorComposite IsMotion::getOdometry ( ) const +{ + return odometry_; +} + } ///// IMPLEMENTATION /////// diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index dd4c37b277a514f40051ee96de70ab04cfe43c62..c8945c8036ff54dc2e0fe2243b3ec0919c66eba4 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -18,6 +18,7 @@ class SensorBase; // std #include <memory> #include <map> +#include <chrono> namespace wolf { @@ -96,8 +97,17 @@ public: * respecting a defined time tolerances */ T select(const TimeStamp& _time_stamp, const double& _time_tolerance); + + /**\brief Select a Pack iterator from the buffer + * + * Select from the buffer the iterator pointing to the closest pack (w.r.t. time stamp), + * respecting a defined time tolerances + */ + Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance); T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance); + + T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance); T selectFirst(); @@ -117,7 +127,7 @@ public: * * elements are ordered from most recent to oldest */ - const std::map<TimeStamp,T>& getContainer(); + std::map<TimeStamp,T>& getContainer(); /**\brief Remove all packs in the buffer with a time stamp older than the specified * @@ -217,7 +227,6 @@ struct ParamsProcessorBase : public ParamsBase { time_tolerance = _server.getParam<double>(prefix + _unique_name + "/time_tolerance"); voting_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_active"); - voting_aux_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_aux_active"); apply_loss_function = _server.getParam<bool>(prefix + _unique_name + "/apply_loss_function"); } @@ -229,14 +238,11 @@ struct ParamsProcessorBase : public ParamsBase */ double time_tolerance; bool voting_active; ///< Whether this processor is allowed to vote for a Key Frame or not - bool voting_aux_active; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not bool apply_loss_function; ///< Whether this processor emplaces factors with loss function or not - std::string print() const + std::string print() const override { - return ParamsBase::print() + "\n" - + "voting_active: " + std::to_string(voting_active) + "\n" - + "voting_aux_active: " + std::to_string(voting_aux_active) + "\n" + return "voting_active: " + std::to_string(voting_active) + "\n" + "time_tolerance: " + std::to_string(time_tolerance) + "\n" + "apply_loss_function: " + std::to_string(apply_loss_function) + "\n"; } @@ -265,6 +271,21 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce unsigned int id() const; + // PROFILING + unsigned int n_capture_callback_; + unsigned int n_kf_callback_; + std::chrono::microseconds acc_duration_capture_; + std::chrono::microseconds acc_duration_kf_; + std::chrono::microseconds max_duration_capture_; + std::chrono::microseconds max_duration_kf_; + std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_; + std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_; + void startCaptureProfiling(); + void stopCaptureProfiling(); + void startKFProfiling(); + void stopKFProfiling(); + void printProfiling(std::ostream& stream = std::cout) const; + protected: /** \brief process an incoming capture * @@ -313,19 +334,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce */ virtual bool voteForKeyFrame() const = 0; - /** \brief Vote for Auxiliary Frame generation - * - * If a Auxiliary Frame criterion is validated, this function returns true, - * meaning that it wants to create a Auxiliary Frame at the \b last Capture. - * - * WARNING! This function only votes! It does not create Auxiliary Frames! - */ - virtual bool voteForAuxFrame() const {return false;}; - virtual bool permittedKeyFrame() final; - virtual bool permittedAuxFrame() final; - void setProblem(ProblemPtr) override; public: @@ -353,16 +363,15 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce bool isVotingActive() const; - bool isVotingAuxActive() const; - void setVotingActive(bool _voting_active = true); int getDim() const; + double getTimeTolerance() const; + void link(SensorBasePtr); template<typename classType, typename... T> static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all); - void setVotingAuxActive(bool _voting_active = true); virtual void printHeader(int depth, // bool constr_by, // @@ -382,24 +391,40 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const; }; -inline bool ProcessorBase::isVotingActive() const +inline void ProcessorBase::startCaptureProfiling() { - return params_->voting_active; + start_capture_ = std::chrono::high_resolution_clock::now(); } -inline bool ProcessorBase::isVotingAuxActive() const +inline void ProcessorBase::stopCaptureProfiling() { - return params_->voting_aux_active; + auto duration_capture = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_capture_); + + max_duration_capture_ = std::max(max_duration_capture_, duration_capture); + acc_duration_capture_ += duration_capture; } -inline void ProcessorBase::setVotingActive(bool _voting_active) +inline void ProcessorBase::startKFProfiling() { - params_->voting_active = _voting_active; + start_kf_ = std::chrono::high_resolution_clock::now(); } -inline void ProcessorBase::setVotingAuxActive(bool _voting_active) +inline void ProcessorBase::stopKFProfiling() +{ + auto duration_kf = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_kf_); + + max_duration_kf_ = std::max(max_duration_kf_, duration_kf); + acc_duration_kf_ += duration_kf; +} + +inline bool ProcessorBase::isVotingActive() const { - params_->voting_aux_active = _voting_active; + return params_->voting_active; +} + +inline void ProcessorBase::setVotingActive(bool _voting_active) +{ + params_->voting_active = _voting_active; } inline bool ProcessorBase::isMotion() const @@ -426,6 +451,10 @@ inline int ProcessorBase::getDim() const { return dim_; } +inline double ProcessorBase::getTimeTolerance() const +{ + return params_->time_tolerance; +} template<typename classType, typename... T> std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all) @@ -439,11 +468,8 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... ///////////////////////////////////////////////////////////////////////////////////////// template <typename T> -T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) +typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) { - if (container_.empty()) - return nullptr; - Buffer<T>::Iterator post = container_.upper_bound(_time_stamp); bool prev_exists = (post != container_.begin()); @@ -458,22 +484,39 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) bool prev_ok = simpleCheckTimeTolerance(prev->first, _time_stamp, _time_tolerance); if (prev_ok && !post_ok) - return prev->second; + return prev; else if (!prev_ok && post_ok) - return post->second; + return post; else if (prev_ok && post_ok) { if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp)) - return post->second; + return post; else - return prev->second; + return prev; } } else if (post_ok) - return post->second; + return post; + + return container_.end(); +} + +template <typename T> +T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) +{ + if (container_.empty()) + return nullptr; + Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance); + + // end is returned from selectIterator if an element of the buffer complying with the time stamp + // and time tolerance has not been found + if (it != container_.end()){ + return it->second; + } + return nullptr; } @@ -495,7 +538,27 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time // otherwise return nullptr (no pack before the provided ts or within the tolerance was found) return nullptr; +} + +template <typename T> +T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) +{ + // There is no element + if (container_.empty()) + return nullptr; + + // Checking on rbegin() since packs are ordered in time + // Return last pack if is older than time stamp + if (container_.rbegin()->first > _time_stamp) + return container_.rbegin()->second; + + // Return last pack if despite being newer, it is within the time tolerance + if (simpleCheckTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance)) + return container_.rbegin()->second; + + // otherwise return nullptr (no pack after the provided ts or within the tolerance was found) + return nullptr; } template <typename T> @@ -527,7 +590,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element) } template <typename T> -const std::map<TimeStamp,T>& Buffer<T>::getContainer() +std::map<TimeStamp,T>& Buffer<T>::getContainer() { return container_; } diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index e50b2f55764cbc2a5570a4192ed8c15f560804fe..7203ff3fd48833c5be92de7445dc174193e6a3e1 100644 --- a/include/core/processor/processor_diff_drive.h +++ b/include/core/processor/processor_diff_drive.h @@ -22,9 +22,9 @@ struct ParamsProcessorDiffDrive : public ParamsProcessorOdom2d ParamsProcessorOdom2d(_unique_name, _server) { } - std::string print() const + std::string print() const override { - return "\n" + ParamsProcessorOdom2d::print(); + return ParamsProcessorOdom2d::print(); } }; @@ -60,7 +60,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override; FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override; - VectorXd getCalibration (const CaptureBasePtr _capture) const override; + VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override; void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; protected: @@ -71,12 +71,15 @@ class ProcessorDiffDrive : public ProcessorOdom2d inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const { - return _capture->getStateBlock("I")->getState(); + if (_capture) + return _capture->getStateBlock('I')->getState(); + else + return getSensor()->getStateBlockDynamic('I')->getState(); } inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration) { - _capture->getStateBlock("I")->setState(_calibration); + _capture->getStateBlock('I')->setState(_calibration); } diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 6d2b81e9ed4425a693c7f669779d1f51a0118845..75053ab0f35d1c338092c723573d5cfbf85755b6 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -41,9 +41,9 @@ struct ParamsProcessorMotion : public ParamsProcessorBase angle_turned = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/angle_turned"); unmeasured_perturbation_std = _server.getParam<double>(prefix + _unique_name + "/unmeasured_perturbation_std"); } - std::string print() const + std::string print() const override { - return "\n" + ParamsProcessorBase::print() + "\n" + return ParamsProcessorBase::print() + "\n" + "max_time_span: " + std::to_string(max_time_span) + "\n" + "max_buff_length: " + std::to_string(max_buff_length) + "\n" + "dist_traveled: " + std::to_string(dist_traveled) + "\n" @@ -247,11 +247,11 @@ class ProcessorMotion : public ProcessorBase, public IsMotion double updateDt(); void integrateOneStep(); - void reintegrateBuffer(CaptureMotionPtr _capture_ptr); + void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const; void splitBuffer(const wolf::CaptureMotionPtr& capture_source, TimeStamp ts_split, const FrameBasePtr& keyframe_target, - const wolf::CaptureMotionPtr& capture_target); + const wolf::CaptureMotionPtr& capture_target) const; /** Pre-process incoming Capture * @@ -281,7 +281,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion PackKeyFramePtr computeProcessingStep(); // These are the pure virtual functions doing the mathematics - protected: + public: /** \brief convert raw CaptureMotion data to the delta-state format * @@ -416,13 +416,26 @@ class ProcessorMotion : public ProcessorBase, public IsMotion virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, const Eigen::VectorXd& delta_step) const = 0; + /** \brief merge two consecutive capture motions into the second one + * Merge two consecutive capture motions into the second one. + * The first capture motion is not removed. + * If the second capture has feature and factor emplaced, they are replaced by a new ones. + * @param cap_prev : the first capture motion to be merged (input) + * @param cap_target : the second capture motion (modified) + */ + void mergeCaptures(CaptureMotionConstPtr cap_prev, + CaptureMotionPtr cap_target); + + protected: /** \brief emplace a CaptureMotion - * \param _ts time stamp + * \param _frame_own frame owning the Capture to create * \param _sensor Sensor that produced the Capture + * \param _ts time stamp * \param _data a vector of motion data - * \param _sata_cov covariances matrix of the motion data data - * \param _frame_own frame owning the Capture to create - * \param _frame_origin frame acting as the origin of motions for the MorionBuffer of the created MotionCapture + * \param _data_cov covariances matrix of the motion data data + * \param _calib calibration vector + * \param _calib_preint calibration vector used during pre-integration + * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture */ virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, const SensorBasePtr& _sensor, @@ -444,14 +457,15 @@ class ProcessorMotion : public ProcessorBase, public IsMotion */ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0; - virtual VectorXd getCalibration (const CaptureBasePtr _capture) const = 0; virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0; Motion motionZero(const TimeStamp& _ts) const; + public: + + virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0; bool hasCalibration() const {return calib_size_ > 0;} - public: //getters CaptureMotionPtr getOrigin() const; CaptureMotionPtr getLast() const; @@ -484,21 +498,24 @@ class ProcessorMotion : public ProcessorBase, public IsMotion CaptureMotionPtr origin_ptr_; CaptureMotionPtr last_ptr_; CaptureMotionPtr incoming_ptr_; - + + FrameBasePtr last_frame_ptr_; + protected: // helpers to avoid allocation - double dt_; ///< Time step - Eigen::VectorXd x_; ///< current state - Eigen::VectorXd delta_; ///< current delta - Eigen::MatrixXd delta_cov_; ///< current delta covariance - Eigen::VectorXd delta_integrated_; ///< integrated delta - Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance - Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration - Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated - Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta - Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params - Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params - Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity + mutable double dt_; ///< Time step + mutable Eigen::VectorXd x_; ///< current state + mutable Eigen::VectorXd delta_; ///< current delta + mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance + mutable Eigen::VectorXd delta_integrated_; ///< integrated delta + mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance + mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration + mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated + mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta + mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params + mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params + + Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity }; } diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index de3aebb4a811e954a27f4ac24c22dd277d9142fd..2ad5b846b59e3c40d98351a0f8b30f75b3f71635 100644 --- a/include/core/processor/processor_odom_2d.h +++ b/include/core/processor/processor_odom_2d.h @@ -31,9 +31,9 @@ struct ParamsProcessorOdom2d : public ParamsProcessorMotion cov_det = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/cov_det"); } - std::string print() const + std::string print() const override { - return "\n" + ParamsProcessorMotion::print() + "\n" + return ParamsProcessorMotion::print() + "\n" + "cov_det: " + std::to_string(cov_det) + "\n"; } }; diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h index 0ff94590e542afe465676b7332d23ad5064ee56e..676cb6cd05d51240cad7a5008ca301ccb5af67e3 100644 --- a/include/core/processor/processor_odom_3d.h +++ b/include/core/processor/processor_odom_3d.h @@ -27,9 +27,9 @@ struct ParamsProcessorOdom3d : public ParamsProcessorMotion { // } - std::string print() const + std::string print() const override { - return "\n" + ParamsProcessorMotion::print(); + return ParamsProcessorMotion::print(); } }; diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index 0f4c3180003140544f251113583e0184e630d9b8..4d0970d4b4002fa78e1c5566cdeb0165e08ca1f4 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -27,7 +27,7 @@ struct ParamsProcessorTracker : public ParamsProcessorBase min_features_for_keyframe = _server.getParam<unsigned int>(prefix + _unique_name + "/min_features_for_keyframe"); max_new_features = _server.getParam<int>(prefix + _unique_name + "/max_new_features"); } - std::string print() const + std::string print() const override { return ParamsProcessorBase::print() + "\n" + "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n" @@ -101,6 +101,7 @@ class ProcessorTracker : public ProcessorBase CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker. CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture. CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed. + FrameBasePtr last_frame_ptr_; FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation. FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index d59daef943bffdc01e835453963f4dac8c7ad7df..f4cae62c31f2c8389107388bdebedbec72a68b5b 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -76,7 +76,7 @@ struct ParamsSensorBase: public ParamsBase std::string prefix = "sensor/"; ~ParamsSensorBase() override = default; using ParamsBase::ParamsBase; - std::string print() const + std::string print() const override { return ""; } @@ -90,20 +90,21 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh private: HardwareBaseWPtr hardware_ptr_; ProcessorBasePtrList processor_list_; - SizeEigen calib_size_; static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory) unsigned int sensor_id_; // sensor ID - std::map<std::string, bool> state_block_dynamic_; // mark dynamic state blocks + std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks - std::map<std::string, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_) + std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_) protected: Eigen::VectorXd noise_std_; // std of sensor noise Eigen::MatrixXd noise_cov_; // cov matrix of noise + CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree) + void setProblem(ProblemPtr _problem) override final; public: @@ -166,29 +167,27 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh public: const ProcessorBasePtrList& getProcessorList() const; - CaptureBasePtr lastCapture(void) const; - CaptureBasePtr lastKeyCapture(void) const; - CaptureBasePtr lastCapture(const TimeStamp& _ts) const; + CaptureBasePtr getLastCapture() const; + void setLastCapture(CaptureBasePtr); + void updateLastCapture(); + CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts) const; bool process(const CaptureBasePtr capture_ptr); // State blocks using HasStateBlocks::addStateBlock; - StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false); - StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); } - StateBlockPtr getStateBlockDynamic(const std::string& _key) const; - StateBlockPtr getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const; - StateBlockPtr getStateBlockDynamic(const char _key) const { return getStateBlockDynamic(std::string(1,_key)); } - StateBlockPtr getStateBlockDynamic(const char _key, const TimeStamp& _ts) const { return getStateBlockDynamic(std::string(1,_key), _ts); } + StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false); + StateBlockPtr getStateBlockDynamic(const char& _key) const; + StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const; // Declare a state block as dynamic or static (with _dymanic = false) - void setStateBlockDynamic(const std::string& _key, bool _dynamic = true); + void setStateBlockDynamic(const char& _key, bool _dynamic = true); - bool isStateBlockDynamic(const std::string& _key) const; - bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const; - bool isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const; - bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const; - bool isStateBlockInCapture(const std::string& _key) const; + bool isStateBlockDynamic(const char& _key) const; + bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const; + bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const; + bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const; + bool isStateBlockInCapture(const char& _key) const; StateBlockPtr getP(const TimeStamp _ts) const; StateBlockPtr getO(const TimeStamp _ts) const; @@ -218,7 +217,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh * \param _size state segment size (-1: whole state) (not used in quaternions) * **/ - void addPriorParameter(const std::string& _key, + void addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx = 0, @@ -234,9 +233,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh unsigned int _start_idx = 0, int _size = -1); - SizeEigen getCalibSize() const; - Eigen::VectorXd getCalibration() const; - void setNoiseStd(const Eigen::VectorXd & _noise_std); void setNoiseCov(const Eigen::MatrixXd & _noise_std); Eigen::VectorXd getNoiseStd() const; @@ -261,12 +257,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh void link(HardwareBasePtr); template<typename classType, typename... T> static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all); - - protected: - SizeEigen computeCalibSize() const; - - private: - void updateCalibSize(); }; } @@ -296,7 +286,12 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const return processor_list_; } -inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic) +inline CaptureBasePtr SensorBase::getLastCapture() const +{ + return last_capture_; +} + +inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic) { assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor"); HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem()); @@ -304,13 +299,13 @@ inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const St return _sb_ptr; } -inline void SensorBase::setStateBlockDynamic(const std::string& _key, bool _dynamic) +inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic) { assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway."); state_block_dynamic_[_key] = _dynamic; } -inline bool SensorBase::isStateBlockDynamic(const std::string& _key) const +inline bool SensorBase::isStateBlockDynamic(const char& _key) const { assert(state_block_dynamic_.count(_key) != 0); return state_block_dynamic_.at(_key); @@ -338,27 +333,17 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr) inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) { - addPriorParameter("P", _x, _cov, _start_idx, _size); + addPriorParameter('P', _x, _cov, _start_idx, _size); } inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov) { - addPriorParameter("O", _x, _cov); + addPriorParameter('O', _x, _cov); } inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) { - addPriorParameter("I", _x, _cov); -} - -inline SizeEigen SensorBase::getCalibSize() const -{ - return calib_size_; -} - -inline void SensorBase::updateCalibSize() -{ - calib_size_ = computeCalibSize(); + addPriorParameter('I', _x, _cov); } } // namespace wolf diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 8238139c3a2bf85238a9d4bacc7ac02994e987dd..212a955b19c1a76aecebaa90f7cc39be73f38a7a 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -37,9 +37,9 @@ struct ParamsSensorDiffDrive : public ParamsSensorBase prior_cov_diag = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_cov_diag"); ticks_cov_factor = _server.getParam<double>(prefix + _unique_name + "/ticks_cov_factor"); } - std::string print() const + std::string print() const override { - return "\n" + ParamsSensorBase::print() + "\n" + return ParamsSensorBase::print() + "\n" + "radius_left: " + std::to_string(radius_left) + "\n" + "radius_right: " + std::to_string(radius_right) + "\n" + "wheel_separation: " + std::to_string(wheel_separation) + "\n" diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h index b2a989a6fe2ef55092429d38b0a75698b1627be0..5f2fc1a717b7c58f985ede8845ea2de01dc17fcb 100644 --- a/include/core/sensor/sensor_odom_2d.h +++ b/include/core/sensor/sensor_odom_2d.h @@ -25,9 +25,9 @@ struct ParamsSensorOdom2d : public ParamsSensorBase k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp"); k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot"); } - std::string print() const + std::string print() const override { - return "\n" + ParamsSensorBase::print() + "\n" + return ParamsSensorBase::print() + "\n" + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n" + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"; } diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h index 37ec082f59543e3ad19cc2bea9370abee1e03c23..ab0e8b46872c21015b02ffcb9d521dbd77e2a606 100644 --- a/include/core/sensor/sensor_odom_3d.h +++ b/include/core/sensor/sensor_odom_3d.h @@ -36,9 +36,9 @@ struct ParamsSensorOdom3d : public ParamsSensorBase min_disp_var = _server.getParam<double>(prefix + _unique_name + "/min_disp_var"); min_rot_var = _server.getParam<double>(prefix + _unique_name + "/min_rot_var"); } - std::string print() const + std::string print() const override { - return "\n" + ParamsSensorBase::print() + "\n" + return ParamsSensorBase::print() + "\n" + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n" + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n" + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n" diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index 9b2a665facbd1cf2590a81cb311075789b33f575..fb38a8be279f9ed6572c729fdebe1551bb99f4ae 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -10,6 +10,7 @@ namespace wolf { WOLF_PTR_TYPEDEFS(SolverManager) +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver) /* * Macro for defining Autoconf solver creator for WOLF's high level API. @@ -20,137 +21,205 @@ WOLF_PTR_TYPEDEFS(SolverManager) * In order to use this macro, the derived solver manager class, SolverManagerClass, * must have a constructor available with the API: * - * SolverManagerClass(const SolverParamsClassPtr _params); + * SolverManagerClass(const ProblemPtr& wolf_problem, + * const ParamsSolverClassPtr _params); */ -#define WOLF_SOLVER_CREATE(SolverClass, SolverParamClass) \ -static SolverManagerPtr create(const std::string& _unique_name, \ - const ParamsServer& _server) \ -{ \ - auto params = std::make_shared<SolverParamClass>(_unique_name, _server); \ - \ - auto solver = std::make_shared<SolverClass>(params); \ - \ - solver->setName(_unique_name); \ - \ - return solver; \ -} \ -static SolverManagerPtr create(const std::string& _unique_name, const SolverParamsPtr _params) \ -{ \ - auto params = std::static_pointer_cast<SolverParamClass>(_params); \ - \ - auto solver = std::make_shared<SolverClass>(params); \ - \ - solver->setName(_unique_name); \ - \ - return solver; \ -} \ - -struct SolverParams: public ParamsBase -{ - SolverParams() = default; - SolverParams(std::string _unique_name, const ParamsServer& _server): - ParamsBase(_unique_name, _server) - { - // - } - - ~SolverParams() override = default; -}; +#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \ +static SolverManagerPtr create(const ProblemPtr& _problem, \ + const ParamsServer& _server) \ +{ \ + auto params = std::make_shared<ParamsSolverClass>(#SolverClass, _server); \ + return std::make_shared<SolverClass>(_problem, params); \ +} \ +static SolverManagerPtr create(const ProblemPtr& _problem, \ + const ParamsSolverPtr _params) \ +{ \ + auto params = std::static_pointer_cast<ParamsSolverClass>(_params); \ + return std::make_shared<SolverClass>(_problem, params); \ +} \ + + struct ParamsSolver; + /** * \brief Solver manager for WOLF */ class SolverManager { -public: - - /** \brief Enumeration of covariance blocks to be computed - * - * Enumeration of covariance blocks to be computed - * - */ - enum class CovarianceBlocksToBeComputed : std::size_t - { - ALL, ///< All blocks and all cross-covariances - ALL_MARGINALS, ///< All marginals - ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks - }; - - /** - * \brief Enumeration for the verbosity of the solver report. - */ - enum class ReportVerbosity : std::size_t - { - QUIET = 0, - BRIEF, - FULL - }; - -protected: - - ProblemPtr wolf_problem_; - -public: - - SolverManager(const ProblemPtr& wolf_problem); + public: - virtual ~SolverManager(); + /** \brief Enumeration of covariance blocks to be computed + * + * Enumeration of covariance blocks to be computed + * + */ + enum class CovarianceBlocksToBeComputed : std::size_t + { + ALL, ///< All blocks and all cross-covariances + ALL_MARGINALS, ///< All marginals + ROBOT_LANDMARKS, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks + GAUSS + }; - std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET); + /** + * \brief Enumeration for the verbosity of the solver report. + */ + enum class ReportVerbosity : std::size_t + { + QUIET = 0, + BRIEF, + FULL + }; - virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0; + // PROFILING + unsigned int n_solve_, n_cov_; + std::chrono::microseconds acc_duration_total_; + std::chrono::microseconds acc_duration_solver_; + std::chrono::microseconds acc_duration_update_; + std::chrono::microseconds acc_duration_state_; + std::chrono::microseconds acc_duration_cov_; - virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0; + std::chrono::microseconds max_duration_total_; + std::chrono::microseconds max_duration_solver_; + std::chrono::microseconds max_duration_update_; + std::chrono::microseconds max_duration_state_; + std::chrono::microseconds max_duration_cov_; - virtual bool hasConverged() = 0; + void printProfiling(std::ostream& stream = std::cout) const; - virtual SizeStd iterations() = 0; + protected: - virtual double initialCost() = 0; + ProblemPtr wolf_problem_; + ParamsSolverPtr params_; + + public: + /** + * \brief Constructor with default params_ + */ + SolverManager(const ProblemPtr& _problem); + /** + * \brief Constructor with given params_ + */ + SolverManager(const ProblemPtr& _problem, + const ParamsSolverPtr& _params); + + virtual ~SolverManager(); + + /** + * \brief Solves with the verbosity defined in params_ + */ + std::string solve(); + /** + * \brief Solves with a given verbosity + */ + std::string solve(const ReportVerbosity report_level); + + virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final; + + virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final; + + virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0; + + virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0; + + virtual bool hasConverged() = 0; + + virtual SizeStd iterations() = 0; + + virtual double initialCost() = 0; + + virtual double finalCost() = 0; + + /** + * \brief Updates solver's problem according to the wolf_problem + */ + virtual void update(); + + ProblemPtr getProblem(); + + double getPeriod() const; + + ReportVerbosity getVerbosity() const; + + virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final; + + virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final; + + virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final; + virtual bool isStateBlockFixed(const StateBlockPtr& st) final; + + virtual bool hasThisLocalParametrization(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) final; + + virtual bool hasLocalParametrization(const StateBlockPtr& st) const final; + + virtual int numFactors() const final; + virtual int numStateBlocks() const final; + virtual int numStateBlocksFloating() const final; + + virtual int numFactorsDerived() const = 0; + virtual int numStateBlocksDerived() const = 0; + + virtual bool check(std::string prefix="") const final; + + protected: + + std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_; + std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_; + std::set<FactorBasePtr> factors_; + std::set<StateBlockPtr> floating_state_blocks_; - virtual double finalCost() = 0; + virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr); + const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const; + double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr); - virtual void update(); + private: + // SolverManager functions + virtual void addFactor(const FactorBasePtr& fac_ptr) final; + virtual void removeFactor(const FactorBasePtr& fac_ptr) final; + virtual void addStateBlock(const StateBlockPtr& state_ptr) final; + virtual void removeStateBlock(const StateBlockPtr& state_ptr) final; + virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final; + virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final; + virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final; - ProblemPtr getProblem(); - - virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr); - - virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const; - - bool check(std::string prefix="") const; - -protected: - - std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_; - std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_; - std::set<FactorBasePtr> factors_; - - virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr); - const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const; - virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr); - -private: - // SolverManager functions - void addFactor(const FactorBasePtr& fac_ptr); - void removeFactor(const FactorBasePtr& fac_ptr); - void addStateBlock(const StateBlockPtr& state_ptr); - void removeStateBlock(const StateBlockPtr& state_ptr); - void updateStateBlockState(const StateBlockPtr& state_ptr); - void updateStateBlockStatus(const StateBlockPtr& state_ptr); - void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr); + protected: + // Derived virtual functions + virtual std::string solveDerived(const ReportVerbosity report_level) = 0; + virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0; + virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0; + virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0; + virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0; + virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0; + virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0; + virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0; + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0; + virtual bool checkDerived(std::string prefix="") const = 0; + virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0; + virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) = 0; + virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0; +}; -protected: - // Derived virtual functions - virtual std::string solveDerived(const ReportVerbosity report_level) = 0; - virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0; - virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0; - virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0; - virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0; - virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0; - virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0; - virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0; - virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0; - virtual bool checkDerived(std::string prefix="") const = 0; +// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity) +struct ParamsSolver: public ParamsBase +{ + std::string prefix = "solver/"; + double period = 0.0; + SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET; + + ParamsSolver() = default; + ParamsSolver(std::string _unique_name, const ParamsServer& _server): + ParamsBase(_unique_name, _server) + { + period = _server.getParam<double>(prefix + "period"); + verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose"); + } + std::string print() const override + { + return "period: " + std::to_string(period) + "\n"; + } + + ~ParamsSolver() override = default; }; } // namespace wolf diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h index efa0db6d76410f4437180b3bc9f2c844c2b878d5..4e1028bf954a7c2ac71e4d133a097700a6150283 100644 --- a/include/core/state_block/factory_state_block.h +++ b/include/core/state_block/factory_state_block.h @@ -116,7 +116,7 @@ inline std::string FactoryStateBlock::getClass() const } template<> -inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed) +inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen::VectorXd& _state, bool _fixed) { typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type); diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index b1af55f7b8c6adafc23d9ef4139b154537f0f9ff..cc4764a4d2452c2494195e47ba892a1ecb0120df 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -16,10 +16,6 @@ namespace wolf { -/// State of nodes containing several state blocks -typedef std::unordered_map<std::string, Eigen::VectorXd> State; -//typedef std::string StateStructure; - class HasStateBlocks { @@ -29,10 +25,10 @@ class HasStateBlocks virtual ~HasStateBlocks(); const StateStructure& getStructure() const { return structure_; } - void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; } - bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; } + void appendToStructure(const char& _frame_type){ structure_ += _frame_type; } + bool isInStructure(const char& _sb_type) { return structure_.find(_sb_type) != std::string::npos; } - const std::unordered_map<std::string, StateBlockPtr>& getStateBlockMap() const; + const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() const; std::vector<StateBlockPtr> getStateBlockVec() const; // Some typical shortcuts -- not all should be coded here, see notes below. @@ -44,27 +40,22 @@ class HasStateBlocks virtual void unfix(); virtual bool isFixed() const; - virtual StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem); - virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); } - virtual unsigned int removeStateBlock(const std::string& _sb_type); - virtual unsigned int removeStateBlock(const char _sb_type); - bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; } - bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); } + virtual StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem); + virtual unsigned int removeStateBlock(const char& _sb_type); + bool hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; } bool hasStateBlock(const StateBlockPtr& _sb) const; - StateBlockPtr getStateBlock(const std::string& _sb_type) const; - StateBlockPtr getStateBlock(const char _sb_type) const { return getStateBlock(std::string(1,_sb_type)); } - bool setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb); - bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return setStateBlock(std::string(1, _sb_type), _sb); } - bool stateBlockKey(const StateBlockPtr& _sb, std::string& _key) const; - std::unordered_map<std::string, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const; + StateBlockPtr getStateBlock(const char& _sb_type) const; + bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb); + bool stateBlockKey(const StateBlockPtr& _sb, char& _key) const; + std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const; // Emplace derived state blocks (angle, quaternion, etc). template<typename SB, typename ... Args> - std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor); + std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor); // Emplace base state blocks. template<typename ... Args> - StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor); + StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor); // Register/remove state blocks to/from wolf::Problem void registerNewStateBlocks(ProblemPtr _problem) const; @@ -82,12 +73,15 @@ class HasStateBlocks unsigned int getLocalSize(const StateStructure& _structure="") const; // Perturb state - void perturb(double amplitude = 0.01); + void perturb(double amplitude = 0.01); + + protected: + void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const; private: StateStructure structure_; - std::unordered_map<std::string, StateBlockPtr> state_block_map_; + std::unordered_map<char, StateBlockPtr> state_block_map_; }; @@ -112,7 +106,7 @@ inline HasStateBlocks::~HasStateBlocks() // } -inline const std::unordered_map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const +inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const { return state_block_map_; } @@ -127,18 +121,18 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const return sbv; } -inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type) -{ - return removeStateBlock(std::string(1, _sb_type)); -} +//inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type) +//{ +// return removeStateBlock(std::string(1, _sb_type)); +//} -inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type) +inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type) { return state_block_map_.erase(_sb_type); } template<typename SB, typename ... Args> -inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor) +inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor) { assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead."); std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...); @@ -149,7 +143,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& } template<typename ... Args> -inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor) +inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor) { assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead."); auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...); @@ -159,7 +153,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty return sb; } -inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb) +inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead."); assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead."); @@ -167,7 +161,7 @@ inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const Stat return true; // success } -inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const +inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) const { auto it = state_block_map_.find(_sb_type); if (it != state_block_map_.end()) @@ -178,12 +172,12 @@ inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_ inline wolf::StateBlockPtr HasStateBlocks::getP() const { - return getStateBlock("P"); + return getStateBlock('P'); } inline wolf::StateBlockPtr HasStateBlocks::getO() const { - return getStateBlock("O"); + return getStateBlock('O'); } inline void HasStateBlocks::fix() @@ -220,6 +214,7 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _ const auto& sb = getStateBlock(key); if (!sb) WOLF_ERROR("Stateblock key ", key, " not in the structure"); + assert (sb && "Stateblock key not in the structure"); sb->setState(vec, _notify); } @@ -237,9 +232,8 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS assert(_state.size() == size && "In FrameBase::setState wrong state size"); unsigned int index = 0; - for (const char ckey : structure) + for (const char key : structure) { - const auto& key = string(1,ckey); const auto& sb = getStateBlock(key); assert (sb && "Stateblock key not in the structure"); @@ -256,9 +250,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state, { SizeEigen index = 0; auto size_it = _sizes.begin(); - for (const auto& ckey : _structure) + for (const auto& key : _structure) { - const auto& key = string(1,ckey); const auto& sb = getStateBlock(key); assert (sb && "Stateblock key not in the structure"); assert(*size_it == sb->getSize() && "State block size mismatch"); @@ -273,9 +266,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state, inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify) { auto vec_it = _vectors.begin(); - for (const auto ckey : _structure) + for (const auto key : _structure) { - const auto key = string(1,ckey); const auto& sb = getStateBlock(key); assert (sb && "Stateblock key not in the structure"); assert(vec_it->size() == sb->getSize() && "State block size mismatch"); @@ -316,10 +308,8 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure VectorComposite state; - for (const auto ckey : structure) + for (const auto key : structure) { - const auto& key = string(1,ckey); // ckey is char - auto state_it = state_block_map_.find(key); if (state_it != state_block_map_.end()) @@ -327,10 +317,6 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure state.emplace(key, state_it->second->getState()); } -// for (auto& pair_key_sb : state_block_map_) -// { -// state.emplace(pair_key_sb.first, pair_key_sb.second->getState()); -// } return state; } @@ -351,11 +337,11 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co return size; } -inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const +inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const { const auto& it = std::find_if(state_block_map_.begin(), state_block_map_.end(), - [_sb](const std::pair<std::string, StateBlockPtr>& pair) + [_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; } @@ -371,7 +357,7 @@ inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const return it != state_block_map_.end(); } -inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& _key) const +inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, char& _key) const { const auto& it = this->find(_sb); @@ -384,7 +370,7 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& } else { - _key = ""; + _key = '0'; // '0' = not found return false; } } diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h index 2236c1d62e94c6223f13529d2be55d9fd60baf00..460835cb9bf03c92cfb73f8516248816212c6571 100644 --- a/include/core/state_block/local_parametrization_angle.h +++ b/include/core/state_block/local_parametrization_angle.h @@ -27,7 +27,7 @@ class LocalParametrizationAngle : public LocalParametrizationBase bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, Eigen::Map<const Eigen::VectorXd>& _x2, Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override; - + bool isValid(const Eigen::VectorXd& state, double tolerance) override; }; inline LocalParametrizationAngle::LocalParametrizationAngle() : @@ -64,6 +64,13 @@ inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>& return true; } +inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance) +{ + //Any real is a valid angle because we use the pi2pi function. Also + //the types don't match. In this case the argument is + //Eigen::Map not Eigen::VectorXd + return true; +} } /* namespace wolf */ #endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */ diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h index 14f2ce38f1dd55b6571884710eb7ed7b7640f985..8494ec3a89215af8287584c8255b241af3486b45 100644 --- a/include/core/state_block/local_parametrization_base.h +++ b/include/core/state_block/local_parametrization_base.h @@ -31,7 +31,7 @@ class LocalParametrizationBase{ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, Eigen::Map<const Eigen::VectorXd>& _x2, Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0; - + virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0; unsigned int getLocalSize() const; unsigned int getGlobalSize() const; }; diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h index 30785bb2cae80e94117d6895657b96617b6adefb..b8bbb981c6cec93c9e4fbfa29049a12f16d137a9 100644 --- a/include/core/state_block/local_parametrization_homogeneous.h +++ b/include/core/state_block/local_parametrization_homogeneous.h @@ -47,6 +47,7 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase bool minus(Eigen::Map<const Eigen::VectorXd>& _h1, Eigen::Map<const Eigen::VectorXd>& _h2, Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override; + bool isValid(const Eigen::VectorXd& state, double tolerance) override; }; } // namespace wolf diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h index 75804f85dabc9d9480ef045944f195fe1a472bb9..c637ab6bb07a23b0e5dd5528d6a1c240327e1473 100644 --- a/include/core/state_block/local_parametrization_quaternion.h +++ b/include/core/state_block/local_parametrization_quaternion.h @@ -73,6 +73,12 @@ public: bool minus(Eigen::Map<const Eigen::VectorXd>& _q1, Eigen::Map<const Eigen::VectorXd>& _q2, Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override; + // inline bool isValid(const Eigen::VectorXd& state) override; + // template<QuaternionDeltaReference DeltaReference> + bool isValid(const Eigen::VectorXd& _state, double tolerance) override + { + return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance; + } }; typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal; diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h index c8706e2b6d081802d4a3b2590538b2e45df0584b..5406fe0c105c67d87e4793ba86a2e52f81bbfa87 100644 --- a/include/core/state_block/state_block.h +++ b/include/core/state_block/state_block.h @@ -159,6 +159,8 @@ public: void plus(const Eigen::VectorXd& _dv); + bool isValid(double tolerance = Constants::EPS); + static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false); }; @@ -319,7 +321,10 @@ inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fi { return std::make_shared<StateBlock>(_state, _fixed); } - +inline bool StateBlock::isValid(double tolerance) +{ + return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true; +} }// namespace wolf #endif diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h index 0d3cafcc80c602a4de1bf59b9ce7c2ddd9123c9a..c7a125ac0d96b1d1b0799ab9e983bccf76bf36db 100644 --- a/include/core/state_block/state_composite.h +++ b/include/core/state_block/state_composite.h @@ -22,16 +22,16 @@ using std::string; using namespace Eigen; typedef std::string StateStructure; -typedef std::unordered_map < std::string, StateBlockPtr > StateBlockMap; +typedef std::unordered_map < char, StateBlockPtr > StateBlockMap; typedef StateBlockMap::const_iterator StateBlockMapCIter; -class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd > +class VectorComposite : public std::unordered_map < char, Eigen::VectorXd > { public: VectorComposite() {}; VectorComposite(const StateStructure& _s); VectorComposite(const StateStructure& _s, const std::list<int>& _sizes); - VectorComposite(const VectorComposite & v) : unordered_map<string, VectorXd>(v){}; + VectorComposite(const VectorComposite & v) : unordered_map<char, VectorXd>(v){}; /** * \brief Construct from Eigen::VectorXd and structure * @@ -76,17 +76,17 @@ class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd }; -class MatrixComposite : public std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > > +class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > > { private: - std::unordered_map < string, unsigned int> size_rows_, size_cols_; + std::unordered_map < char, unsigned int> size_rows_, size_cols_; unsigned int rows() const; unsigned int cols() const; void sizeAndIndices(const StateStructure & _struct_rows, const StateStructure & _struct_cols, - std::unordered_map < string, unsigned int>& _indices_rows, - std::unordered_map < string, unsigned int>& _indices_cols, + std::unordered_map < char, unsigned int>& _indices_rows, + std::unordered_map < char, unsigned int>& _indices_cols, unsigned int& _rows, unsigned int& _cols) const; @@ -129,25 +129,25 @@ class MatrixComposite : public std::unordered_map < std::string, std::unordered_ static MatrixComposite identity(const StateStructure& _structure, const std::list<int>& _sizes); - unsigned int count(const std::string &_row, - const std::string &_col) const; + unsigned int count(const char &_row, + const char &_col) const; - bool emplace(const std::string &_row, - const std::string &_col, + bool emplace(const char &_row, + const char &_col, const Eigen::MatrixXd &_mat_blk); // throw error if queried block is not present - bool at(const std::string &_row, - const std::string &_col, + bool at(const char &_row, + const char &_col, Eigen::MatrixXd &_mat_blk) const; - const MatrixXd& at(const std::string &_row, - const std::string &_col) const; - MatrixXd& at(const std::string &_row, - const std::string &_col); + const MatrixXd& at(const char &_row, + const char &_col) const; + MatrixXd& at(const char &_row, + const char &_col); // make some shadowed API available - using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::at; - using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::count; + using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::at; + using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count; MatrixXd matrix(const StateStructure & _struct_rows, @@ -266,25 +266,25 @@ class StateBlockComposite const StateBlockMap& getStateBlockMap() const; - StateBlockPtr add(const std::string& _sb_type, const StateBlockPtr& _sb); + StateBlockPtr add(const char& _sb_type, const StateBlockPtr& _sb); // Emplace derived state blocks (angle, quaternion, etc). template<typename SB, typename ... Args> - std::shared_ptr<SB> emplace(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor); + std::shared_ptr<SB> emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor); // Emplace base state blocks. template<typename ... Args> - inline StateBlockPtr emplace(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor); + inline StateBlockPtr emplace(const char& _sb_type, Args&&... _args_of_base_state_block_constructor); - unsigned int remove(const std::string& _sb_type); - bool has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; } + unsigned int remove(const char& _sb_type); + bool has(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; } bool has(const StateBlockPtr& _sb) const; - StateBlockPtr at(const std::string& _sb_type) const; - void set(const std::string& _sb_type, const StateBlockPtr& _sb); - void set(const std::string& _sb_type, const VectorXd& _vec); + StateBlockPtr at(const char& _sb_type) const; + void set(const char& _sb_type, const StateBlockPtr& _sb); + void set(const char& _sb_type, const VectorXd& _vec); void setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors); - bool key(const StateBlockPtr& _sb, std::string& _key) const; - std::string key(const StateBlockPtr& _sb) const; + bool key(const StateBlockPtr& _sb, char& _key) const; + char key(const StateBlockPtr& _sb) const; StateBlockMapCIter find(const StateBlockPtr& _sb) const; // identity and zero (they are the same with different names) @@ -321,7 +321,7 @@ class StateBlockComposite //////////// IMPLEMENTATION //////////// -inline unsigned int MatrixComposite::count(const std::string &_row, const std::string &_col) const +inline unsigned int MatrixComposite::count(const char &_row, const char &_col) const { const auto& rit = this->find(_row); @@ -334,7 +334,7 @@ inline unsigned int MatrixComposite::count(const std::string &_row, const std::s template<typename SB, typename ... Args> -inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string &_sb_type, +inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char &_sb_type, Args &&... _args_of_derived_state_block_constructor) { assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead."); @@ -346,7 +346,7 @@ inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string } template<typename ... Args> -inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_type, +inline StateBlockPtr wolf::StateBlockComposite::emplace(const char &_sb_type, Args &&... _args_of_base_state_block_constructor) { assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead."); @@ -358,7 +358,7 @@ inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_t } inline MatrixComposite::MatrixComposite (const MatrixComposite& m) - : unordered_map<string, unordered_map<string, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_) + : unordered_map<char, unordered_map<char, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_) { } diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 152a7b40e8d154f0b251d8fd322c5f0473126972..23939e2e81229de9bd4ab833ecf86ba2bddfb137 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -6,42 +6,73 @@ namespace wolf{ class Problem; class FrameBase; -class TimeStamp; } //Wolf includes #include "core/common/wolf.h" #include "core/common/node_base.h" +#include "core/common/time_stamp.h" //std includes namespace wolf { +class TrajectoryIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator +{ + public: + TrajectoryIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src) + : std::map<TimeStamp, FrameBasePtr>::const_iterator(std::move(src)) + { + + } + + // override the indirection operator + const FrameBasePtr& operator*() const { + return std::map<TimeStamp, FrameBasePtr>::const_iterator::operator*().second; + } +}; + +class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator +{ + public: + TrajectoryRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src) + : std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator(std::move(src)) + { + + } + + // override the indirection operator + const FrameBasePtr& operator*() const { + return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second; + } +}; + //class TrajectoryBase class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase> { friend FrameBase; private: - std::list<FrameBasePtr> frame_list_; + FrameMap frame_map_; protected: - FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame - FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame + std::string frame_structure_; // Defines the structure of the Frames in the Trajectory. + // FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame + // FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame public: TrajectoryBase(); ~TrajectoryBase() override; // Frames - const FrameBasePtrList& getFrameList() const; + const FrameMap& getFrameMap() const; FrameBasePtr getLastFrame() const; - FrameBasePtr getLastKeyFrame() const; - FrameBasePtr getLastKeyOrAuxFrame() const; - FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const; - FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const; - void sortFrame(FrameBasePtr _frm_ptr); - void updateLastFrames(); + FrameBasePtr getFirstFrame() const; + FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const; + TrajectoryIter begin() const; + TrajectoryIter end() const; + TrajectoryRevIter rbegin() const; + TrajectoryRevIter rend() const; virtual void printHeader(int depth, // bool constr_by, // @@ -65,30 +96,41 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj public: // factors void getFactorList(FactorBasePtrList & _fac_list) const; - - protected: - FrameBaseConstIter computeFrameOrder(FrameBasePtr _frame_ptr); - void moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place); }; -inline const FrameBasePtrList& TrajectoryBase::getFrameList() const +inline const FrameMap& TrajectoryBase::getFrameMap() const { - return frame_list_; + return frame_map_; } -inline FrameBasePtr TrajectoryBase::getLastFrame() const +inline FrameBasePtr TrajectoryBase::getFirstFrame() const { - return frame_list_.back(); + auto it = static_cast<TrajectoryIter>(frame_map_.begin()); + return *it; } - -inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const +inline TrajectoryIter TrajectoryBase::begin() const +{ + return static_cast<TrajectoryIter>(frame_map_.begin()); +} +inline TrajectoryIter TrajectoryBase::end() const { - return last_key_frame_ptr_; + return static_cast<TrajectoryIter>(frame_map_.end()); +} +inline TrajectoryRevIter TrajectoryBase::rbegin() const +{ + return static_cast<TrajectoryRevIter>(frame_map_.rbegin()); +} +inline TrajectoryRevIter TrajectoryBase::rend() const +{ + return static_cast<TrajectoryRevIter>(frame_map_.rend()); } -inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const +inline FrameBasePtr TrajectoryBase::getLastFrame() const { - return last_key_or_aux_frame_ptr_; + // return last_key_frame_ptr_; + auto last = this->rbegin(); + if(last != this->rend()) return *(this->rbegin()); + else return nullptr; } } // namespace wolf diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h index 2ffef15edf2e8a719581f76bbd957836ba6dc051..9fd06f2f0d78e82ee1eda5800c10707b4c84b759 100644 --- a/include/core/tree_manager/tree_manager_base.h +++ b/include/core/tree_manager/tree_manager_base.h @@ -55,9 +55,9 @@ struct ParamsTreeManagerBase : public ParamsBase ~ParamsTreeManagerBase() override = default; - std::string print() const + std::string print() const override { - return ParamsBase::print() + "\n"; + return ""; } }; diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h index 0ff5f31ed2734b7840518bf18dfc32073dac4cec..86bc095f47220e8c82f94c9952eafbe9133cdc96 100644 --- a/include/core/tree_manager/tree_manager_sliding_window.h +++ b/include/core/tree_manager/tree_manager_sliding_window.h @@ -1,7 +1,7 @@ #ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ #define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ -#include "../tree_manager/tree_manager_base.h" +#include "core/tree_manager/tree_manager_base.h" namespace wolf { @@ -15,20 +15,20 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) : ParamsTreeManagerBase(_unique_name, _server) { - n_key_frames = _server.getParam<unsigned int>(prefix + "/n_key_frames"); - fix_first_key_frame = _server.getParam<bool> (prefix + "/fix_first_key_frame"); + n_frames = _server.getParam<unsigned int>(prefix + "/n_frames"); + fix_first_frame = _server.getParam<bool> (prefix + "/fix_first_frame"); viral_remove_empty_parent = _server.getParam<bool> (prefix + "/viral_remove_empty_parent"); } std::string print() const { - return "\n" + ParamsTreeManagerBase::print() + "\n" - + "n_key_frames: " + std::to_string(n_key_frames) + "\n" - + "fix_first_key_frame: " + std::to_string(fix_first_key_frame) + "\n" + return ParamsTreeManagerBase::print() + "\n" + + "n_frames: " + std::to_string(n_frames) + "\n" + + "fix_first_frame: " + std::to_string(fix_first_frame) + "\n" + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n"; } - unsigned int n_key_frames; - bool fix_first_key_frame; + unsigned int n_frames; + bool fix_first_frame; bool viral_remove_empty_parent; }; @@ -43,7 +43,7 @@ class TreeManagerSlidingWindow : public TreeManagerBase ~TreeManagerSlidingWindow() override{} - void keyFrameCallback(FrameBasePtr _key_frame) override; + void keyFrameCallback(FrameBasePtr _frame) override; protected: ParamsTreeManagerSlidingWindowPtr params_sw_; diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h new file mode 100644 index 0000000000000000000000000000000000000000..3d455f89049e94181c762a8a9ef794474f94fd5c --- /dev/null +++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h @@ -0,0 +1,51 @@ +#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ +#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ + +#include "core/tree_manager/tree_manager_sliding_window.h" + +namespace wolf +{ + +WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate) +WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate) + +struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow +{ + ParamsTreeManagerSlidingWindowDualRate() = default; + ParamsTreeManagerSlidingWindowDualRate(std::string _unique_name, const wolf::ParamsServer & _server) : + ParamsTreeManagerSlidingWindow(_unique_name, _server) + { + n_frames_recent = _server.getParam<unsigned int>(prefix + "/n_frames_recent"); + assert(n_frames_recent <= n_frames); + rate_old_frames = _server.getParam<unsigned int>(prefix + "/rate_old_frames"); + } + std::string print() const + { + return ParamsTreeManagerBase::print() + "\n" + + "n_frames_recent: " + std::to_string(n_frames_recent) + "\n" + + "rate_old_frames: " + std::to_string(rate_old_frames) + "\n"; + } + + unsigned int n_frames_recent, rate_old_frames; +}; + +class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow +{ + public: + TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params); + ; + WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate) + + ~TreeManagerSlidingWindowDualRate() override{} + + void keyFrameCallback(FrameBasePtr _key_frame) override; + + protected: + ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_; + unsigned int count_frames_; + //TrajectoryIter first_recent_frame_it_; +}; + +} /* namespace wolf */ + +#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ */ diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h index 8c8e34935d728a505537643fa4c51bf001787bb6..0c9502c4ec0cc12ce041611d3a86d5c035af9d09 100644 --- a/include/core/utils/converter.h +++ b/include/core/utils/converter.h @@ -20,9 +20,35 @@ /** @file + converter.h + @brief This file implements a set of simple functions that deal with the correspondence between + classes and their string representation. The YAML autosetup framework makes heavy use of this file. + */ + +//Note: In order to deal with string representations we make heavy use of regexps. +// We use the standard C++11 regular expressions capabilities. + +/* +** This file is structured essentially in two parts: +** in the first part (which starts after the CONVERTERS ~~~~ STRING ----> TYPE line) +** we have functions to convert from string to C++ class. For example: + string v1 = "[3,4,5,6,7,8,9,10,11]"; + vector<int> v = converter<vector<int>>::convert(v1); + This code snippet transforms the string v1 which represents an std::vector into an actual std::vector value. + The second part (which starts after the TYPES ----> ToSTRING line) has the functions to + transform from C++ classes to strings. For instance, if we wanted to convert from a C++ class + to a string we would do something like this: + vector<int> vect{ 10, 20, 30 }; + string str = converter<std::string>::convert(vect); + //str == "[10,20,30]" */ namespace wolf{ + //This definition is a bit of a "hack". The reason of redefining the pair class is to be able + //to have two string representations of a pair, namely + //"(·,·)" -> typical pair/tuple representation + //"{·,·}" -> key-value pair representation used to represent maps. + //This is purely an aesthetic reason and could be removed without problems. template<class A, class B> struct Wpair : std::pair<A,B> { @@ -94,6 +120,16 @@ struct converter<bool>{ } }; template<> +struct converter<char>{ + static char convert(std::string val){ + //Here we should check that val.length() == 1 and get val[0] into a char variable + if(val.length() == 1) return val.at(0); + else throw std::runtime_error("Invalid converstion to char. String too long. String provided: " + val); + throw std::runtime_error("Invalid char conversion. String provided: " + val); + } +}; + //// TYPES ----> ToSTRING +template<> struct converter<std::string>{ static std::string convert(std::string val){ return val; @@ -109,6 +145,9 @@ struct converter<std::string>{ static std::string convert(double val){ return std::to_string(val); } + static std::string convert(char val){ + return std::to_string(val); + } template<typename A> static std::string convert(utils::list<A> val){ std::string result = ""; @@ -171,17 +210,17 @@ struct converter<std::string>{ static std::string convert(VectorComposite val){ //TODO // throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :("); - auto helper = std::unordered_map<StateStructure, Eigen::VectorXd>(); + auto helper = std::unordered_map<char, Eigen::VectorXd>(); for(const auto& pair: val) - helper.insert(std::pair<StateStructure, Eigen::VectorXd>(pair.first, pair.second)); + helper.insert(std::pair<char, Eigen::VectorXd>(pair.first, pair.second)); return converter<std::string>::convert(helper); } static std::string convert(MatrixComposite val){ //TODO // throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :("); - auto helper = std::unordered_map< StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(); + auto helper = std::unordered_map< char, std::unordered_map<char, Eigen::MatrixXd>>(); for(const auto& pair: val) - helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(pair.first, pair.second)); + helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(pair.first, pair.second)); return converter<std::string>::convert(helper); } }; @@ -305,13 +344,13 @@ struct converter<std::unordered_map<A,B>>{ template<> struct converter<VectorComposite>{ static VectorComposite convert(std::string val){ - auto unordered_map = converter<std::unordered_map<StateStructure, Eigen::VectorXd>>::convert(val); + auto unordered_map = converter<std::unordered_map<char, Eigen::VectorXd>>::convert(val); // VectorComposite result = VectorComposite(unordered_map); // return result; auto helper = VectorComposite(); for(auto const& it: unordered_map) { - helper.insert(std::pair<StateStructure, Eigen::VectorXd>(it.first, it.second)); + helper.insert(std::pair<char, Eigen::VectorXd>(it.first, it.second)); } return helper; } @@ -319,14 +358,14 @@ struct converter<VectorComposite>{ template<> struct converter<MatrixComposite>{ static MatrixComposite convert(std::string val){ - auto unordered_map = converter<std::unordered_map<StateStructure, - std::unordered_map<StateStructure, Eigen::MatrixXd>>>::convert(val); + auto unordered_map = converter<std::unordered_map<char, + std::unordered_map<char, Eigen::MatrixXd>>>::convert(val); // VectorComposite result = VectorComposite(unordered_map); // return result; auto helper = MatrixComposite(); for(auto const& it: unordered_map) { - helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(it.first, it.second)); + helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(it.first, it.second)); } return helper; } diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 880af68b86919dbab088f7f702f07ac41feefb98..f29109d1862d134aa592df4b4c843cac6b0a8bd0 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -23,26 +23,26 @@ CaptureBase::CaptureBase(const std::string& _type, assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!"); if (_sensor_ptr) { - if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P")) + if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P')) { assert(_p_ptr && "Pointer to dynamic position params is null!"); - addStateBlock("P", _p_ptr, nullptr); + addStateBlock('P', _p_ptr, nullptr); } else assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist"); - if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O")) + if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O')) { assert(_o_ptr && "Pointer to dynamic orientation params is null!"); - addStateBlock("O", _o_ptr, nullptr); + addStateBlock('O', _o_ptr, nullptr); } else assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist"); - if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I")) + if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I')) { assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!"); - addStateBlock("I", _intr_ptr, nullptr); + addStateBlock('I', _intr_ptr, nullptr); } else assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist"); @@ -56,7 +56,6 @@ CaptureBase::CaptureBase(const std::string& _type, CaptureBase::~CaptureBase() { - removeStateBlocks(getProblem()); } void CaptureBase::remove(bool viral_remove_empty_parent) @@ -71,6 +70,10 @@ void CaptureBase::remove(bool viral_remove_empty_parent) is_removing_ = true; CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it + // SensorBase::last_capture_ + if (getSensor() and getSensor()->getLastCapture() == this_C) + getSensor()->updateLastCapture(); + // remove downstream while (!constrained_by_list_.empty()) { @@ -149,15 +152,7 @@ bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const } -const StateStructure& CaptureBase::getStructure() const -{ - if (getSensor()) - return getSensor()->getStructure(); - else - return HasStateBlocks::getStructure(); -} - -StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const +StateBlockPtr CaptureBase::getStateBlock(const char& _key) const { if (getSensor() and getSensor()->hasStateBlock(_key)) { @@ -182,11 +177,9 @@ void CaptureBase::unfix() void CaptureBase::move(FrameBasePtr _frm_ptr) { - WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame"); - WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr"); + WOLF_WARN_COND(this->getFrame() == nullptr, "Moving Capture ", id(), " at ts=", getTimeStamp(), " not linked to any frame. Consider just linking it with link() instead of move()!"); - assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF"); - assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame"); + assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!"); // Unlink if (this->getFrame()) @@ -215,7 +208,7 @@ void CaptureBase::link(FrameBasePtr _frm_ptr) } else { - WOLF_WARN("Linking with a nullptr"); + WOLF_WARN("Linking Capture ", id(), " to a nullptr"); } } @@ -224,16 +217,26 @@ void CaptureBase::setProblem(ProblemPtr _problem) if (_problem == nullptr || _problem == this->getProblem()) return; + assert(getFrame() && "Cannot set problem: Capture has no Frame!"); + NodeBase::setProblem(_problem); registerNewStateBlocks(_problem); + // update SensorBase::last_capture_ + if (getSensor() and + (not getSensor()->getLastCapture() or + getSensor()->getLastCapture()->getTimeStamp() < time_stamp_)) + getSensor()->setLastCapture(shared_from_this()); + for (auto ft : feature_list_) ft->setProblem(_problem); } void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { - _stream << _tabs << "Cap" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp(); + _stream << _tabs << "Cap" << id() + << " " << getType() + << " ts = " << std::setprecision(3) << getTimeStamp(); if(getSensor() != nullptr) { @@ -247,36 +250,13 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s { _stream << " <-- "; for (auto cby : getConstrainedByList()) - _stream << "Fac" << cby->id() << " \t"; + if (cby) + _stream << "Fac" << cby->id() << " \t"; } _stream << std::endl; - if(getStateBlockMap().size() > 0) - { - if (_metric && _state_blocks){ - for (const auto& key : getStructure()) - { - auto sb = getStateBlock(key); - _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl; - } - } - else if (_metric) - { - _stream << _tabs << (isFixed() ? "Fix" : "Est"); - _stream << ",\t x= ( " << std::setprecision(2) << getStateVector() << " )"; - _stream << std::endl; - } - else if (_state_blocks) - { - _stream << " sb:"; - for (const auto& key : getStructure()) - { - const auto& sb = getStateBlock(key); - _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; "; - } - _stream << std::endl; - } - } + if (getStateBlockMap().size() > 0) + printState(_metric, _state_blocks, _stream, _tabs); } void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const @@ -284,7 +264,8 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 3) for (auto f : getFeatureList()) - f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); + if (f) + f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs) const @@ -364,7 +345,7 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os auto frm_cap = _cap_ptr->getFrame(); inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr << " ---> Frm" << frm_cap->id() << " @ " << frm_cap - << " -X-> Frm" << id(); + << " -X-> Frm" << id() << "\n"; auto frm_cap_list = frm_cap->getCaptureList(); auto frame_has_cap = std::find_if(frm_cap_list.begin(), frm_cap_list.end(), [&_cap_ptr](CaptureBasePtr cap){ return cap == _cap_ptr;}); log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation); @@ -376,6 +357,28 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os << " -X-> Cap" << id(); log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation); } + //Check that the capture is within time tolerance of some processor + auto frame = getFrame(); + double time_diff = fabs(getTimeStamp() - frame->getTimeStamp()); + + //It looks like some gtests add captures by hand, without using processors, so the processor list is empty. + //This inicialization is needed because if the list empty the execution does not go into the loop and the + //assertion fails + bool match_any_prc_timetolerance; + if(getSensor() != nullptr ) + { + match_any_prc_timetolerance = getSensor()->getProcessorList().empty(); + for(auto const& prc : getSensor()->getProcessorList()) + { + match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance()); + } + inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr + << " ts =" << getTimeStamp() << "Frm" << frame->id() + << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of" + << " any processor in sensor " << getSensor()->id() << "\n"; + log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation); + } + return log; } bool CaptureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index da17a57af80971b21f4a69322ba0c2ade54225d4..16a535dc987deea66756f4c8468ac3005f2b3573 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -75,7 +75,9 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { - _stream << _tabs << "CapM" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp(); + _stream << _tabs << "CapM" << id() + << " " << getType() + << " ts = " << std::setprecision(3) << getTimeStamp(); if(getSensor() != nullptr) { @@ -83,11 +85,12 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool } else _stream << " -> Sen-"; + if (auto OC = getOriginCapture()) { - _stream << " -> OCap" << OC->id(); + _stream << " -> Origin: Cap" << OC->id(); if (auto OF = OC->getFrame()) - _stream << " ; OFrm" << OF->id(); + _stream << " ; Frm" << OF->id(); } _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : ""); @@ -99,18 +102,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool } _stream << std::endl; - if (_state_blocks) - for (const auto& sb : getStateBlockVec()) - { - if(sb != nullptr) - { - _stream << _tabs << " " << "sb: "; - _stream << (sb->isFixed() ? "Fix" : "Est"); - if (_metric) - _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )"; - _stream << std::endl; - } - } + if (getStateBlockMap().size() > 0) + printState(_metric, _state_blocks, _stream, _tabs); _stream << _tabs << " " << "buffer size : " << getBuffer().size() << std::endl; if ( _metric && ! getBuffer().empty()) @@ -120,8 +113,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool // { // _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl; // _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl; -//// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl; -//// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl; +// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl; +// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl; // } } diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/solver_ceres.cpp similarity index 63% rename from src/ceres_wrapper/ceres_manager.cpp rename to src/ceres_wrapper/solver_ceres.cpp index b5bcfa4b7b6dc09e990adbecb93685c0294dbef5..67b009f243ace9d81664709c375a2008351cb73f 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -1,40 +1,36 @@ -#include "core/ceres_wrapper/ceres_manager.h" +#include "core/ceres_wrapper/solver_ceres.h" #include "core/ceres_wrapper/create_numeric_diff_cost_function.h" +#include "core/ceres_wrapper/cost_function_wrapper.h" +#include "core/ceres_wrapper/iteration_update_callback.h" +#include "core/ceres_wrapper/local_parametrization_wrapper.h" #include "core/trajectory/trajectory_base.h" #include "core/map/map_base.h" #include "core/landmark/landmark_base.h" #include "core/utils/make_unique.h" -namespace wolf { +namespace wolf +{ + +SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) : + SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>()) +{ +} -CeresManager::CeresManager(const ProblemPtr& _wolf_problem, - const ceres::Solver::Options& _ceres_options) : - SolverManager(_wolf_problem), - ceres_options_(_ceres_options) +SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, + const ParamsCeresPtr& _params) + : SolverManager(_wolf_problem, _params) + , params_ceres_(_params) { - ceres::Covariance::Options covariance_options; -#if CERES_VERSION_MINOR >= 13 - covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD; - covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; -#elif CERES_VERSION_MINOR >= 10 - covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD; -#else - covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD; -#endif - covariance_options.num_threads = 1; - covariance_options.apply_loss_function = false; - //covariance_options.null_space_rank = -1; - covariance_ = wolf::make_unique<ceres::Covariance>(covariance_options); - - ceres::Problem::Options problem_options; - problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - - ceres_problem_ = wolf::make_unique<ceres::Problem>(problem_options); + covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options); + ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options); + + if (params_ceres_->update_immediately) + getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_, + params_ceres_->min_num_iterations, + params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET)); } -CeresManager::~CeresManager() +SolverCeres::~SolverCeres() { while (!fac_2_residual_idx_.empty()) removeFactorDerived(fac_2_residual_idx_.begin()->first); @@ -44,21 +40,37 @@ CeresManager::~CeresManager() removeStateBlockDerived(state_blocks_.begin()->first); state_blocks_.erase(state_blocks_.begin()); } -} - SolverManagerPtr CeresManager::create(const ProblemPtr &_wolf_problem, const ParamsServer& _server) -{ - ceres::Solver::Options opt; - opt.max_num_iterations = _server.getParam<int>("solver/max_num_iterations"); - // CeresManager m = CeresManager(wolf_problem, opt); - return std::make_shared<CeresManager>(_wolf_problem, opt); + while (!getSolverOptions().callbacks.empty()) + { + delete getSolverOptions().callbacks.back(); + getSolverOptions().callbacks.pop_back(); + } } -std::string CeresManager::solveDerived(const ReportVerbosity report_level) +std::string SolverCeres::solveDerived(const ReportVerbosity report_level) { // run Ceres Solver - ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_); + ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_); + + /*/ if termination by user (Iteration callback, update and solve again) + // solve until max iterations reached + if (params_ceres_->update_immediately) + { + auto max_num_iterations = getSolverOptions().max_num_iterations; + while (summary_.termination_type == ceres::USER_SUCCESS) + { + // decrease wasted iterations + getSolverOptions().max_num_iterations -= summary_.iterations.size(); + if (getSolverOptions().max_num_iterations <= 0) + break; + // update and solve again + update(); + ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_); + } + getSolverOptions().max_num_iterations = max_num_iterations; + }*/ std::string report; //return report @@ -70,14 +82,11 @@ std::string CeresManager::solveDerived(const ReportVerbosity report_level) return report; } -void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks) +bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks) { // update problem update(); - // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM - wolf_problem_->clearCovariance(); - // CREATE DESIRED COVARIANCES LIST std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs; std::vector<std::pair<const double*, const double*>> double_pairs; @@ -89,13 +98,12 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks // first create a vector containing all state blocks std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks; //frame state blocks - for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList()) - if (fr_ptr->isKeyOrAux()) - for (const auto& key : fr_ptr->getStructure()) - { - const auto& sb = fr_ptr->getStateBlock(key); - all_state_blocks.push_back(sb); - } + for(auto fr_ptr : *wolf_problem_->getTrajectory()) + for (const auto& key : fr_ptr->getStructure()) + { + const auto& sb = fr_ptr->getStateBlock(key); + all_state_blocks.push_back(sb); + } // landmark state blocks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) @@ -105,40 +113,54 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks } // double loop all against all (without repetitions) for (unsigned int i = 0; i < all_state_blocks.size(); i++) + { + assert(isStateBlockRegisteredDerived(all_state_blocks[i])); for (unsigned int j = i; j < all_state_blocks.size(); j++) { + assert(isStateBlockRegisteredDerived(all_state_blocks[i])); + state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]); double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]), getAssociatedMemBlockPtr(all_state_blocks[j])); } + } break; } case CovarianceBlocksToBeComputed::ALL_MARGINALS: { // first create a vector containing all state blocks - for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList()) - if (fr_ptr->isKeyOrAux()) - for (const auto& key1 : wolf_problem_->getFrameStructure()) - for (const auto& key2 : wolf_problem_->getFrameStructure()) - { - const auto& sb1 = fr_ptr->getStateBlock(key1); - const auto& sb2 = fr_ptr->getStateBlock(key2); - state_block_pairs.emplace_back(sb1, sb2); - double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2)); - if (sb1 == sb2) - break; - } + for(auto fr_ptr : *wolf_problem_->getTrajectory()) + for (const auto& key1 : wolf_problem_->getFrameStructure()) + { + const auto& sb1 = fr_ptr->getStateBlock(key1); + assert(isStateBlockRegisteredDerived(sb1)); + + for (const auto& key2 : wolf_problem_->getFrameStructure()) + { + const auto& sb2 = fr_ptr->getStateBlock(key2); + assert(isStateBlockRegisteredDerived(sb2)); + + state_block_pairs.emplace_back(sb1, sb2); + double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2)); + if (sb1 == sb2) + break; + } + } // landmark state blocks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) for (auto sb : l_ptr->getUsedStateBlockVec()) + { + assert(isStateBlockRegisteredDerived(sb)); for(auto sb2 : l_ptr->getUsedStateBlockVec()) { + assert(isStateBlockRegisteredDerived(sb2)); state_block_pairs.emplace_back(sb, sb2); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2)); if (sb == sb2) break; } + } // // loop all marginals (PO marginals) // for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++) // { @@ -155,7 +177,10 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS: { //robot-robot - auto last_key_frame = wolf_problem_->getLastKeyFrame(); + auto last_key_frame = wolf_problem_->getLastFrame(); + + assert(isStateBlockRegisteredDerived(last_key_frame->getP())); + assert(isStateBlockRegisteredDerived(last_key_frame->getO())); state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP()); state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO()); @@ -177,6 +202,8 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++) { + assert(isStateBlockRegisteredDerived(*state_it)); + // robot - landmark state_block_pairs.emplace_back(last_key_frame->getP(), *state_it); state_block_pairs.emplace_back(last_key_frame->getO(), *state_it); @@ -188,20 +215,65 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks // landmark marginal for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++) { - state_block_pairs.emplace_back(*state_it, *next_state_it); - double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)), + assert(isStateBlockRegisteredDerived(*next_state_it)); + + state_block_pairs.emplace_back(*state_it, *next_state_it); + double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)), getAssociatedMemBlockPtr((*next_state_it))); } } } break; } + case CovarianceBlocksToBeComputed::GAUSS: + { + // State blocks: + // - Last KF: PV + + // last KF + FrameBasePtr last_frame = wolf_problem_->getLastFrame(); + if (not last_frame) + return false; + + // Error + if (not last_frame->hasStateBlock('P') or + not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or + not last_frame->hasStateBlock('V') or + not isStateBlockRegisteredDerived(last_frame->getStateBlock('V'))) + { + WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P or V, returning..."); + WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found"); + WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P')); + WOLF_WARN_COND(not last_frame->getStateBlock('V'), "SolverCeres::computeCovariances: KF state block 'V' not found"); + WOLF_WARN_COND(last_frame->getStateBlock('V') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')), "SolverCeres::computeCovariances: KF state block 'V' not registered ", last_frame->getStateBlock('V')); + + return false; + } + + // only marginals of P and V + state_block_pairs.emplace_back(last_frame->getStateBlock('P'), + last_frame->getStateBlock('P')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('P')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('P'))); + state_block_pairs.emplace_back(last_frame->getStateBlock('V'), + last_frame->getStateBlock('V')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('V')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('V'))); + + break; + } + default: + throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value"); + } //std::cout << "pairs... " << double_pairs.size() << std::endl; // COMPUTE DESIRED COVARIANCES if (covariance_->Compute(double_pairs, ceres_problem_.get())) { + // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM + wolf_problem_->clearCovariance(); + // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { @@ -210,34 +282,39 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); // std::cout << "covariance got switch: " << std::endl << cov << std::endl; } + return true; } - else - std::cout << "WARNING: Couldn't compute covariances!" << std::endl; + + WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!"); + return false; } -void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list) + +bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) { - //std::cout << "CeresManager: computing covariances..." << std::endl; + //std::cout << "SolverCeres: computing covariances..." << std::endl; // update problem update(); - // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM - wolf_problem_->clearCovariance(); - // CREATE DESIRED COVARIANCES LIST std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs; std::vector<std::pair<const double*, const double*>> double_pairs; // double loop all against all (without repetitions) - for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++){ + for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++) + { if (*st_it1 == nullptr){ continue; } + assert(isStateBlockRegisteredDerived(*st_it1)); + for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++) { if (*st_it2 == nullptr){ continue; } + assert(isStateBlockRegisteredDerived(*st_it2)); + state_block_pairs.emplace_back(*st_it1, *st_it2); double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)), getAssociatedMemBlockPtr((*st_it2))); @@ -248,6 +325,10 @@ void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list) // COMPUTE DESIRED COVARIANCES if (covariance_->Compute(double_pairs, ceres_problem_.get())) + { + // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM + wolf_problem_->clearCovariance(); + // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { @@ -256,13 +337,16 @@ void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list) wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl; } - else - std::cout << "WARNING: Couldn't compute covariances!" << std::endl; + return true; + } + + WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!"); + return false; } -void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr) +void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) { - assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map"); + assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map"); auto cost_func_ptr = createCostFunction(fac_ptr); fac_2_costfunction_[fac_ptr] = cost_func_ptr; @@ -271,6 +355,7 @@ void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr) res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size()); for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector()) { + assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb"); res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) ); } @@ -288,7 +373,7 @@ void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr) assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); } -void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr) +void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr) { assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map"); @@ -299,10 +384,10 @@ void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr) assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); } -void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr) +void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr) { assert(state_ptr); - assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added"); + assert(!isStateBlockRegisteredDerived(state_ptr) && "adding a state block already added"); assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block"); ceres::LocalParameterization* local_parametrization_ptr = nullptr; @@ -325,25 +410,28 @@ void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr) updateStateBlockStatusDerived(state_ptr); } -void CeresManager::removeStateBlockDerived(const StateBlockPtr& state_ptr) +void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr) { - //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; + //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; assert(state_ptr); - assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres"); + assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres"); + ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr)); state_blocks_local_param_.erase(state_ptr); } -void CeresManager::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) +void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) { assert(state_ptr != nullptr); + assert(isStateBlockRegisteredDerived(state_ptr) && "updating status of a state block that is not in ceres"); + if (state_ptr->isFixed()) ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr)); else ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr)); } -void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) +void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) { assert(state_ptr != nullptr); @@ -379,27 +467,27 @@ void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockP addFactorDerived(fac); } -bool CeresManager::hasConverged() +bool SolverCeres::hasConverged() { return summary_.termination_type == ceres::CONVERGENCE; } -SizeStd CeresManager::iterations() +SizeStd SolverCeres::iterations() { return summary_.iterations.size(); } -double CeresManager::initialCost() +double SolverCeres::initialCost() { return double(summary_.initial_cost); } -double CeresManager::finalCost() +double SolverCeres::finalCost() { return double(summary_.final_cost); } -ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fac_ptr) +ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac_ptr) { assert(_fac_ptr != nullptr); @@ -415,7 +503,7 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fa throw std::invalid_argument( "Wrong Jacobian Method!" ); } -bool CeresManager::checkDerived(std::string prefix) const +bool SolverCeres::checkDerived(std::string prefix) const { bool ok = true; @@ -424,12 +512,12 @@ bool CeresManager::checkDerived(std::string prefix) const ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size()) { - WOLF_ERROR("CeresManager::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size()); + WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size()); ok = false; } if (ceres_problem_->NumParameterBlocks() != state_blocks_.size()) { - WOLF_ERROR("CeresManager::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix); + WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix); ok = false; } @@ -439,7 +527,7 @@ bool CeresManager::checkDerived(std::string prefix) const { if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data())) { - WOLF_ERROR("CeresManager::check: any state block is missing in ceres problem - in ", prefix); + WOLF_ERROR("SolverCeres::check: any state block is missing in ceres problem - in ", prefix); ok = false; } } @@ -450,27 +538,27 @@ bool CeresManager::checkDerived(std::string prefix) const // SolverManager::factors_ if (factors_.count(fac_res_pair.first) == 0) { - WOLF_ERROR("CeresManager::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix); + WOLF_ERROR("SolverCeres::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix); ok = false; } // costfunction - residual if (fac_2_costfunction_.count(fac_res_pair.first) == 0) { - WOLF_ERROR("CeresManager::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix); + WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix); ok = false; } //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) { - WOLF_ERROR("CeresManager::check: fac_2_costfunction_ and ceres mismatch - in ", prefix); + WOLF_ERROR("SolverCeres::check: fac_2_costfunction_ and ceres mismatch - in ", prefix); ok = false; } // factor - residual if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor()) { - WOLF_ERROR("CeresManager::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix); + WOLF_ERROR("SolverCeres::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix); ok = false; } @@ -479,7 +567,7 @@ bool CeresManager::checkDerived(std::string prefix) const ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks); if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size()) { - WOLF_ERROR("CeresManager::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix); + WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix); ok = false; } else @@ -489,7 +577,7 @@ bool CeresManager::checkDerived(std::string prefix) const { if (getAssociatedMemBlockPtr(st) != param_blocks[i]) { - WOLF_ERROR("CeresManager::check: parameter", i, " mismatch - in ", prefix); + WOLF_ERROR("SolverCeres::check: parameter", i, " mismatch - in ", prefix); ok = false; } i++; @@ -508,7 +596,7 @@ void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& origina original.makeCompressed(); } -const Eigen::SparseMatrixd CeresManager::computeHessian() const +const Eigen::SparseMatrixd SolverCeres::computeHessian() const { Eigen::SparseMatrixd H; Eigen::SparseMatrixd A; @@ -539,7 +627,7 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i]; if (!sb->isFixed()) { - assert(state_blocks_local_param_.count(sb) != 0 && "factor involving a state block not added"); + assert(state_blocks_.count(sb) != 0 && "factor involving a state block not added"); assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols"); // insert since A_block_row has just been created so it's empty for sure @@ -569,10 +657,18 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const return H; } +bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) +{ + return state_blocks_local_param_.count(st) == 1 + && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param + && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) + == state_blocks_local_param_.at(st).get(); +} } // namespace wolf #include "core/solver/factory_solver.h" namespace wolf { - WOLF_REGISTER_SOLVER(CeresManager) + WOLF_REGISTER_SOLVER(SolverCeres) } // namespace wolf diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 4e18cb47d8485a7b9ee83853e863ddf20bb7e78a..c03fe00ad9b3a6018747b07572d0d6c1dbfd4a56 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -151,20 +151,23 @@ void FactorBase::remove(bool viral_remove_empty_parent) CaptureBasePtr FactorBase::getCapture() const { - assert(getFeature() != nullptr && "calling getCapture before linking with a feature"); - return getFeature()->getCapture(); + auto ftr = getFeature(); + if (ftr != nullptr) return ftr->getCapture(); + else return nullptr; } FrameBasePtr FactorBase::getFrame() const { - assert(getCapture() != nullptr && "calling getFrame before linking with a capture"); - return getCapture()->getFrame(); + auto cpt = getCapture(); + if(cpt != nullptr) return cpt->getFrame(); + else return nullptr; } SensorBasePtr FactorBase::getSensor() const { - assert(getCapture() != nullptr && "calling getSensor before linking with a capture"); - return getCapture()->getSensor(); + auto cpt = getCapture(); + if(cpt != nullptr) return cpt->getSensor(); + else return nullptr; } void FactorBase::setStatus(FactorStatus _status) @@ -257,10 +260,6 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) _ftr_ptr->addFactor(shared_from_this()); this->setFeature(_ftr_ptr); - // if frame, should be key - if (getCapture() and getFrame()) - assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame."); - // set problem ( and register factor ) WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM."); this->setProblem(_ftr_ptr->getProblem()); @@ -271,7 +270,7 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) auto frame_other = frm_ow.lock(); if(frame_other != nullptr) { - assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other."); + assert(frame_other->getProblem() && "Forbidden: linking a factor with a floating frame_other."); frame_other->addConstrainedBy(shared_from_this()); } } diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 69133345e0f57c28e3135aa3b50e0d99e179be50..c2af9127c6a2e028d98f4f5413490117bfbd7825 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -182,7 +182,8 @@ void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s { _stream << " <--\t"; for (auto cby : getConstrainedByList()) - _stream << "Fac" << cby->id() << " \t"; + if (cby) + _stream << "Fac" << cby->id() << " \t"; } _stream << std::endl; if (_metric) @@ -196,7 +197,8 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 4) for (auto c : getFactorList()) - c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); + if (c) + c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 4147b98ca8dcd67d76fb732f11cbbee85cea9df8..515de7b3de60ac3d9ad8fa5d48b58ba9b6939165 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -12,15 +12,13 @@ namespace wolf { unsigned int FrameBase::frame_id_count_ = 0; -FrameBase::FrameBase(const FrameType & _tp, - const TimeStamp& _ts, +FrameBase::FrameBase(const TimeStamp& _ts, const std::string _frame_structure, const VectorComposite& _state) : NodeBase("FRAME", "FrameBase"), HasStateBlocks(_frame_structure), trajectory_ptr_(), frame_id_(++frame_id_count_), - type_(_tp), time_stamp_(_ts) { for (const auto& pair_key_vec : _state) @@ -28,33 +26,30 @@ FrameBase::FrameBase(const FrameType & _tp, const auto& key = pair_key_vec.first; const auto& vec = pair_key_vec.second; - StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed + StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed addStateBlock(key, sb); } } -FrameBase::FrameBase(const FrameType & _tp, - const TimeStamp& _ts, +FrameBase::FrameBase(const TimeStamp& _ts, const std::string _frame_structure, const std::list<VectorXd>& _vectors) : NodeBase("FRAME", "FrameBase"), HasStateBlocks(_frame_structure), trajectory_ptr_(), frame_id_(++frame_id_count_), - type_(_tp), time_stamp_(_ts) { assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!"); auto vec_it = _vectors.begin(); - for (const auto ckey : _frame_structure) + for (const auto key : _frame_structure) { - const auto& key = string(1,ckey); const auto& vec = *vec_it; - StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed + StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed addStateBlock(key, sb); @@ -63,8 +58,7 @@ FrameBase::FrameBase(const FrameType & _tp, } -FrameBase::FrameBase(const FrameType & _tp, - const TimeStamp& _ts, +FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : @@ -72,20 +66,19 @@ FrameBase::FrameBase(const FrameType & _tp, HasStateBlocks(""), trajectory_ptr_(), frame_id_(++frame_id_count_), - type_(_tp), time_stamp_(_ts) { if (_p_ptr) { - addStateBlock("P", _p_ptr); + addStateBlock('P', _p_ptr); } if (_o_ptr) { - addStateBlock("O", _o_ptr); + addStateBlock('O', _o_ptr); } if (_v_ptr) { - addStateBlock("V", _v_ptr); + addStateBlock('V', _v_ptr); } } @@ -117,8 +110,7 @@ void FrameBase::remove(bool viral_remove_empty_parent) } // Remove Frame State Blocks - if ( isKeyOrAux() ) - removeStateBlocks(getProblem()); + removeStateBlocks(getProblem()); // remove from upstream TrajectoryBasePtr T = trajectory_ptr_.lock(); @@ -132,52 +124,12 @@ void FrameBase::remove(bool viral_remove_empty_parent) void FrameBase::setTimeStamp(const TimeStamp& _ts) { time_stamp_ = _ts; - if (isKeyOrAux() && getTrajectory() != nullptr) - getTrajectory()->sortFrame(shared_from_this()); } -void FrameBase::setNonEstimated() +void FrameBase::link(ProblemPtr _prb) { - // unregister if previously estimated - if (isKeyOrAux()) - for (const auto& sb : getStateBlockVec()) - getProblem()->notifyStateBlock(sb, REMOVE); - - type_ = NON_ESTIMATED; - if (getTrajectory()) - { - getTrajectory()->sortFrame(shared_from_this()); - getTrajectory()->updateLastFrames(); - } -} - -void FrameBase::setKey() -{ - // register if previously not estimated - if (!isKeyOrAux()) - registerNewStateBlocks(getProblem()); - - type_ = KEY; - - if (getTrajectory()) - { - getTrajectory()->sortFrame(shared_from_this()); - getTrajectory()->updateLastFrames(); - } -} - -void FrameBase::setAux() -{ - if (!isKeyOrAux()) - registerNewStateBlocks(getProblem()); - - type_ = AUXILIARY; - - if (getTrajectory()) - { - getTrajectory()->sortFrame(shared_from_this()); - getTrajectory()->updateLastFrames(); - } + assert(_prb != nullptr && "Trying to link Frame with a null problem pointer!"); + this->link(_prb->getTrajectory()); } bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const @@ -190,12 +142,12 @@ FrameBasePtr FrameBase::getPreviousFrame() const assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory"); //look for the position of this node in the upper list (frame list of trajectory) - for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ ) + for (auto f_it = getTrajectory()->rbegin(); f_it != getTrajectory()->rend(); f_it++ ) { if ( this->frame_id_ == (*f_it)->id() ) { f_it++; - if (f_it != getTrajectory()->getFrameList().rend()) + if (f_it != getTrajectory()->rend()) { return *f_it; } @@ -211,11 +163,11 @@ FrameBasePtr FrameBase::getPreviousFrame() const FrameBasePtr FrameBase::getNextFrame() const { //std::cout << "finding next frame of " << this->frame_id_ << std::endl; - auto f_it = getTrajectory()->getFrameList().rbegin(); + auto f_it = getTrajectory()->rbegin(); f_it++; //starting from second last frame //look for the position of this node in the frame list of trajectory - while (f_it != getTrajectory()->getFrameList().rend()) + while (f_it != getTrajectory()->rend()) { if ( this->frame_id_ == (*f_it)->id()) { @@ -326,7 +278,7 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr) } else { - WOLF_WARN("Linking with a nullptr"); + WOLF_WARN("Linking Frame ", id(), " to a nullptr"); } } @@ -336,8 +288,7 @@ void FrameBase::setProblem(ProblemPtr _problem) return; NodeBase::setProblem(_problem); - if (this->isKey()) - registerNewStateBlocks(_problem); + registerNewStateBlocks(_problem); for (auto cap : capture_list_) cap->setProblem(_problem); @@ -345,7 +296,7 @@ void FrameBase::setProblem(ProblemPtr _problem) void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { - _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() + _stream << _tabs << "Frm" << id() << " " << getStructure() << " ts = " << std::setprecision(3) << getTimeStamp() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : ""); @@ -353,37 +304,12 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta { _stream << " <-- "; for (auto cby : getConstrainedByList()) - _stream << "Fac" << cby->id() << " \t"; + if (cby) + _stream << "Fac" << cby->id() << " \t"; } _stream << std::endl; - if (_metric && _state_blocks) - { - for (const auto& key : getStructure()) - { - auto sb = getStateBlock(key); - _stream << _tabs << " " << key - << "[" << (sb->isFixed() ? "Fix" : "Est") - << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )" - << std::endl; - } - } - else if (_metric) - { - _stream << _tabs << " " << (isFixed() ? "Fix" : "Est"); - _stream << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )"; - _stream << std::endl; - } - else if (_state_blocks) - { - _stream << _tabs << " " << "sb:"; - for (const auto& key : getStructure()) - { - const auto& sb = getStateBlock(key); - _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; "; - } - _stream << std::endl; - } + printState(_metric, _state_blocks, _stream, _tabs); } void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const @@ -391,7 +317,8 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 2) for (auto C : getCaptureList()) - C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); + if (C) + C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs) const @@ -400,7 +327,7 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea std::stringstream inconsistency_explanation; if (_verbose) { - _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "EFrm") : "Frm") + _stream << _tabs << "Frm" << id() << " @ " << _frm_ptr.get() << std::endl; _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl; _stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl; @@ -433,11 +360,6 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea << " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n"; log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation); - // // check trajectory pointer - // inconsistency_explanation << "Frame trajectory pointer is " << getTrajectory() - // << " but trajectory pointer is" << trajectory_ptr << "\n"; - // log.assertTrue((getTrajectory() == T, inconsistency_explanation); - // check constrained_by for (auto cby : getConstrainedByList()) { @@ -473,9 +395,8 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr << " ---> Trj" << " @ " << trj_ptr << " -X-> Frm" << id(); - auto trj_frm_list = trj_ptr->getFrameList(); - auto trj_has_frm = std::find_if(trj_frm_list.begin(), trj_frm_list.end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;}); - log.assertTrue(trj_has_frm != trj_frm_list.end(), inconsistency_explanation); + auto trj_has_frm = std::find_if(trj_ptr->begin(), trj_ptr->end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;}); + log.assertTrue(trj_has_frm != trj_ptr->end(), inconsistency_explanation); for(auto C : getCaptureList()) { diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp index 72f07b8e216e6ca830ec9021dbb7ca1144d09860..e6e5a15c3c216d2abfa9242a48bb97081e22d016 100644 --- a/src/hardware/hardware_base.cpp +++ b/src/hardware/hardware_base.cpp @@ -30,7 +30,8 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 1) for (auto S : getSensorList()) - S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); + if (S) + S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 0fdd18775a2ea50c2ca6ee51ffaeaff9db7c85fa..11e26334cfedde1e6ffcae6f4588a57651c51eaa 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -21,11 +21,11 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State { if (_p_ptr) { - addStateBlock("P", _p_ptr, nullptr); + addStateBlock('P', _p_ptr, nullptr); } if (_o_ptr) { - addStateBlock("O", _o_ptr, nullptr); + addStateBlock('O', _o_ptr, nullptr); } } @@ -165,33 +165,12 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _ { _stream << "\t<-- "; for (auto cby : getConstrainedByList()) - _stream << "Fac" << cby->id() << " \t"; + if (cby) + _stream << "Fac" << cby->id() << " \t"; } _stream << std::endl; - if (_metric && _state_blocks){ - for (const auto& key : getStructure()) - { - auto sb = getStateBlock(key); - _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl; - } - } - else if (_metric) - { - _stream << _tabs << " " << (isFixed() ? "Fix" : "Est"); - _stream << ",\t x = ( " << std::setprecision(2) << getStateVector().transpose() << " )"; - _stream << std::endl; - } - else if (_state_blocks) - { - _stream << _tabs << " " << "sb:"; - for (const auto& key : getStructure()) - { - const auto& sb = getStateBlock(key); - _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; "; - } - _stream << std::endl; - } + printState(_metric, _state_blocks, _stream, _tabs); } void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp index 67c37971b4e0517017f03b9cd798080fcd40808d..7f6db136f0d1644bb4d2ff193fc3c7d776fdfa26 100644 --- a/src/map/map_base.cpp +++ b/src/map/map_base.cpp @@ -99,7 +99,8 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 1) for (auto L : getLandmarkList()) - L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); + if (L) + L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs) const diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 813077742ceca8ee0b7b60913fab479145c4298b..3dceffdd79180d3bb3bd9f5d4a27d33321ace5b7 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -325,20 +325,17 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const return (*sen_it); } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim, // - const Eigen::VectorXd& _frame_state) +FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim, // + const Eigen::VectorXd& _frame_state) { VectorComposite state; SizeEigen index = 0; SizeEigen size = 0; - for (const auto& ckey : _frame_structure) + for (const auto& key : _frame_structure) { - const auto& key = string(1,ckey); // ckey is char - - if (key == "O") + if (key == 'O') { if (_dim == 2) size = 1; else size = 4; @@ -350,69 +347,71 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // state.emplace(key, _frame_state.segment(index, size)); + // append new key to frame structure + if (frame_structure_.find(key) == std::string::npos) // not found + frame_structure_ += std::string(1,key); + index += size; } assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large."); auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, - _frame_key_type, _time_stamp, _frame_structure, state); + return frm; } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim) +FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim) { - return emplaceFrame(_frame_key_type, - _time_stamp, + return emplaceFrame(_time_stamp, _frame_structure, getState(_time_stamp)); } -FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // +FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // const StateStructure& _frame_structure, // const VectorComposite& _frame_state) { return FrameBase::emplace<FrameBase>(getTrajectory(), - _frame_key_type, _time_stamp, _frame_structure, _frame_state); } -FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const VectorComposite& _frame_state) +FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // + const VectorComposite& _frame_state) { + // append new keys to frame structure + for (const auto& pair_key_vec : _frame_state) + { + const auto& key = pair_key_vec.first; + if (frame_structure_.find(key) == std::string::npos) // not found + frame_structure_ += std::string(1,key); + } + return FrameBase::emplace<FrameBase>(getTrajectory(), - _frame_key_type, _time_stamp, getFrameStructure(), _frame_state); } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // +FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // const Eigen::VectorXd& _frame_state) { - return emplaceFrame(_frame_key_type, - _time_stamp, + return emplaceFrame(_time_stamp, this->getFrameStructure(), this->getDim(), _frame_state); } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp) +FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp) { - return emplaceFrame(_frame_key_type, - _time_stamp, + return emplaceFrame(_time_stamp, this->getFrameStructure(), this->getDim()); } @@ -429,15 +428,14 @@ TimeStamp Problem::getTimeStamp ( ) const if (not ts.ok()) { - const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); + const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame(); if (last_kf_or_aux) ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state } - if (not ts.ok()) - WOLF_WARN( "Problem has nowhere to find a valid timestamp!"); + WOLF_DEBUG_COND(not ts.ok(), "Problem has nowhere to find a valid timestamp!"); return ts; } @@ -464,7 +462,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const VectorComposite state_last; - const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); + const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame(); if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure); @@ -472,9 +470,8 @@ VectorComposite Problem::getState(const StateStructure& _structure) const else if (prior_options_ and prior_options_->mode != "nothing") state_last = prior_options_->state; - for (const auto& ckey : structure) + for (const auto& key : structure) { - const auto& key = string(1,ckey); if (state.count(key) == 0) { auto state_last_it = state_last.find(key); @@ -483,7 +480,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const state.emplace(key, state_last_it->second); else - state.emplace(key, stateZero(key).at(key)); + state.emplace(key, stateZero(string(1,key)).at(key)); } } @@ -514,7 +511,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ VectorComposite state_last; - const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); + const auto& last_kf_or_aux = trajectory_ptr_->closestFrameToTimeStamp(_ts); if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure); @@ -522,9 +519,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ else if (prior_options_ and prior_options_->mode != "nothing") state_last = prior_options_->state; - for (const auto& ckey : structure) + for (const auto& key : structure) { - const auto& key = string(1,ckey); if (state.count(key) == 0) { auto state_last_it = state_last.find(key); @@ -533,7 +529,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ state.emplace(key, state_last_it->second); else - state.emplace(key, stateZero(key).at(key)); + state.emplace(key, stateZero(string(1,key)).at(key)); } } @@ -544,16 +540,17 @@ SizeEigen Problem::getDim() const { return dim_; } -StateStructure Problem::getFrameStructure() const + +const StateStructure& Problem::getFrameStructure() const { return frame_structure_; } void Problem::appendToStructure(const StateStructure& _structure) { - for (const auto& ckey : _structure) - if (frame_structure_.find(ckey) == std::string::npos) // now key not found in problem structure -> append! - frame_structure_ += ckey; + for (const auto& key : _structure) + if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append! + frame_structure_ += key; } void Problem::setTreeManager(TreeManagerBasePtr _gm) @@ -589,11 +586,10 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const StateStructure structure = (_structure == "" ? getFrameStructure() : _structure); VectorComposite state; - for (const auto& ckey : structure) + for (const auto& key : structure) { - const auto& key = string(1,ckey); VectorXd vec; - if (key == "O") + if (key == 'O') { if (dim_ == 2) vec = VectorXd::Zero(1); else if (dim_ == 3) vec = Quaterniond::Identity().coeffs(); @@ -622,6 +618,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); + // pause processor profiling + if (_processor_ptr) + _processor_ptr->stopCaptureProfiling(); + for (auto sensor : hardware_ptr_->getSensorList()) for (auto processor : sensor->getProcessorList()) if (processor && (processor != _processor_ptr) ) @@ -646,31 +646,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro // notify tree manager if (tree_manager_) tree_manager_->keyFrameCallback(_keyframe_ptr); -} - -bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const -{ - // This should implement a black list of processors that have forbidden auxiliary frame creation - // This decision is made at problem level, not at processor configuration level. - // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors. - // Currently allowing all processors to vote: - return true; -} - -void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance) -{ - // TODO -// if (_processor_ptr) -// { -// WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp()); -// } -// else -// { -// WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp()); -// } -// -// processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance); + // resume processor profiling + if (_processor_ptr) + _processor_ptr->startCaptureProfiling(); } StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti) @@ -876,15 +855,9 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& return success; } -bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const +bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const { - FrameBasePtr frm = getLastKeyFrame(); - return getFrameCovariance(frm, cov); -} - -bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const -{ - FrameBasePtr frm = getLastKeyOrAuxFrame(); + FrameBasePtr frm = getLastFrame(); return getFrameCovariance(frm, cov); } @@ -968,24 +941,9 @@ FrameBasePtr Problem::getLastFrame() const return trajectory_ptr_->getLastFrame(); } -FrameBasePtr Problem::getLastKeyFrame() const -{ - return trajectory_ptr_->getLastKeyFrame(); -} - -FrameBasePtr Problem::getLastKeyOrAuxFrame() const -{ - return trajectory_ptr_->getLastKeyOrAuxFrame(); -} - -FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const +FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const { - return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); -} - -FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const -{ - return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); + return trajectory_ptr_->closestFrameToTimeStamp(_ts); } void Problem::setPriorOptions(const std::string& _mode, @@ -1041,9 +999,8 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) if (prior_options_->mode != "nothing" and prior_options_->mode != "") { - prior_keyframe = emplaceFrame(KEY, - _ts, - prior_options_->state); + prior_keyframe = emplaceFrame(_ts, + prior_options_->state); if (prior_options_->mode == "fix") prior_keyframe->fix(); @@ -1067,7 +1024,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size"); // feature - auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + key, state_blk, covar_blk); + auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), state_blk, covar_blk); // factor if (sb->hasLocalParametrization()) @@ -1203,14 +1160,11 @@ void Problem::perturb(double amplitude) S->perturb(amplitude); // Frames and Captures - for (auto& F : getTrajectory()->getFrameList()) + for (auto& F : *(getTrajectory())) { - if (F->isKeyOrAux()) - { - F->perturb(amplitude); - for (auto& C : F->getCaptureList()) - C->perturb(amplitude); - } + F->perturb(amplitude); + for (auto& C : F->getCaptureList()) + C->perturb(amplitude); } // Landmarks diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp index 1bfd06a5e9669f76ef48c49a559cade0dc7845f0..a17142e80bf69198674b29f0dc5f3bde955412a4 100644 --- a/src/processor/is_motion.cpp +++ b/src/processor/is_motion.cpp @@ -5,5 +5,6 @@ using namespace wolf; void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr) { + setOdometry(_prb_ptr->stateZero(state_structure_)); _prb_ptr->addProcessorIsMotion(_motion_ptr); } diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index c02228864e1db24fb0b43826736a26c1c6688451..e8f891b8c51098909b5db479f7e3d3a8139457c0 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -12,7 +12,13 @@ ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ParamsProcessor processor_id_(++processor_id_count_), params_(_params), dim_(_dim), - sensor_ptr_() + sensor_ptr_(), + n_capture_callback_(0), + n_kf_callback_(0), + acc_duration_capture_(0), + acc_duration_kf_(0), + max_duration_capture_(0), + max_duration_kf_(0) { // WOLF_DEBUG("constructed +p" , id()); } @@ -27,16 +33,14 @@ bool ProcessorBase::permittedKeyFrame() return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this()); } -bool ProcessorBase::permittedAuxFrame() -{ - return isVotingAuxActive() && getProblem()->permitAuxFrame(shared_from_this()); -} - void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) { assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame"); WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp()); + // profiling + n_kf_callback_++; + startKFProfiling(); // asking if key frame should be stored if (storeKeyFrame(_keyframe_ptr)) @@ -46,6 +50,8 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _ if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other)) processKeyFrame(_keyframe_ptr, _time_tol_other); + // profiling + stopKFProfiling(); } void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) @@ -53,6 +59,10 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture"); WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp()); + // profiling + n_capture_callback_++; + startCaptureProfiling(); + // apply prior in problem if not done (very first capture) if (getProblem() && !getProblem()->isPriorSet()) getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp()); @@ -64,6 +74,9 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) // if trigger, process directly without buffering if (triggerInCapture(_capture_ptr)) processCapture(_capture_ptr); + + // profiling + stopCaptureProfiling(); } void ProcessorBase::remove() @@ -101,7 +114,7 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr) } else { - WOLF_WARN("Linking with a nullptr"); + WOLF_WARN("Linking Processor ", id(), " to a nullptr"); } } @@ -118,7 +131,7 @@ void ProcessorBase::setProblem(ProblemPtr _problem) // adding processor is motion to the processor is motion vector auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this()); if (is_motion_ptr) - getProblem()->addProcessorIsMotion(is_motion_ptr); + is_motion_ptr->addToProblem(_problem, is_motion_ptr); } ///////////////////////////////////////////////////////////////////////////////////////// @@ -220,7 +233,6 @@ void BufferPackKeyFrame::print(void) const void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { _stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; - } void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const @@ -261,4 +273,22 @@ bool ProcessorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, b _log.compose(local_log); return _log.is_consistent_; } + +void ProcessorBase::printProfiling(std::ostream& _stream) const +{ + _stream << "\n" << getType() << " - " << getName() << ":" + << "\n\ttotal time: " << 1e-6*(acc_duration_capture_ + acc_duration_kf_).count()<< " s" + << "\n\tProcessing captures:" + << "\n\t\ttotal time: " << 1e-6*acc_duration_capture_.count() << " s" + << "\n\t\tcaptures processed: " << n_capture_callback_ + << "\n\t\taverage time: " << 1e-3*acc_duration_capture_.count() / n_capture_callback_ << " ms" + << "\n\t\tmax time: " << 1e-3*max_duration_capture_.count() << " ms" + << "\n\tProcessing keyframes:" + << "\n\t\ttotal time: " << 1e-6*acc_duration_kf_.count() << " s" + << "\n\t\tkf processed: " << n_kf_callback_ + << "\n\t\taverage time: " << 1e-3*acc_duration_kf_.count() / n_kf_callback_ << " ms" + << "\n\t\tmax time: " << 1e-3*max_duration_kf_.count() << " ms" << std::endl; + +} + } // namespace wolf diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index d47dbbad36a9328df374b4b4d08d453db3eae6e1..64536a9195540dedbdb39405e50a658fec3412c0 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -23,6 +23,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, SizeEigen _calib_size, ParamsProcessorMotionPtr _params_motion) : ProcessorBase(_type, _dim, _params_motion), + IsMotion(_state_structure), params_motion_(_params_motion), processing_step_(RUNNING_WITHOUT_KF), x_size_(_state_size), @@ -33,7 +34,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, origin_ptr_(), last_ptr_(), incoming_ptr_(), - dt_(0.0), + last_frame_ptr_(), + dt_(0.0), x_(_state_size), delta_(_delta_size), delta_cov_(_delta_cov_size, _delta_cov_size), @@ -44,7 +46,6 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, jacobian_delta_(delta_cov_size_, delta_cov_size_), jacobian_calib_(delta_cov_size_, calib_size_) { - setStateStructure(_state_structure); // } @@ -53,10 +54,45 @@ ProcessorMotion::~ProcessorMotion() // std::cout << "destructed -p-Mot" << id() << std::endl; } -void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source, + +void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev, + CaptureMotionPtr cap_target) +{ + assert(cap_prev == cap_target->getOriginCapture() && "merging not consecutive capture motions"); + + // add prev buffer (discarding the first zero motion) + cap_target->getBuffer().pop_front(); + cap_target->getBuffer().insert(cap_target->getBuffer().begin(), + cap_prev->getBuffer().begin(), + cap_prev->getBuffer().end()); + + // change origin + cap_target->setOriginCapture(cap_prev->getOriginCapture()); + + // change calibration params for preintegration from origin + cap_target->setCalibrationPreint(getCalibration(cap_prev->getOriginCapture())); + + // reintegrate buffer + reintegrateBuffer(cap_target); + + // replace existing feature and factor (if they exist) + if (!cap_target->getFeatureList().empty()) + { + // remove feature and factor (automatically) + cap_target->getFeatureList().back()->remove(); + + assert(cap_target->getFeatureList().empty());// there should be one feature only + + // emplace new feature and factor + auto new_feature = emplaceFeature(cap_target); + emplaceFactor(new_feature, cap_prev->getOriginCapture()); + } +} + +void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source, TimeStamp _ts_split, const FrameBasePtr& _keyframe_target, - const wolf::CaptureMotionPtr& _capture_target) + const CaptureMotionPtr& _capture_target) const { /** we are doing this: * @@ -400,10 +436,12 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) * */ + // Set the capture's calibration with best results available (in case origin_ptr_ received a solve()) + setCalibration(last_ptr_, getCalibration(origin_ptr_)); // Set the frame of last_ptr as key auto key_frame = last_ptr_->getFrame(); - key_frame ->setKey(); + key_frame ->link(getProblem()); // create motion feature and add it to the key_capture auto key_feature = emplaceFeature(last_ptr_); @@ -412,18 +450,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) auto factor = emplaceFactor(key_feature, origin_ptr_); // create a new frame - auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED, - getTimeStamp(), - getStateStructure(), - getProblem()->getState()); + auto frame_new = std::make_shared<FrameBase>(getTimeStamp(), + getStateStructure(), + getProblem()->getState()); // create a new capture auto capture_new = emplaceCapture(frame_new, getSensor(), key_frame->getTimeStamp(), Eigen::VectorXd::Zero(data_size_), getSensor()->getNoiseCov(), - getCalibration(last_ptr_), - getCalibration(last_ptr_), + getCalibration(origin_ptr_), + getCalibration(origin_ptr_), last_ptr_); // reset the new buffer capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ; @@ -434,15 +471,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // Reset pointers origin_ptr_ = last_ptr_; last_ptr_ = capture_new; + last_frame_ptr_ = frame_new; // callback to other processors getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance); + } // clear incoming just in case incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer. postProcess(); + } VectorComposite ProcessorMotion::getState(const StateStructure& _structure) const @@ -640,10 +680,10 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin) { - FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, - _ts_origin, - getStateStructure(), - _x_origin); + FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(), + _ts_origin, + getStateStructure(), + _x_origin); setOrigin(key_frame_ptr); return key_frame_ptr; @@ -654,7 +694,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr."); assert(_origin_frame->getTrajectory() != nullptr && "ProcessorMotion::setOrigin: origin frame must be in the trajectory."); - assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME."); TimeStamp origin_ts = _origin_frame->getTimeStamp(); @@ -665,25 +704,24 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) origin_ts, Eigen::VectorXd::Zero(data_size_), getSensor()->getNoiseCov(), - getSensor()->getCalibration(), - getSensor()->getCalibration(), + getCalibration(), + getCalibration(), nullptr); // ---------- LAST ---------- // Make non-key-frame for last Capture - auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED, - origin_ts, - getStateStructure(), - _origin_frame->getState()); + last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts, + getStateStructure(), + _origin_frame->getState()); // emplace (emtpy) last Capture - last_ptr_ = emplaceCapture(new_frame_ptr, + last_ptr_ = emplaceCapture(last_frame_ptr_, getSensor(), origin_ts, Eigen::VectorXd::Zero(data_size_), getSensor()->getNoiseCov(), - getSensor()->getCalibration(), - getSensor()->getCalibration(), + getCalibration(), + getCalibration(), origin_ptr_); // clear and reset buffer @@ -743,9 +781,12 @@ void ProcessorMotion::integrateOneStep() jacobian_delta_, jacobian_delta_preint_, jacobian_calib_); + + // integrate odometry + statePlusDelta(odometry_, delta_, dt_, odometry_); } -void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) +void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const { VectorXd calib = _capture_ptr->getCalibrationPreint(); @@ -808,6 +849,19 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const { assert(_ts.ok()); + // assert(last_ptr_ != nullptr); + + //Need to uncomment this line so that wolf_ros_apriltag doesn't crash + if(last_ptr_ == nullptr) return nullptr; + + // First check if last_ptr is the one we are looking for + if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance)) + return last_ptr_; + + + // Then look in the Wolf tree... + // ----------------------------- + // // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp // Note: since the buffer goes from a KF in the past until the next KF, we need to: @@ -817,8 +871,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp FrameBasePtr frame = nullptr; CaptureBasePtr capture = nullptr; CaptureMotionPtr capture_motion = nullptr; - for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin(); - frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend(); + for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin(); + frame_rev_iter != getProblem()->getTrajectory()->rend(); ++frame_rev_iter) { frame = *frame_rev_iter; @@ -923,7 +977,7 @@ TimeStamp ProcessorMotion::getTimeStamp ( ) const not last_ptr_) { WOLF_DEBUG("Processor has no time stamp. Returning a non-valid timestamp equal to 0"); - return TimeStamp(0); + return TimeStamp::Invalid(); } if (getBuffer().empty()) @@ -936,10 +990,10 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo { _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; if (getOrigin()) - _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm") + _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm" << getOrigin()->getFrame()->id() << std::endl; if (getLast()) - _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm") + _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm" << getLast()->getFrame()->id() << std::endl; if (getIncoming()) _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl; diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index 284090e238f3c75327424cb76f3946e277e23bdf..2da7c330b59ffce0c7eceb11515e4f992a9eb3d9 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -93,8 +93,8 @@ void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x, assert(_delta.size() == delta_size_ && "Wrong _x_plus_delta vector size"); // This is just a frame composition in 2d - _x_plus_delta["P"] = _x.at("P") + Eigen::Rotation2Dd(_x.at("O")(0)).matrix() * _delta.head<2>(); - _x_plus_delta["O"] = Vector1d(pi2pi(_x.at("O")(0) + _delta(2))); + _x_plus_delta['P'] = _x.at('P') + Eigen::Rotation2Dd(_x.at('O')(0)).matrix() * _delta.head<2>(); + _x_plus_delta['O'] = Vector1d(pi2pi(_x.at('O')(0) + _delta(2))); } bool ProcessorOdom2d::voteForKeyFrame() const diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index 5b01e98b3ad8fbbe6f31bedc4f25b1a9cfe76747..c7be4d48c4d086d5d4f594dfa7849419d8cc5c30 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -13,7 +13,11 @@ ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) : min_disp_var_ (0.1), // around 10cm error min_rot_var_ (0.1) // around 6 degrees error { - // + // Set constant parts of Jacobians + jacobian_delta_preint_.setIdentity(6,6); + jacobian_delta_.setIdentity(6,6); + jacobian_calib_.setZero(6,0); + unmeasured_perturbation_cov_ = pow(params_odom_3d_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix6d::Identity(); } ProcessorOdom3d::~ProcessorOdom3d() @@ -147,13 +151,13 @@ void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x, { assert(_delta.size() == delta_size_ && "Wrong _delta vector size"); - Eigen::Map<const Eigen::Vector3d> p (_x.at("P").data() ); - Eigen::Map<const Eigen::Quaterniond> q (_x.at("O").data() ); + Eigen::Map<const Eigen::Vector3d> p (_x.at('P').data() ); + Eigen::Map<const Eigen::Quaterniond> q (_x.at('O').data() ); Eigen::Map<const Eigen::Vector3d> dp (_delta.data() ); Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 ); - _x_plus_delta["P"] = p + q * dp; - _x_plus_delta["O"] = (q * dq).coeffs(); + _x_plus_delta['P'] = p + q * dp; + _x_plus_delta['O'] = (q * dq).coeffs(); } diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index ef801eb6d525375272fd95d89a50eec9bed4dc1e..52421e86398bc049321e8b3dc5f75dc56d2d3001 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -24,6 +24,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type, origin_ptr_(nullptr), last_ptr_(nullptr), incoming_ptr_(nullptr), + last_frame_ptr_(nullptr), state_structure_(_structure) { // @@ -80,10 +81,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) { WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" ); - FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, - incoming_ptr_->getTimeStamp(), - getStateStructure(), - getProblem()->getState(getStateStructure())); + FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(), + incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + getProblem()->getState()); incoming_ptr_->link(kfrm); // Process info @@ -113,10 +114,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) { WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" ); - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp()); + FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + getProblem()->getState()); incoming_ptr_->link(frm); - // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF. // Process info @@ -130,6 +131,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) resetDerived(); origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; + last_frame_ptr_ = frm; incoming_ptr_ = nullptr; break; @@ -149,8 +151,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) last_old_frame->remove(); // Create new frame - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp()); + FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + getProblem()->getState()); incoming_ptr_->link(frm); // Detect new Features, initialize Landmarks, create Factors, ... @@ -163,6 +166,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) resetDerived(); origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; + last_frame_ptr_ = frm; incoming_ptr_ = nullptr; break; @@ -183,10 +187,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // We create a KF // set KF on last last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); - last_ptr_->getFrame()->setKey(); + last_ptr_->getFrame()->link(getProblem()); // // make F; append incoming to new F - // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); + // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp()); // incoming_ptr_->link(frm); // Establish factors @@ -199,47 +203,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) resetDerived(); // make F; append incoming to new F - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp(), - last_ptr_->getFrame()->getState()); + FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + last_ptr_->getFrame()->getState()); incoming_ptr_->link(frm); - origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; + last_frame_ptr_ = frm; incoming_ptr_ = nullptr; } - /* TODO: create an auxiliary frame - else if (voteForAuxFrame() && permittedAuxFrame()) - { - // We create an Auxiliary Frame - - // set AuxF on last - last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); - last_ptr_->getFrame()->setAuxiliary(); - - // make F; append incoming to new F - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); - incoming_ptr_->link(frm); - - // Establish factors - establishFactors(); - - // Call the new auxframe callback in order to let the ProcessorMotion to establish their factors - getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance); - - // Advance this - advanceDerived(); - - // Replace last frame for a new one in incoming - last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame - // do not remove! last_ptr_->remove(); - incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp()); - - // Update pointers - last_ptr_ = incoming_ptr_; - incoming_ptr_ = nullptr; - }*/ else { // We do not create a KF @@ -248,14 +221,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) advanceDerived(); // Replace last frame for a new one in incoming - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp(), - last_ptr_->getFrame()->getState()); + FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + last_ptr_->getFrame()->getState()); incoming_ptr_->link(frm); last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove(); // Update pointers last_ptr_ = incoming_ptr_; + last_frame_ptr_ = frm; incoming_ptr_ = nullptr; } break; @@ -302,11 +276,11 @@ void ProcessorTracker::computeProcessingStep() if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance)) { - if (last_ptr_->getFrame()->isKey()) + if (last_ptr_->getFrame()->getProblem()) { WOLF_WARN("||*||"); WOLF_INFO(" ... It seems you missed something!"); - WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)"); + WOLF_INFO("Pack's KF and last's Frame have matching time stamps (i.e. below time tolerances)"); WOLF_INFO("Check the following for correctness:"); WOLF_INFO(" - You have all processors installed before starting receiving any data"); WOLF_INFO(" - You have configured all your processors with compatible time tolerances"); @@ -337,10 +311,10 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo { _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; if (getOrigin()) - _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm") + _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm" << getOrigin()->getFrame()->id() << std::endl; if (getLast()) - _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm") + _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm" << getLast()->getFrame()->id() << std::endl; if (getIncoming()) _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl; diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index 3804962f62fcbfba13b56e7c64866be7497eeaa8..cda95e5ed6259e7ef11d94d01c3fe939ffe61941 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -91,7 +91,7 @@ unsigned int ProcessorTrackerFeature::processKnown() if (known_features_last_.empty()) { - WOLF_TRACE("Empty last feature list, returning..."); + WOLF_DEBUG("Empty last feature list, returning..."); return 0; } diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp index 6d7dd0f50da30dc79e3c5c9cca980bc3364a0349..da6a88b75e98e0ada648f76155f5d12501c21c4d 100644 --- a/src/processor/track_matrix.cpp +++ b/src/processor/track_matrix.cpp @@ -214,7 +214,7 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id) const { auto& ts = pair_ts_ftr.first; auto& ftr = pair_ts_ftr.second; - if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->isKey()) + if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem()) track_kf[ts] = ftr; } return track_kf; diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index f8c3c76c7e5865d241846516ce9989226d9985cd..0d2d54638d145f333efa0e349990beda54d1b17e 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -20,10 +20,10 @@ SensorBase::SensorBase(const std::string& _type, NodeBase("SENSOR", _type), HasStateBlocks(""), hardware_ptr_(), - calib_size_(0), sensor_id_(++sensor_id_count_), // simple ID factory noise_std_(_noise_size), - noise_cov_(_noise_size, _noise_size) + noise_cov_(_noise_size, _noise_size), + last_capture_(nullptr) { assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway."); assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway."); @@ -33,15 +33,14 @@ SensorBase::SensorBase(const std::string& _type, noise_cov_.setZero(); if (_p_ptr) - addStateBlock("P", _p_ptr, _p_dyn); + addStateBlock('P', _p_ptr, _p_dyn); if (_o_ptr) - addStateBlock("O", _o_ptr, _o_dyn); + addStateBlock('O', _o_ptr, _o_dyn); if (_intr_ptr) - addStateBlock("I", _intr_ptr, _intr_dyn); + addStateBlock('I', _intr_ptr, _intr_dyn); - updateCalibSize(); } SensorBase::SensorBase(const std::string& _type, @@ -55,23 +54,21 @@ SensorBase::SensorBase(const std::string& _type, NodeBase("SENSOR", _type), HasStateBlocks(""), hardware_ptr_(), - calib_size_(0), sensor_id_(++sensor_id_count_), // simple ID factory noise_std_(_noise_std), - noise_cov_(_noise_std.size(), _noise_std.size()) + noise_cov_(_noise_std.size(), _noise_std.size()), + last_capture_(nullptr) { setNoiseStd(_noise_std); if (_p_ptr) - addStateBlock("P", _p_ptr, _p_dyn); + addStateBlock('P', _p_ptr, _p_dyn); if (_o_ptr) - addStateBlock("O", _o_ptr, _o_dyn); + addStateBlock('O', _o_ptr, _o_dyn); if (_intr_ptr) - addStateBlock("I", _intr_ptr, _intr_dyn); - - updateCalibSize(); + addStateBlock('I', _intr_ptr, _intr_dyn); } SensorBase::~SensorBase() @@ -82,9 +79,8 @@ SensorBase::~SensorBase() void SensorBase::removeStateBlocks() { - for (const auto& _key : getStructure()) + for (const auto& key : getStructure()) { - auto key = std::string(1,_key); auto sbp = getStateBlock(key); if (sbp != nullptr) { @@ -105,7 +101,6 @@ void SensorBase::fixExtrinsics() if (sbp != nullptr) sbp->fix(); } - updateCalibSize(); } void SensorBase::unfixExtrinsics() @@ -116,40 +111,35 @@ void SensorBase::unfixExtrinsics() if (sbp != nullptr) sbp->unfix(); } - updateCalibSize(); } void SensorBase::fixIntrinsics() { - for (const auto& _key : getStructure()) + for (const auto& key : getStructure()) { - auto key = std::string(1,_key); - if (key != "P" and key != "O") // exclude extrinsics + if (key != 'P' and key != 'O') // exclude extrinsics { auto sbp = getStateBlockDynamic(key); if (sbp != nullptr) sbp->fix(); } } - updateCalibSize(); } void SensorBase::unfixIntrinsics() { - for (const auto& _key : getStructure()) + for (const auto& key : getStructure()) { - auto key = std::string(1,_key); - if (key != "P" and key != "O") // exclude extrinsics + if (key != 'P' and key != 'O') // exclude extrinsics { auto sbp = getStateBlockDynamic(key); if (sbp != nullptr) sbp->unfix(); } } - updateCalibSize(); } -void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) +void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) { assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters"); @@ -201,7 +191,7 @@ void SensorBase::registerNewStateBlocks() const auto key = pair_key_sbp.first; auto sbp = pair_key_sbp.second; - if (sbp && !isStateBlockInCapture(key)) + if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key)) getProblem()->notifyStateBlock(sbp, ADD); } } @@ -217,131 +207,94 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) { noise_cov_ = _noise_cov; } -CaptureBasePtr SensorBase::lastCapture(void) const +void SensorBase::setLastCapture(CaptureBasePtr cap) { - // we search for the most recent Capture of this sensor which belongs to a KeyFrame - CaptureBasePtr capture = nullptr; - if (getProblem()) - { - FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList(); - FrameBaseRevIter frame_rev_it = frame_list.rbegin(); - while (frame_rev_it != frame_list.rend()) - { - capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); - if (capture) - // found the most recent Capture made by this sensor ! - break; - frame_rev_it++; - } - } - return capture; + assert(cap); + assert(cap->getSensor() == shared_from_this()); + assert(cap->getTimeStamp().ok()); + assert(not last_capture_ or last_capture_->getTimeStamp() < cap->getTimeStamp()); + last_capture_ = cap; } -CaptureBasePtr SensorBase::lastKeyCapture(void) const +void SensorBase::updateLastCapture() { // we search for the most recent Capture of this sensor which belongs to a KeyFrame - CaptureBasePtr capture = nullptr; if (getProblem()) { - FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList(); - FrameBaseRevIter frame_rev_it = frame_list.rbegin(); - while (frame_rev_it != frame_list.rend()) + // auto frame_list = getProblem()->getTrajectory()->getFrameMap(); + auto trajectory = getProblem()->getTrajectory(); + TrajectoryRevIter frame_rev_it = trajectory->rbegin(); + while (frame_rev_it != trajectory->rend()) { - if ((*frame_rev_it)->isKey()) + auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); + if (capture and not capture->isRemoving()) { - capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); - if (capture) - // found the most recent Capture made by this sensor ! - break; + // found the most recent Capture made by this sensor ! + last_capture_ = capture; + return; } frame_rev_it++; } } - return capture; + + // no captures found + last_capture_ = nullptr; } -CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const +CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const { // we search for the most recent Capture of this sensor before _ts - CaptureBasePtr capture = nullptr; - if (getProblem()) - { - FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList(); + if (not getProblem()) + return nullptr; - // We iterate in reverse since we're likely to find it close to the rbegin() place. - FrameBaseRevIter frame_rev_it = frame_list.rbegin(); - while (frame_rev_it != frame_list.rend()) + // auto frame_list = getProblem()->getTrajectory()->getFrameMap(); + auto trajectory = getProblem()->getTrajectory(); + + // We iterate in reverse since we're likely to find it close to the rbegin() place. + TrajectoryRevIter frame_rev_it = trajectory->rbegin(); + while (frame_rev_it != trajectory->rend()) + { + if ((*frame_rev_it)->getTimeStamp() <= _ts) { - if ((*frame_rev_it)->getTimeStamp() <= _ts) - { - capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); - if (capture) - // found the most recent Capture made by this sensor ! - break; - } - frame_rev_it++; + auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); + if (capture) + // found the most recent Capture made by this sensor ! + return capture; } + frame_rev_it++; } - return capture; + + return nullptr; } StateBlockPtr SensorBase::getP(const TimeStamp _ts) const { - return getStateBlockDynamic("P", _ts); + return getStateBlockDynamic('P', _ts); } StateBlockPtr SensorBase::getO(const TimeStamp _ts) const { - return getStateBlockDynamic("O", _ts); + return getStateBlockDynamic('O', _ts); } StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const { - return getStateBlockDynamic("I", _ts); + return getStateBlockDynamic('I', _ts); } StateBlockPtr SensorBase::getP() const { - return getStateBlockDynamic("P"); + return getStateBlockDynamic('P'); } StateBlockPtr SensorBase::getO() const { - return getStateBlockDynamic("O"); + return getStateBlockDynamic('O'); } StateBlockPtr SensorBase::getIntrinsic() const { - return getStateBlockDynamic("I"); -} - -SizeEigen SensorBase::computeCalibSize() const -{ - SizeEigen sz = 0; - for (const auto& pair_key_sb : getStateBlockMap()) - { - auto sb = pair_key_sb.second; - if (sb && !sb->isFixed()) - sz += sb->getSize(); - } - return sz; -} - -Eigen::VectorXd SensorBase::getCalibration() const -{ - SizeEigen index = 0; - SizeEigen sz = getCalibSize(); - Eigen::VectorXd calib(sz); - for (const auto& key : getStructure()) - { - auto sb = getStateBlockDynamic(key); - if (sb && !sb->isFixed()) - { - calib.segment(index, sb->getSize()) = sb->getState(); - index += sb->getSize(); - } - } - return calib; + return getStateBlockDynamic('I'); } bool SensorBase::process(const CaptureBasePtr capture_ptr) @@ -382,7 +335,7 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr) processor_list_.remove(_proc_ptr); } -StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key) const +StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const { CaptureBasePtr cap; @@ -392,7 +345,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key) const return getStateBlock(_key); } -StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const +StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const { CaptureBasePtr cap; @@ -402,12 +355,11 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const Ti return getStateBlock(_key); } -bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const +bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const { if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key)) { - _cap = lastCapture(); - // cap = lastKeyCapture(); + _cap = last_capture_; return _cap != nullptr; } @@ -415,11 +367,11 @@ bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& return false; } -bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const +bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const { if (isStateBlockDynamic(_key)) { - _cap = lastCapture(_ts); + _cap = findLastCaptureBefore(_ts); return _cap != nullptr; } @@ -427,14 +379,14 @@ bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& return false; } -bool SensorBase::isStateBlockInCapture(const std::string& _key) const +bool SensorBase::isStateBlockInCapture(const char& _key) const { CaptureBasePtr cap; return isStateBlockInCapture(_key, cap); } -bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const +bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const { CaptureBasePtr cap; @@ -463,7 +415,7 @@ void SensorBase::link(HardwareBasePtr _hw_ptr) } else { - WOLF_WARN("Linking with a nullptr"); + WOLF_WARN("Linking Sensor ", id(), " to a nullptr"); } } @@ -474,39 +426,7 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st _stream << " -- " << getProcessorList().size() << "p"; _stream << std::endl; - if (_metric && _state_blocks) - { - _stream << _tabs << " " << "sb: "; - for (auto& _key : getStructure()) - { - auto key = std::string(1,_key); - auto sb = getStateBlock(key); - _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); "; - } - _stream << std::endl; - } - else if (_metric) - { - _stream << _tabs << " " << "( "; - for (auto& _key : getStructure()) - { - auto key = std::string(1,_key); - auto sb = getStateBlock(key); - _stream << sb->getState().transpose() << " "; - } - _stream << ")" << std::endl; - } - else if (_state_blocks) - { - _stream << _tabs << " " << "sb: "; - for (auto& _key : getStructure()) - { - auto key = std::string(1,_key); - auto sb = getStateBlock(key); - _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; "; - } - _stream << std::endl; - } + printState(_metric, _state_blocks, _stream, _tabs); } void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const @@ -515,7 +435,8 @@ void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl if (_depth >= 2) for (auto p : getProcessorList()) - p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); + if (p) + p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs) const @@ -561,6 +482,20 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr << " -X-> Sen" << id(); log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation); } + // check last_capture_ + if (getProblem()->getTimeStamp().ok()) + { + auto last_capture_found = findLastCaptureBefore(getProblem()->getTimeStamp()); + inconsistency_explanation << "SensorBase::last_capture_: " + << (last_capture_ ? std::to_string(last_capture_->id()) : "-") + << " @ " << last_capture_ + << " is not the actual last capture: " + << (last_capture_found ? + std::to_string(last_capture_found->id()) : + "-") + << " @ " << last_capture_found << std::endl; + log.assertTrue(last_capture_ == last_capture_found, inconsistency_explanation); + } return log; } diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 24a674f5f517e7bbd5ac8448c8678a7517632b90..99dfb8729c9bc5cb05f00758494571de9d52fc23 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -5,10 +5,29 @@ namespace wolf { -SolverManager::SolverManager(const ProblemPtr& _wolf_problem) : - wolf_problem_(_wolf_problem) +SolverManager::SolverManager(const ProblemPtr& _problem) : + SolverManager(_problem, std::make_shared<ParamsSolver>()) { - assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr."); +} + +SolverManager::SolverManager(const ProblemPtr& _problem, + const ParamsSolverPtr& _params) : + n_solve_(0), + n_cov_(0), + acc_duration_total_(0), + acc_duration_solver_(0), + acc_duration_update_(0), + acc_duration_state_(0), + acc_duration_cov_(0), + max_duration_total_(0), + max_duration_solver_(0), + max_duration_update_(0), + max_duration_state_(0), + max_duration_cov_(0), + wolf_problem_(_problem), + params_(_params) +{ + assert(_problem != nullptr && "Passed a nullptr ProblemPtr."); } SolverManager::~SolverManager() @@ -68,10 +87,19 @@ void SolverManager::update() } // UPDATE STATE BLOCKS (state, fix or local parameterization) + std::set<StateBlockPtr> new_floating_state_blocks; for (auto state_pair : state_blocks_) { auto state_ptr = state_pair.first; + // Check for "floating" state blocks (not involved in any factor -> not observable problem) + if (state_blocks_2_factors_.at(state_ptr).empty()) + { + WOLF_DEBUG("SolverManager::update(): StateBlock ", state_ptr, " is 'Floating' (not involved in any factor). Storing it apart."); + new_floating_state_blocks.insert(state_ptr); + continue; + } + // state update if (state_ptr->stateUpdated()) updateStateBlockState(state_ptr); @@ -85,6 +113,20 @@ void SolverManager::update() updateStateBlockLocalParametrization(state_ptr); } + // REMOVE NEWLY DETECTED "floating" STATE BLOCKS (will be added next update() call) + for (auto state_ptr : new_floating_state_blocks) + { + removeStateBlock(state_ptr); + floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()! + } + // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be set in addStateBlock) + for (auto state_ptr : floating_state_blocks_) + { + state_ptr->resetStateUpdated(); + state_ptr->resetFixUpdated(); + state_ptr->resetLocalParamUpdated(); + } + #ifdef _WOLF_DEBUG assert(check("after update()")); #endif @@ -95,14 +137,32 @@ wolf::ProblemPtr SolverManager::getProblem() return wolf_problem_; } +std::string SolverManager::solve() +{ + return solve(params_->verbose); +} + std::string SolverManager::solve(const ReportVerbosity report_level) { + auto start_total = std::chrono::high_resolution_clock::now(); + n_solve_++; + // update problem + auto start_update = std::chrono::high_resolution_clock::now(); update(); + auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_update); + acc_duration_update_ += duration_update; + max_duration_update_ = std::max(max_duration_update_,duration_update); + // call derived solver + auto start_solver = std::chrono::high_resolution_clock::now(); std::string report = solveDerived(report_level); + auto duration_solver = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_solver); + acc_duration_solver_ += duration_solver; + max_duration_solver_ = std::max(max_duration_solver_,duration_solver); // update StateBlocks with optimized state value. + auto start_state = std::chrono::high_resolution_clock::now(); /// @todo whatif someone has changed the state notification during opti ?? /// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one. @@ -114,16 +174,52 @@ std::string SolverManager::solve(const ReportVerbosity report_level) if (!stateblock_statevector.first->isFixed()) stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_ } + auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state); + acc_duration_state_ += duration_state; + max_duration_state_ = std::max(max_duration_state_,duration_state); + + auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total); + acc_duration_total_ += duration_total; + max_duration_total_ = std::max(max_duration_total_,duration_total); return report; } + +bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks) +{ + auto start_cov = std::chrono::high_resolution_clock::now(); + n_cov_++; + + auto ret = computeCovariancesDerived(blocks); + + auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov); + acc_duration_cov_ += duration_cov; + max_duration_cov_ = std::max(max_duration_cov_,duration_cov); + + return ret; +} + +bool SolverManager::computeCovariances(const std::vector<StateBlockPtr>& st_list) +{ + auto start_cov = std::chrono::high_resolution_clock::now(); + n_cov_++; + + auto ret = computeCovariancesDerived(st_list); + + auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov); + acc_duration_cov_ += duration_cov; + max_duration_cov_ = std::max(max_duration_cov_,duration_cov); + + return ret; +} + void SolverManager::addFactor(const FactorBasePtr& fac_ptr) { // Warning if adding an already added if (factors_.count(fac_ptr) != 0) { - WOLF_WARN("Tried to add a factor that was already added !"); + WOLF_WARN("Tried to add the factor ", fac_ptr->id(), " that was already added !"); return; } @@ -137,10 +233,20 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr) for (const auto& st : fac_ptr->getStateBlockPtrVector()) if (state_blocks_.count(st) == 0) { - WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later."); - // put back notification in problem (will be added next update() call) and do nothing - wolf_problem_->notifyFactor(fac_ptr, ADD); - return; + // Check if it was stored as a 'floating' state block + if (floating_state_blocks_.count(st) == 1) + { + WOLF_DEBUG("SolverManager::addFactor(): Factor ", fac_ptr->id(), " involves state block ", st, " stored as 'floating'. Adding the state block to the solver."); + floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()! + addStateBlock(st); + } + else + { + WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later."); + // put back notification in problem (will be added next update() call) and do nothing + wolf_problem_->notifyFactor(fac_ptr, ADD); + return; + } } // factors @@ -163,7 +269,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr) // Warning if removing a missing factor if (factors_.count(fac_ptr) == 0) { - WOLF_WARN("Tried to remove a factor that is missing !"); + WOLF_WARN("Tried to remove factor ", fac_ptr->id(), " that is missing !"); return; } @@ -187,12 +293,19 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr) // Warning if adding an already added state block if (state_blocks_.count(state_ptr) != 0) { - WOLF_WARN("Tried to add a StateBlock that was already added !"); + WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that was already added !"); + return; + } + // Warning if adding a floating state block + if (floating_state_blocks_.count(state_ptr) != 0) + { + WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that is stored as floating !"); return; } assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added"); assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added"); + assert(state_ptr->isValid() && "SolverManager::addStateBlock state block state not valid (local parameterization)"); // stateblock maps state_blocks_.emplace(state_ptr, state_ptr->getState()); @@ -209,10 +322,16 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr) void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr) { + // check if stored as 'floating' state block + if (floating_state_blocks_.count(state_ptr) == 1) + { + floating_state_blocks_.erase(state_ptr); + return; + } // Warning if removing a missing state block if (state_blocks_.count(state_ptr) == 0) { - WOLF_WARN("Tried to remove a StateBlock that was not added !"); + WOLF_WARN("Tried to remove the StateBlock ", state_ptr, " that was not added !"); return; } @@ -257,8 +376,12 @@ void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr) void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr) { + assert(state_ptr && "SolverManager::updateStateBlockState null state block"); + assert(state_blocks_.count(state_ptr) == 1 && "SolverManager::updateStateBlockState unregistered state block"); + assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)"); + assert(state_ptr->getState().size() == getAssociatedMemBlock(state_ptr).size()); + Eigen::VectorXd new_state = state_ptr->getState(); - // We assume the same size for the states in both WOLF and the solver. std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr)); // reset flag state_ptr->resetStateUpdated(); @@ -278,8 +401,10 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state auto it = state_blocks_.find(state_ptr); if (it == state_blocks_.end()) + { + WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr); throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !"); - + } return it->second; } @@ -288,8 +413,10 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state auto it = state_blocks_.find(state_ptr); if (it == state_blocks_.end()) - throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !"); - + { + WOLF_ERROR("Tried to retrieve the memory block const ptr of an unregistered StateBlock: ", state_ptr); + throw std::runtime_error("Tried to retrieve the memory block const ptr of an unregistered StateBlock !"); + } return it->second.data(); } @@ -298,21 +425,85 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) auto it = state_blocks_.find(state_ptr); if (it == state_blocks_.end()) - throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !"); - + { + WOLF_ERROR("Tried to retrieve the memory block ptr of an unregistered StateBlock: ", state_ptr); + throw std::runtime_error("Tried to retrieve the memory block ptr of an unregistered StateBlock !"); + } return it->second.data(); } -bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) +SolverManager::ReportVerbosity SolverManager::getVerbosity() const { - return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr); + return params_->verbose; +} + +bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const +{ + return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr)); +} + +bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const +{ + return floating_state_blocks_.count(state_ptr) == 1; } bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const { - return isFactorRegisteredDerived(fac_ptr); + return factors_.count(fac_ptr) == 1 and isFactorRegisteredDerived(fac_ptr); +} + +bool SolverManager::isStateBlockFixed(const StateBlockPtr& st) +{ + if (!isStateBlockRegistered(st)) + return false; + + if (isStateBlockFloating(st)) + return st->isFixed(); + + return isStateBlockFixedDerived(st); +} + +bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) +{ + if (!isStateBlockRegistered(st)) + return false; + + if (isStateBlockFloating(st)) + return st->getLocalParametrization() == local_param; + + return hasThisLocalParametrizationDerived(st, local_param); +}; + +bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const +{ + if (!isStateBlockRegistered(st)) + return false; + + if (isStateBlockFloating(st)) + return st->hasLocalParametrization(); + + return hasLocalParametrizationDerived(st); +}; + +int SolverManager::numFactors() const +{ + return factors_.size(); +} + +int SolverManager::numStateBlocks() const +{ + return state_blocks_.size() + floating_state_blocks_.size(); +} + +int SolverManager::numStateBlocksFloating() const +{ + return floating_state_blocks_.size(); } +double SolverManager::getPeriod() const +{ + return params_->period; +} bool SolverManager::check(std::string prefix) const { @@ -335,15 +526,32 @@ bool SolverManager::check(std::string prefix) const ok = false; } - // factor involving state block in factors_ - for (auto fac : sb_fac_it->second) + // no factors involving state block + if (sb_fac_it->second.empty()) { - if (factors_.count(fac) == 0) + WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix); + ok = false; + } + else + { + // factor involving state block in factors_ + for (auto fac : sb_fac_it->second) { - WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix); - ok = false; + if (factors_.count(fac) == 0) + { + WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix); + ok = false; + } } } + + // can't be in both state_blocks_ and floating_state_blocks_ + if (floating_state_blocks_.count(sb_fac_it->first) == 1) + { + WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix); + ok = false; + } + sb_vec_it++; sb_fac_it++; } @@ -362,10 +570,63 @@ bool SolverManager::check(std::string prefix) const } } - // checkDerived + // CHECK DERIVED ---------------------- ok &= checkDerived(prefix); + // CHECK IF DERIVED IS UP TO DATE ---------------------- + // state blocks registered in derived solver + for (auto sb : state_blocks_) + if (!isStateBlockRegisteredDerived(sb.first)) + { + WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix); + ok = false; + } + + // factors registered in derived solver + for (auto fac : factors_) + if (!isFactorRegisteredDerived(fac)) + { + WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix); + ok = false; + } + + // numbers + if (numStateBlocksDerived() != state_blocks_.size()) + { + WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix); + ok = false; + } + if (numFactorsDerived() != numFactors()) + { + WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix); + ok = false; + } + return ok; } +void SolverManager::printProfiling(std::ostream& _stream) const +{ + _stream <<"\nSolverManager:" + <<"\nTotal values:" + << "\n\tSolving state: " << 1e-6*acc_duration_total_.count() << " s - executions: " << n_solve_ + << "\n\t\tUpdate problem: " << 1e-6*acc_duration_update_.count() << " s" << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)" + << "\n\t\tSolver: " << 1e-6*acc_duration_solver_.count() << " s" << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)" + << "\n\t\tUpdate state: " << 1e-6*acc_duration_state_.count() << " s" << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)" + << "\n\tSolving covariance: " << 1e-6*acc_duration_cov_.count() << " s - executions: " << n_cov_ + <<"\nAverage:" + << "\n\tSolving state: " << 1e-3*acc_duration_total_.count() / n_solve_ << " ms" + << "\n\t\tUpdate problem: " << 1e-3*acc_duration_update_.count() / n_solve_ << " ms" + << "\n\t\tSolver: " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms" + << "\n\t\tUpdate state: " << 1e-3*acc_duration_state_.count() / n_solve_ << " ms" + << "\n\tSolving covariance: " << 1e-3*acc_duration_cov_.count() / n_cov_ << " ms" + <<"\nMax values:" + << "\n\tSolving state: " << 1e-3*max_duration_total_.count() << " ms" + << "\n\t\tUpdate problem: " << 1e-3*max_duration_update_.count() << " ms" + << "\n\t\tSolver: " << 1e-3*max_duration_solver_.count() << " ms" + << "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms" + << "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl; + +} + } // namespace wolf diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp index 908a2f4aabcddc90248ab99261c7d5824c54c826..f94353d93dff9a0425e984372dde89b0116c8b4d 100644 --- a/src/solver_suitesparse/solver_manager.cpp +++ b/src/solver_suitesparse/solver_manager.cpp @@ -1,4 +1,4 @@ -#include "core/ceres_wrapper/ceres_manager.h" +#include "../../include/core/ceres_wrapper/solver_ceres.h" SolverManager::SolverManager() { diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp index 83cf44b26bdfcad7e415633ef6d8fe783564a75c..ce743db05620f2e5bea90fd6cb8512c9669d394f 100644 --- a/src/state_block/has_state_blocks.cpp +++ b/src/state_block/has_state_blocks.cpp @@ -4,7 +4,7 @@ namespace wolf { -StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) +StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead."); state_block_map_.emplace(_sb_type, _sb); @@ -54,4 +54,39 @@ void HasStateBlocks::perturb(double amplitude) } } +void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const +{ + if (_metric && _state_blocks) + { + for (const auto &key : getStructure()) + { + auto sb = getStateBlock(key); + if (sb) + _stream << _tabs << " " << key + << "[" << (sb->isFixed() ? "Fix" : "Est") + << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )" + << " @ " << sb << std::endl; + } + } + else if (_metric) + { + _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") + << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )" + << std::endl; + } + else if (_state_blocks) + { + _stream << _tabs << " " << "sb:"; + for (const auto &key : getStructure()) + { + const auto &sb = getStateBlock(key); + if (sb) + _stream << " " << key + << "[" << (sb->isFixed() ? "Fix" : "Est") + << "] @ " << sb; + } + _stream << std::endl; + } +} + } diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp index b2fa1c111e028445333236e01384144aeafc31a6..846b14a49934c70c0d3879efd03f454ffab74605 100644 --- a/src/state_block/local_parametrization_homogeneous.cpp +++ b/src/state_block/local_parametrization_homogeneous.cpp @@ -60,5 +60,9 @@ bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _ return true; } +bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, double tolerance) +{ + return _state.size() == global_size_; +} } // namespace wolf diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp index 90b7f47c6b5ffa2a28374b5673827d325224430f..0f4efab6f1f093d8f1425d0855a453c6370f1c93 100644 --- a/src/state_block/state_composite.cpp +++ b/src/state_block/state_composite.cpp @@ -11,9 +11,8 @@ namespace wolf{ VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes) { auto size_it = _sizes.begin(); - for ( const auto& ckey : _structure) + for ( const auto& key : _structure) { - const auto& key = string(1,ckey); const auto& size = *size_it; this->emplace(key, VectorXd(size).setZero()); @@ -26,9 +25,8 @@ VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _stru { int index = 0; auto size_it = _sizes.begin(); - for ( const auto& ckey : _structure) + for ( const auto& key : _structure) { - const auto& key = string(1,ckey); const auto& size = *size_it; (*this)[key] = _v.segment(index,size); @@ -40,9 +38,8 @@ VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _stru VectorComposite::VectorComposite (const StateStructure& _s) { - for (const auto& ckey : _s) + for (const auto& key : _s) { - const auto& key = string(1,ckey); // ckey is char this->emplace(key,VectorXd(0)); } } @@ -53,9 +50,8 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l auto vector_it = _vectors.begin(); - for (const auto& ckey : _structure) + for (const auto& key : _structure) { - string key(1,ckey); this->emplace(key, *vector_it); vector_it++; } @@ -67,9 +63,8 @@ Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const // traverse once with unordered_map access std::vector<const VectorXd*> vp; unsigned int size = 0; - for (const auto& ckey : _structure) + for (const auto& key : _structure) { - std::string key(1,ckey); // ckey is char vp.push_back(&(this->at(key))); size += vp.back()->size(); } @@ -125,9 +120,8 @@ void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, { int index = 0; auto size_it = _sizes.begin(); - for ( const auto& ckey : _structure) + for ( const auto& key : _structure) { - const auto& key = string(1,ckey); // ckey is char const auto& size = *size_it; (*this)[key] = _v.segment(index,size); @@ -156,7 +150,7 @@ wolf::VectorComposite operator -(const wolf::VectorComposite &_x) ////// MATRIX COMPOSITE ////////// -bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, const Eigen::MatrixXd &_mat_blk) +bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk) { // check rows if (size_rows_.count(_row) == 0) @@ -175,7 +169,7 @@ bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, return true; } -bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen::MatrixXd &_mat_blk) const +bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const { const auto &row_it = this->find(_row); if(row_it != this->end()) @@ -190,7 +184,7 @@ bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen return true; } -Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col) +Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) { const auto &row_it = this->find(_row); assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite."); @@ -201,7 +195,7 @@ Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string return col_it->second; } -const Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col) const +const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const { const auto &row_it = this->find(_row); assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite."); @@ -287,7 +281,7 @@ MatrixComposite MatrixComposite::operator - (void) const for (const auto& pair_rkey_row : *this) { - m.unordered_map<string,unordered_map<string, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<string, MatrixXd>()); + m.unordered_map<char,unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<char, MatrixXd>()); for (const auto& pair_ckey_blk : pair_rkey_row.second) { m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second); @@ -449,8 +443,8 @@ MatrixComposite operator * (double scalar_left, const MatrixComposite& M) MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const { - std::unordered_map < string, unsigned int> indices_rows; - std::unordered_map < string, unsigned int> indices_cols; + std::unordered_map < char, unsigned int> indices_rows; + std::unordered_map < char, unsigned int> indices_cols; unsigned int rows, cols; sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols); @@ -498,22 +492,20 @@ unsigned int MatrixComposite::cols() const void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows, const StateStructure &_struct_cols, - std::unordered_map<string, unsigned int> &_indices_rows, - std::unordered_map<string, unsigned int> &_indices_cols, + std::unordered_map<char, unsigned int> &_indices_rows, + std::unordered_map<char, unsigned int> &_indices_cols, unsigned int &_rows, unsigned int &_cols) const { _rows = 0; _cols = 0; - for (const auto& crow : _struct_rows) + for (const auto& row : _struct_rows) { - string row = string(1,crow); // crow is char _indices_rows[row] = _rows; _rows += size_rows_.at(row); } - for (const auto& ccol : _struct_cols) + for (const auto& col : _struct_cols) { - string col = string(1,ccol); // ccol is char _indices_cols[col] = _cols; _cols += size_cols_.at(col); } @@ -521,16 +513,9 @@ void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows, MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure) { - for (const auto& rkey_char : _row_structure) - { - string rkey(1,rkey_char); // key was char - for (const auto& ckey_char : _col_structure) - { - string ckey(1,ckey_char); // key was char - + for (const auto& rkey : _row_structure) + for (const auto& ckey : _col_structure) this->emplace(rkey, ckey, MatrixXd(0,0)); - } - } } MatrixComposite::MatrixComposite (const StateStructure& _row_structure, @@ -542,15 +527,11 @@ MatrixComposite::MatrixComposite (const StateStructure& _row_structure, assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!"); auto rsize_it = _row_sizes.begin(); - for (const auto& rkey_char : _row_structure) + for (const auto& rkey : _row_structure) { - string rkey(1,rkey_char); // key was char - auto csize_it = _col_sizes.begin(); - for (const auto& ckey_char : _col_structure) + for (const auto& ckey : _col_structure) { - string ckey(1,ckey_char); // key was char - this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized. csize_it ++; @@ -570,21 +551,17 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m, SizeEigen rindex = 0, cindex; auto rsize_it = _row_sizes.begin(); - for (const auto& rkey_char : _row_structure) + for (const auto& rkey : _row_structure) { assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows"); - string rkey(1,rkey_char); // key was char - cindex = 0; auto csize_it = _col_sizes.begin(); - for (const auto& ckey_char : _col_structure) + for (const auto& ckey : _col_structure) { assert(_m.cols() >= cindex + *csize_it && "Provided matrix has insufficient number of columns"); - string ckey(1,ckey_char); // key was char - this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it)); cindex += *csize_it; @@ -609,15 +586,11 @@ MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, con assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!"); auto rsize_it = _row_sizes.begin(); - for (const auto& rkey_char : _row_structure) + for (const auto& rkey : _row_structure) { - string rkey(1,rkey_char); // key was char - auto csize_it = _col_sizes.begin(); - for (const auto& ckey_char : _col_structure) + for (const auto& ckey : _col_structure) { - string ckey(1,ckey_char); // key was char - m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it)); csize_it ++; @@ -639,7 +612,7 @@ MatrixComposite MatrixComposite::identity (const StateStructure& _structure, con while (rkey_it != _structure.end()) { - const auto& rkey = string(1,*rkey_it); + const auto& rkey = *rkey_it; const auto rsize = *rsize_it; m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block @@ -652,7 +625,7 @@ MatrixComposite MatrixComposite::identity (const StateStructure& _structure, con while (ckey_it != _structure.end()) { - const auto& ckey = string(1,*ckey_it); + const auto& ckey = *ckey_it; const auto& csize = *csize_it; m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block @@ -763,7 +736,7 @@ bool StateBlockComposite::isFixed() const return fixed; } -unsigned int StateBlockComposite::remove(const std::string &_sb_type) +unsigned int StateBlockComposite::remove(const char &_sb_type) { return state_block_map_.erase(_sb_type); } @@ -778,7 +751,7 @@ bool StateBlockComposite::has(const StateBlockPtr &_sb) const return found; } -StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const +StateBlockPtr StateBlockComposite::at(const char &_sb_type) const { auto it = state_block_map_.find(_sb_type); if (it != state_block_map_.end()) @@ -787,7 +760,7 @@ StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const return nullptr; } -void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr &_sb) +void StateBlockComposite::set(const char& _sb_type, const StateBlockPtr &_sb) { auto it = state_block_map_.find(_sb_type); assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead."); @@ -795,7 +768,7 @@ void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr & it->second = _sb; } -void StateBlockComposite::set(const std::string& _key, const VectorXd &_vec) +void StateBlockComposite::set(const char& _key, const VectorXd &_vec) { auto it = state_block_map_.find(_key); assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead."); @@ -810,15 +783,14 @@ void StateBlockComposite::setVectors(const StateStructure& _structure, const std assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match"); auto vec_it = _vectors.begin(); - for (const auto ckey : _structure) + for (const auto key : _structure) { - string key(1,ckey); set (key, *vec_it); vec_it++; } } -bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const +bool StateBlockComposite::key(const StateBlockPtr &_sb, char &_key) const { const auto& it = this->find(_sb); @@ -831,12 +803,12 @@ bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const } else { - _key = ""; + _key ='0'; return false; } } -std::string StateBlockComposite::key(const StateBlockPtr& _sb) const +char StateBlockComposite::key(const StateBlockPtr& _sb) const { const auto& it = this->find(_sb); @@ -845,7 +817,7 @@ std::string StateBlockComposite::key(const StateBlockPtr& _sb) const if (found) return it->first; else - return ""; + return '0'; } @@ -853,7 +825,7 @@ StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const { const auto& it = std::find_if(state_block_map_.begin(), state_block_map_.end(), - [_sb](const std::pair<std::string, StateBlockPtr>& pair) + [_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; } @@ -862,7 +834,7 @@ StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const return it; } -StateBlockPtr StateBlockComposite::add(const std::string &_sb_type, const StateBlockPtr &_sb) +StateBlockPtr StateBlockComposite::add(const char &_sb_type, const StateBlockPtr &_sb) { assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead."); @@ -955,9 +927,8 @@ SizeEigen StateBlockComposite::stateSize() const SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const { SizeEigen size = 0; - for (const auto& ckey : _structure) + for (const auto& key : _structure) { - string key(1,ckey); size += state_block_map_.at(key)->getSize(); } return size; @@ -968,9 +939,8 @@ VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) cons VectorXd x(stateSize(_structure)); SizeEigen index = 0; SizeEigen size; - for (const auto& ckey : _structure) + for (const auto& key : _structure) { - string key(1,ckey); const auto& sb = state_block_map_.at(key); size = sb->getSize(); x.segment(index,size) = sb->getState(); @@ -984,9 +954,8 @@ void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd _vector.resize(stateSize(_structure)); SizeEigen index = 0; SizeEigen size; - for (const auto& ckey : _structure) + for (const auto& key : _structure) { - string key(1,ckey); const auto& sb = state_block_map_.at(key); size = sb->getSize(); _vector.segment(index,size) = sb->getState(); diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index ad760c8c3ccf2942f734c256e7b04ed3d59aecf7..4a64613a3445bfead7565f38ae5527f01f2fde5c 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -4,11 +4,9 @@ namespace wolf { TrajectoryBase::TrajectoryBase() : - NodeBase("TRAJECTORY", "TrajectoryBase"), - last_key_frame_ptr_(nullptr), - last_key_or_aux_frame_ptr_(nullptr) + NodeBase("TRAJECTORY", "TrajectoryBase") { -// WOLF_DEBUG("constructed T"); + frame_map_ = FrameMap(); } TrajectoryBase::~TrajectoryBase() @@ -19,16 +17,9 @@ TrajectoryBase::~TrajectoryBase() FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) { // add to list - frame_list_.push_back(_frame_ptr); + assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one"); - if (_frame_ptr->isKeyOrAux()) - { - // sort - sortFrame(_frame_ptr); - - // update last_estimated and last_key - updateLastFrames(); - } + frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr); return _frame_ptr; } @@ -36,116 +27,71 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr) { // add to list - frame_list_.remove(_frame_ptr); - - // update last_estimated and last_key - if (_frame_ptr->isKeyOrAux()) - updateLastFrames(); + // frame_map_.erase(_frame_ptr); + frame_map_.erase(_frame_ptr->getTimeStamp()); } void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const { - for(auto fr_ptr : getFrameList()) + for(auto fr_ptr : *this) fr_ptr->getFactorList(_fac_list); } -void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr) -{ - moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr)); -} - -void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place) -{ - if (*_place != _frm_ptr) - { - frame_list_.remove(_frm_ptr); - frame_list_.insert(_place, _frm_ptr); - updateLastFrames(); - } -} - -FrameBaseConstIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr) -{ - for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) - if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp()) - return frm_rit.base(); - return getFrameList().begin(); -} -void TrajectoryBase::updateLastFrames() +FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const { - bool last_estimated_set = false; - - // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp - for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit) - if ((*frm_rit)->isKeyOrAux()) - { - if (!last_estimated_set) - { - last_key_or_aux_frame_ptr_ = (*frm_rit); - last_estimated_set = true; - } - if ((*frm_rit)->isKey()) - { - last_key_frame_ptr_ = (*frm_rit); - break; - } - } -} - -FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const -{ - // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp FrameBasePtr closest_kf = nullptr; - double min_dt = 1e9; - - for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++) - if ((*frm_rit)->isKey()) + //If frame_map_ is empty then closestFrameToTimeStamp is meaningless + if(not frame_map_.empty()) + { + //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub + //In the std they fulfill the following property: + // lb is the first element such that ts <= lb, alternatively the smallest element that is NOT less than ts. + // lb definition is NOT the ACTUAL lower bound but the following position so, lb = lb_true + 1. + + auto lower_bound = frame_map_.lower_bound(_ts); + if((lower_bound != this->end() and lower_bound->first == _ts) or lower_bound == this->begin() ) closest_kf = lower_bound->second; + else { - double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); - if (dt < min_dt) - { - min_dt = dt; - closest_kf = *frm_rit; - } + auto upper_bound = lower_bound; + //I find it easier to reason if lb < ts < ub. Remember that we have got rid of the + //equality case and the out of bounds cases so this inequality is complete (it is not misssing cases). + //Therefore, we need to decrease the lower_bound to the previous element + lower_bound = std::prev(lower_bound); + + //If ub points to end() it means that the last frame is still less than _ts, therefore certainly + //it will be the closest one + if(upper_bound == this->end()) closest_kf = lower_bound->second; else - break; - } - return closest_kf; -} - -FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const -{ - // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp - FrameBasePtr closest_kf = nullptr; - double min_dt = 1e9; - - for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++) - if ((*frm_rit)->isKeyOrAux()) - { - double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); - if (dt < min_dt) { - min_dt = dt; - closest_kf = *frm_rit; + //Easy stuff just calculate the distance return minimum + auto lb_diff = fabs(lower_bound->first - _ts); + auto ub_diff = fabs(upper_bound->first - _ts); + if(lb_diff < ub_diff) + { + closest_kf = lower_bound->second; + } + else + { + closest_kf = upper_bound->second; + } } - else - break; } + } return closest_kf; } void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const { - _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameList().size()) + "F") : "") << std::endl; + _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl; } void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 1) - for (auto F : getFrameList()) - F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); - + for (auto F : *this) + if (F) + F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const @@ -169,7 +115,7 @@ bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, auto trj_ptr = std::static_pointer_cast<TrajectoryBase>(_node_ptr); auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs); _log.compose(local_log); - for (auto F : getFrameList()) + for (auto F : *this) F->check(_log, F, _verbose, _stream, _tabs + " "); return _log.is_consistent_; } diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp index 5816567557bdd2ca5779615834a3f48a418bbabe..3c64b2ee87d2e26318788c4eddd547606edb6cb0 100644 --- a/src/tree_manager/tree_manager_sliding_window.cpp +++ b/src/tree_manager/tree_manager_sliding_window.cpp @@ -3,32 +3,22 @@ namespace wolf { -void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame) +void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame) { - int n_kf(0); - FrameBasePtr first_KF(nullptr), second_KF(nullptr); - for (auto frm : getProblem()->getTrajectory()->getFrameList()) - { - if (frm->isKey()) - { - n_kf++; - if (first_KF == nullptr) - first_KF = frm; - else if (second_KF == nullptr) - second_KF = frm; - } - } + int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames - // remove first KF if too many KF - if (n_kf > params_sw_->n_key_frames) + // remove first frame if too many frames + if (n_f > params_sw_->n_frames) { - WOLF_DEBUG("TreeManagerSlidingWindow removing first frame"); - first_KF->remove(params_sw_->viral_remove_empty_parent); - if (params_sw_->fix_first_key_frame) + if (params_sw_->fix_first_frame) { WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame"); - second_KF->fix(); + auto second_frame = std::next(getProblem()->getTrajectory()->begin())->second; + if (second_frame) + second_frame->fix(); } + WOLF_DEBUG("TreeManagerSlidingWindow removing first frame"); + getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_empty_parent); } } diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2009cd89edf486af2efa892f58615a1e0fa05b0f --- /dev/null +++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp @@ -0,0 +1,86 @@ +#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h" +#include "core/capture/capture_motion.h" +#include "core/processor/processor_motion.h" + +namespace wolf +{ + +TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) : + TreeManagerSlidingWindow(_params), + params_swdr_(_params), + count_frames_(0) +{ + NodeBase::setType("TreeManagerSlidingWindowDualRate"); +} + +void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame) +{ + int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames + + // recent segment not complete + if (n_f <= params_swdr_->n_frames_recent) + return; + + + // REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames + if (count_frames_ != 0) + { + WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames"); + FrameBasePtr remove_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(), + params_swdr_->n_frames_recent)->second; + FrameBasePtr keep_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(), + params_swdr_->n_frames_recent - 1)->second; + + // compose motion captures for all processors motion + for (auto is_motion : getProblem()->getProcessorIsMotionList()) + { + auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion); + if (proc_motion == nullptr) + continue; + + auto cap_prev = std::static_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor())); + auto cap_next = std::static_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor())); + + // merge captures (if exist) + if (cap_prev and cap_next) + { + WOLF_DEBUG("TreeManagerSlidingWindow merging capture ", cap_prev->id(), " with ", cap_next->id()); + assert(cap_next->getOriginCapture() == cap_prev); + proc_motion->mergeCaptures(cap_prev, cap_next); + } + + } + + // remove frame + remove_recent_frame->remove(params_swdr_->viral_remove_empty_parent); + } + // REMOVE OLDEST FRAME: when first recent frame is kept, remove oldest frame (if max frames reached) + else if (n_f > params_swdr_->n_frames) + { + if (params_swdr_->fix_first_frame) + { + WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame"); + auto second_frame = *std::next(getProblem()->getTrajectory()->begin()); + if (second_frame) + second_frame->fix(); + } + WOLF_DEBUG("TreeManagerSlidingWindow removing first frame"); + getProblem()->getTrajectory()->getFirstFrame()->remove(params_swdr_->viral_remove_empty_parent); + } + + // iterate counter + count_frames_++; + if (count_frames_ == params_swdr_->rate_old_frames) + count_frames_ = 0; +} + + +} /* namespace wolf */ + +// Register in the FactoryTreeManager +#include "core/tree_manager/factory_tree_manager.h" +namespace wolf { +WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate); +WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowDualRate); +} // namespace wolf + diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp index 82057b000ffaf1eb34b674dbe5ac9a224b5efb7a..f07cc4d559c911e9cfc9e4ecb2797417c9b21201 100644 --- a/src/yaml/parser_yaml.cpp +++ b/src/yaml/parser_yaml.cpp @@ -345,6 +345,12 @@ void ParserYaml::updateActiveName(std::string _tag) { active_name_ = _tag; } +/* +** @brief This function is responsible for parsing the first level of the YAML file. +** The first level here can be thought as the entry point of the YAML file. Since we impose a certain structure +** this function is responsible for enforcing said structure. +** + */ void ParserYaml::parseFirstLevel(YAML::Node _n, std::string _file) { @@ -482,13 +488,13 @@ void ParserYaml::parse() { std::vector<std::string> tags = std::vector<std::string>(); tags.push_back("ROS subscriber"); - walkTreeR(it.n_, tags, "ROS subscriber/" + it.topic_); + walkTreeR(it.n_, tags, "ROS subscriber/" + it.type_ + " - " + it.topic_); } for (auto it : publisher_managers_) { std::vector<std::string> tags = std::vector<std::string>(); tags.push_back("ROS publisher"); - walkTreeR(it.n_, tags, "ROS publisher/" + it.topic_); + walkTreeR(it.n_, tags, "ROS publisher/" + it.type_ + " - " + it.topic_); } std::list<std::string> plugins; std::list<std::string> packages_subscriber, packages_publisher; @@ -550,6 +556,9 @@ void ParserYaml::parse() } parsing_file_.pop(); } +/* +** @brief This function gives the ability to run the parser without enforcing the wolf YAML structure. + */ void ParserYaml::parse_freely() { parsing_file_.push(generatePath(file_)); diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp index 9c21e3842304e475146194bc04da7be9f73de396..bd168ee20b162426a85d7b1b78e508f2c00a8fe1 100644 --- a/src/yaml/processor_odom_3d_yaml.cpp +++ b/src/yaml/processor_odom_3d_yaml.cpp @@ -34,7 +34,6 @@ static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _f YAML::Node keyframe_vote = config["keyframe_vote"]; params->voting_active = keyframe_vote["voting_active"] .as<bool>(); - params->voting_aux_active = keyframe_vote["voting_aux_active"].as<bool>(); params->max_time_span = keyframe_vote["max_time_span"] .as<double>(); params->max_buff_length = keyframe_vote["max_buff_length"] .as<SizeEigen>(); params->dist_traveled = keyframe_vote["dist_traveled"] .as<double>(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 05717bad197c33e241146a8cf4d08fc337d99ea2..4363e38a4d4df67a73ce92086b7a3ae071c4a561 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -28,7 +28,7 @@ include_directories(${GTEST_INCLUDE_DIRS}) # # # Create a specific test executable for gtest_example # wolf_add_gtest(gtest_example gtest_example.cpp) # -target_link_libraries(gtest_example ${PROJECT_NAME}) # +target_link_libraries(gtest_example ${PLUGIN_NAME}) # # # ########################################################### set(SRC_DUMMY @@ -36,7 +36,7 @@ set(SRC_DUMMY dummy/processor_tracker_landmark_dummy.cpp ) add_library(dummy SHARED ${SRC_DUMMY}) -target_link_libraries(dummy ${PROJECT_NAME}) +target_link_libraries(dummy ${PLUGIN_NAME}) ################# ADD YOUR TESTS BELOW #################### # # # ==== IN ALPHABETICAL ORDER! ==== # @@ -46,221 +46,231 @@ target_link_libraries(dummy ${PROJECT_NAME}) # CaptureBase class test wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp) -target_link_libraries(gtest_capture_base ${PROJECT_NAME}) +target_link_libraries(gtest_capture_base ${PLUGIN_NAME}) # Converter from String to various types used by the parameters server wolf_add_gtest(gtest_converter gtest_converter.cpp) -target_link_libraries(gtest_converter ${PROJECT_NAME}) +target_link_libraries(gtest_converter ${PLUGIN_NAME}) # FactorBase class test wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp) -target_link_libraries(gtest_factor_base ${PROJECT_NAME}) +target_link_libraries(gtest_factor_base ${PLUGIN_NAME}) # FactorAutodiff class test wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp) -target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME}) +target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME}) # FactoryStateBlock factory test wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp) -target_link_libraries(gtest_factory_state_block ${PROJECT_NAME}) +target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME}) # FeatureBase classes test wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp) -target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME}) +target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME}) # Node Emplace test wolf_add_gtest(gtest_emplace gtest_emplace.cpp) -target_link_libraries(gtest_emplace ${PROJECT_NAME} dummy) +target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy) # FeatureBase classes test wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp) -target_link_libraries(gtest_feature_base ${PROJECT_NAME}) +target_link_libraries(gtest_feature_base ${PLUGIN_NAME}) # FrameBase classes test wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp) -target_link_libraries(gtest_frame_base ${PROJECT_NAME}) +target_link_libraries(gtest_frame_base ${PLUGIN_NAME}) # HasStateBlocks classes test wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp) -target_link_libraries(gtest_has_state_blocks ${PROJECT_NAME}) +target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME}) # LocalParametrizationXxx classes test wolf_add_gtest(gtest_local_param gtest_local_param.cpp) -target_link_libraries(gtest_local_param ${PROJECT_NAME}) +target_link_libraries(gtest_local_param ${PLUGIN_NAME}) # Logging test wolf_add_gtest(gtest_logging gtest_logging.cpp) -target_link_libraries(gtest_logging ${PROJECT_NAME}) +target_link_libraries(gtest_logging ${PLUGIN_NAME}) # MotionBuffer class test wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp) -target_link_libraries(gtest_motion_buffer ${PROJECT_NAME}) +target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME}) # PackKFBuffer wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp) -target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME} dummy) +target_link_libraries(gtest_pack_KF_buffer ${PLUGIN_NAME} dummy) # Parameters server wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR}) -target_link_libraries(gtest_param_server ${PROJECT_NAME}) +target_link_libraries(gtest_param_server ${PLUGIN_NAME}) # YAML parser wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp) -target_link_libraries(gtest_parser_yaml ${PROJECT_NAME}) +target_link_libraries(gtest_parser_yaml ${PLUGIN_NAME}) # Problem class test wolf_add_gtest(gtest_problem gtest_problem.cpp) -target_link_libraries(gtest_problem ${PROJECT_NAME} dummy) +target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy) # ProcessorBase class test wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp) -target_link_libraries(gtest_processor_base ${PROJECT_NAME} dummy) +target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy) # ProcessorMotion class test wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp) -target_link_libraries(gtest_processor_motion ${PROJECT_NAME}) +target_link_libraries(gtest_processor_motion ${PLUGIN_NAME}) # Rotation test wolf_add_gtest(gtest_rotation gtest_rotation.cpp) # SE3 test wolf_add_gtest(gtest_SE3 gtest_SE3.cpp) -target_link_libraries(gtest_SE3 ${PROJECT_NAME}) +target_link_libraries(gtest_SE3 ${PLUGIN_NAME}) # SensorBase test wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp) -target_link_libraries(gtest_sensor_base ${PROJECT_NAME}) +target_link_libraries(gtest_sensor_base ${PLUGIN_NAME}) # shared_from_this test wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp) -target_link_libraries(gtest_shared_from_this ${PROJECT_NAME}) +target_link_libraries(gtest_shared_from_this ${PLUGIN_NAME}) # SolverManager test wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp) -target_link_libraries(gtest_solver_manager ${PROJECT_NAME}) +target_link_libraries(gtest_solver_manager ${PLUGIN_NAME}) + +# SolverManagerMultithread test +wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp) +target_link_libraries(gtest_solver_manager_multithread ${PLUGIN_NAME}) # StateBlock class test wolf_add_gtest(gtest_state_block gtest_state_block.cpp) -target_link_libraries(gtest_state_block ${PROJECT_NAME}) +target_link_libraries(gtest_state_block ${PLUGIN_NAME}) # StateBlock class test wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp) -target_link_libraries(gtest_state_composite ${PROJECT_NAME}) +target_link_libraries(gtest_state_composite ${PLUGIN_NAME}) # TimeStamp class test wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp) -target_link_libraries(gtest_time_stamp ${PROJECT_NAME}) +target_link_libraries(gtest_time_stamp ${PLUGIN_NAME}) # TrackMatrix class test wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp) -target_link_libraries(gtest_track_matrix ${PROJECT_NAME}) +target_link_libraries(gtest_track_matrix ${PLUGIN_NAME}) # TrajectoryBase class test wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp) -target_link_libraries(gtest_trajectory ${PROJECT_NAME}) +target_link_libraries(gtest_trajectory ${PLUGIN_NAME}) # TreeManager class test wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp) -target_link_libraries(gtest_tree_manager ${PROJECT_NAME}) +target_link_libraries(gtest_tree_manager ${PLUGIN_NAME}) # ------- Now Derived classes ---------- -IF (Ceres_FOUND) -# CeresManager test -wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp) -target_link_libraries(gtest_ceres_manager ${PROJECT_NAME}) -ENDIF(Ceres_FOUND) - # FactorAbs(P/O/V) classes test wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp) -target_link_libraries(gtest_factor_absolute ${PROJECT_NAME}) +target_link_libraries(gtest_factor_absolute ${PLUGIN_NAME}) # FactorAutodiffDistance3d test wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp) -target_link_libraries(gtest_factor_autodiff_distance_3d ${PROJECT_NAME}) +target_link_libraries(gtest_factor_autodiff_distance_3d ${PLUGIN_NAME}) # FactorBlockDifference class test wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp) -target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME}) +target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME}) # FactorOdom3d class test wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp) -target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME}) +target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME}) # FactorOdom2d class test wolf_add_gtest(gtest_factor_odom_2d gtest_factor_odom_2d.cpp) -target_link_libraries(gtest_factor_odom_2d ${PROJECT_NAME}) +target_link_libraries(gtest_factor_odom_2d ${PLUGIN_NAME}) # FactorOdom3d class test wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp) -target_link_libraries(gtest_factor_odom_3d ${PROJECT_NAME}) +target_link_libraries(gtest_factor_odom_3d ${PLUGIN_NAME}) # FactorPose2d class test wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp) -target_link_libraries(gtest_factor_pose_2d ${PROJECT_NAME}) +target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME}) # FactorPose3d class test wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp) -target_link_libraries(gtest_factor_pose_3d ${PROJECT_NAME}) +target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME}) # FactorRelativePose2dWithExtrinsics class test wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp) -target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PROJECT_NAME}) +target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAME}) # FactorRelative2dAnalytic class test wolf_add_gtest(gtest_factor_relative_2d_analytic gtest_factor_relative_2d_analytic.cpp) -target_link_libraries(gtest_factor_relative_2d_analytic ${PROJECT_NAME}) +target_link_libraries(gtest_factor_relative_2d_analytic ${PLUGIN_NAME}) # MakePosDef function test wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp) -target_link_libraries(gtest_make_posdef ${PROJECT_NAME}) +target_link_libraries(gtest_make_posdef ${PLUGIN_NAME}) # Map yaml save/load test wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp) -target_link_libraries(gtest_map_yaml ${PROJECT_NAME}) +target_link_libraries(gtest_map_yaml ${PLUGIN_NAME}) # Parameter prior test wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) -target_link_libraries(gtest_param_prior ${PROJECT_NAME}) +target_link_libraries(gtest_param_prior ${PLUGIN_NAME}) # ProcessorDiffDriveSelfcalib class test wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp) -target_link_libraries(gtest_processor_diff_drive ${PROJECT_NAME}) +target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME}) # ProcessorLoopClosureBase class test wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp) -target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME}) +target_link_libraries(gtest_processor_loopclosure ${PLUGIN_NAME}) # ProcessorFrameNearestNeighborFilter class test # wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp) -# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PROJECT_NAME}) +# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PLUGIN_NAME}) # ProcessorMotion in 2d wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp) -target_link_libraries(gtest_odom_2d ${PROJECT_NAME}) +target_link_libraries(gtest_odom_2d ${PLUGIN_NAME}) # ProcessorOdom3d class test wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp) -target_link_libraries(gtest_processor_odom_3d ${PROJECT_NAME}) +target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME}) # ProcessorTrackerFeatureDummy class test wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp) -target_link_libraries(gtest_processor_tracker_feature_dummy ${PROJECT_NAME} dummy) +target_link_libraries(gtest_processor_tracker_feature_dummy ${PLUGIN_NAME} dummy) # ProcessorTrackerLandmarkDummy class test wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp) -target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dummy) +target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dummy) # SensorDiffDriveSelfcalib class test wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp) -target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME}) +target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME}) + +IF (Ceres_FOUND) + # SolverCeres test + wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp) + target_link_libraries(gtest_solver_ceres ${PLUGIN_NAME}) + + # SolverCeresMultithread test + wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp) + target_link_libraries(gtest_solver_ceres_multithread ${PLUGIN_NAME}) +ENDIF(Ceres_FOUND) # TreeManagerSlidingWindow class test wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp) -target_link_libraries(gtest_tree_manager_sliding_window ${PROJECT_NAME}) +target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME}) + +# TreeManagerSlidingWindowDualRate class test +wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp) +target_link_libraries(gtest_tree_manager_sliding_window_dual_rate ${PLUGIN_NAME}) # yaml conversions wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp) -target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME}) - - +target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME}) diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h index 15db720a3c42f8151dd8745dc35d0232fc0fe885..aab56cd1ab532a6a7d3e4b876f2de6dea0ea1b7b 100644 --- a/test/dummy/solver_manager_dummy.h +++ b/test/dummy/solver_manager_dummy.h @@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy); class SolverManagerDummy : public SolverManager { public: - std::list<FactorBasePtr> factors_; + std::set<FactorBasePtr> factors_derived_; std::map<StateBlockPtr,bool> state_block_fixed_; std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_; @@ -26,35 +26,34 @@ class SolverManagerDummy : public SolverManager { }; - bool isStateBlockRegistered(const StateBlockPtr& st) override + bool isStateBlockFixedDerived(const StateBlockPtr& st) override { - return state_blocks_.find(st)!=state_blocks_.end(); + return state_block_fixed_.at(st); }; - bool isStateBlockFixed(const StateBlockPtr& st) const + bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override { - return state_block_fixed_.at(st); + return state_block_local_param_.at(st) == local_param; }; - bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override + bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override { - return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end(); + return state_block_local_param_.at(st) != nullptr; }; - bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const + virtual int numStateBlocksDerived() const override { - return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param; - }; + return state_block_fixed_.size(); + } - bool hasLocalParametrization(const StateBlockPtr& st) const + virtual int numFactorsDerived() const override { - return state_block_local_param_.find(st) != state_block_local_param_.end(); + return factors_derived_.size(); }; - void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {}; - void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {}; - bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override {return true;}; - bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override {return true;}; + bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override {return false;}; + bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override {return false;}; + // The following are dummy implementations bool hasConverged() override { return true; } @@ -62,19 +61,17 @@ class SolverManagerDummy : public SolverManager double initialCost() override { return double(1); } double finalCost() override { return double(0); } - - protected: bool checkDerived(std::string prefix="") const override {return true;} std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");}; void addFactorDerived(const FactorBasePtr& fac_ptr) override { - factors_.push_back(fac_ptr); + factors_derived_.insert(fac_ptr); }; void removeFactorDerived(const FactorBasePtr& fac_ptr) override { - factors_.remove(fac_ptr); + factors_derived_.erase(fac_ptr); }; void addStateBlockDerived(const StateBlockPtr& state_ptr) override { @@ -92,10 +89,16 @@ class SolverManagerDummy : public SolverManager }; void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override { - if (state_ptr->getLocalParametrization() == nullptr) - state_block_local_param_.erase(state_ptr); - else - state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); + state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); + }; + bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override + { + return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1; + }; + + bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override + { + return factors_derived_.count(fac_ptr) == 1; }; }; diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h index 662c7d15677173fc302c02b10b1f660297753daf..1e7e70f2ee476404cb96b070e72f280c7a570a6e 100644 --- a/test/dummy/tree_manager_dummy.h +++ b/test/dummy/tree_manager_dummy.h @@ -20,7 +20,7 @@ struct ParamsTreeManagerDummy : public ParamsTreeManagerBase bool toy_param; - std::string print() const + std::string print() const override { return ParamsTreeManagerBase::print() + "\n" + "toy_param: " + std::to_string(toy_param) + "\n"; diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp index 27acfea3069b92076bd30e4abd4ac28d787147bf..a99f7a19551f3e450e6b4958ded294db8980c62c 100644 --- a/test/gtest_SE3.cpp +++ b/test/gtest_SE3.cpp @@ -146,15 +146,15 @@ TEST(SE3, composeVectorComposite) VectorComposite x1, x2, xc("PO", {3,4}); - x1.emplace("P", p1); - x1.emplace("O", q1.coeffs()); - x2.emplace("P", p2); - x2.emplace("O", q2.coeffs()); + x1.emplace('P', p1); + x1.emplace('O', q1.coeffs()); + x2.emplace('P', p2); + x2.emplace('O', q2.coeffs()); compose(x1, x2, xc); - ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8); - ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8); + ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8); + ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8); } TEST(SE3, composeVectorComposite_Jacobians) @@ -174,26 +174,26 @@ TEST(SE3, composeVectorComposite_Jacobians) VectorComposite x1, x2, xc("PO", {3,4}); MatrixComposite J_xc_x1, J_xc_x2; - x1.emplace("P", p1); - x1.emplace("O", q1.coeffs()); - x2.emplace("P", p2); - x2.emplace("O", q2.coeffs()); + x1.emplace('P', p1); + x1.emplace('O', q1.coeffs()); + x2.emplace('P', p2); + x2.emplace('O', q2.coeffs()); // we'll do xc = x1 * x2 and obtain Jacobians compose(x1, x2, xc, J_xc_x1, J_xc_x2); - ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8); - ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8); + ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8); + ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "P"), Matrix3d::Identity() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "O"), - R1 * skew(p2) , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "P"), Matrix3d::Zero() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "O"), R2.transpose() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), - R1 * skew(p2) , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "P"), R1 , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "O"), Matrix3d::Zero() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "P"), Matrix3d::Zero() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "O"), Matrix3d::Identity() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1 , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity() , 1e-8); } TEST(SE3, exp_0_Composite) @@ -205,13 +205,13 @@ TEST(SE3, exp_0_Composite) VectorComposite tau; - tau.emplace("P", p); - tau.emplace("O", theta); + tau.emplace('P', p); + tau.emplace('O', theta); VectorComposite x = SE3::exp(tau); - ASSERT_MATRIX_APPROX(x.at("P"), Vector3d::Zero(), 1e-8); - ASSERT_MATRIX_APPROX(x.at("O"), Quaterniond::Identity().coeffs(), 1e-8); + ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8); + ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8); } @@ -223,20 +223,20 @@ TEST(SE3, plus_0_Composite) Vector4d q; q.setRandom().normalized(); VectorComposite x; - x.emplace("P", p); - x.emplace("O", q); + x.emplace('P', p); + x.emplace('O', q); Vector3d dp; dp.setZero(); Vector3d dtheta; dtheta.setZero(); VectorComposite tau; - tau.emplace("P", dp); - tau.emplace("O", dtheta); + tau.emplace('P', dp); + tau.emplace('O', dtheta); VectorComposite res = plus(x, tau); - ASSERT_MATRIX_APPROX(res.at("P"), p, 1e-8); - ASSERT_MATRIX_APPROX(res.at("O"), q, 1e-8); + ASSERT_MATRIX_APPROX(res.at('P'), p, 1e-8); + ASSERT_MATRIX_APPROX(res.at('O'), q, 1e-8); } TEST(SE3, interpolate_I_xyz) diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp index 4c004cd72e14e9e69bb343e288ab86e3a5b9354e..3a0cbc4555c88bd40f0a656e2a73ff2a619654b6 100644 --- a/test/gtest_capture_base.cpp +++ b/test/gtest_capture_base.cpp @@ -100,6 +100,93 @@ TEST(CaptureBase, process) ASSERT_TRUE(C->process()); // This should not fail (although it does nothing) } +TEST(CaptureBase, move_from_F_to_KF) +{ + ProblemPtr problem = Problem::create("PO", 2); + + auto KF = problem->emplaceFrame(0.0); // dummy F object + + auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0); + + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + + auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0); + + ASSERT_EQ(KF->getCaptureList().size(), 1); + ASSERT_EQ(F->getCaptureList().size(), 1); + + C->move(KF); + + ASSERT_EQ(KF->getCaptureList().size(), 2); + ASSERT_EQ(F->getCaptureList().size(), 0); + ASSERT_TRUE(C->getProblem()); +} + +TEST(CaptureBase, move_from_F_to_null) +{ + ProblemPtr problem = Problem::create("PO", 2); + + FrameBasePtr F0 = nullptr; + + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + + auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0); + + ASSERT_EQ(F->getCaptureList().size(), 1); + + C->move(F0); + + ASSERT_EQ(F->getCaptureList().size(), 0); + ASSERT_FALSE(C->getProblem()); +} + +TEST(CaptureBase, move_from_null_to_KF) +{ + ProblemPtr problem = Problem::create("PO", 2); + + auto KF = problem->emplaceFrame(0.0); // dummy F object + + auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0); + + auto C = std::make_shared<CaptureBase>("Dummy", 0.0); + + ASSERT_EQ(KF->getCaptureList().size(), 1); + + C->move(KF); + + ASSERT_EQ(KF->getCaptureList().size(), 2); + ASSERT_TRUE(C->getProblem()); +} + +TEST(CaptureBase, move_from_null_to_F) +{ + ProblemPtr problem = Problem::create("PO", 2); + + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + + auto C = std::make_shared<CaptureBase>("Dummy", 0.0); + + C->move(F); + + ASSERT_EQ(F->getCaptureList().size(), 1); + + ASSERT_FALSE(C->getProblem()); +} + +TEST(CaptureBase, move_from_KF_to_F) +{ + ProblemPtr problem = Problem::create("PO", 2); + + auto KF = problem->emplaceFrame(0.0); // dummy F object + + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + + auto C = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0); + + ASSERT_DEATH(C->move(F), ""); +} + + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp deleted file mode 100644 index 5586332e6eb0470e8ffef1c9b0eb2d9d2fd1b313..0000000000000000000000000000000000000000 --- a/test/gtest_ceres_manager.cpp +++ /dev/null @@ -1,634 +0,0 @@ -/* - * gtest_ceres_manager.cpp - * - * Created on: Jun, 2018 - * Author: jvallve - */ - -#include "core/utils/utils_gtest.h" - - -#include "core/problem/problem.h" -#include "core/sensor/sensor_base.h" -#include "core/state_block/state_block.h" -#include "core/capture/capture_void.h" -#include "core/factor/factor_pose_2d.h" -#include "core/factor/factor_quaternion_absolute.h" -#include "core/solver/solver_manager.h" -#include "core/ceres_wrapper/ceres_manager.h" -#include "core/state_block/local_parametrization_angle.h" -#include "core/state_block/local_parametrization_quaternion.h" - -#include "ceres/ceres.h" - -#include <iostream> - -using namespace wolf; -using namespace Eigen; - -WOLF_PTR_TYPEDEFS(CeresManagerWrapper); -class CeresManagerWrapper : public CeresManager -{ - public: - CeresManagerWrapper(const ProblemPtr& wolf_problem) : - CeresManager(wolf_problem) - { - }; - - bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st) - { - return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st)); - }; - - bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st) - { - return state_blocks_.find(st)!=state_blocks_.end(); - }; - - bool isStateBlockFixed(const StateBlockPtr& st) - { - return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st)); - }; - - int numStateBlocks() - { - return ceres_problem_->NumParameterBlocks(); - }; - - int numFactors() - { - return ceres_problem_->NumResidualBlocks(); - }; - - bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override - { - return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end(); - }; - - bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) - { - return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() && - state_blocks_local_param_.at(st)->getLocalParametrization() == local_param && - ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get(); - }; - - bool hasLocalParametrization(const StateBlockPtr& st) const - { - return state_blocks_local_param_.find(st) != state_blocks_local_param_.end(); - }; - -}; - -TEST(CeresManager, Create) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // check double ointers to branches - ASSERT_EQ(P, ceres_manager_ptr->getProblem()); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, AddStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, DoubleAddStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // add stateblock again - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, UpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock unfixed - ASSERT_FALSE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); - - // Fix frame - sb_ptr->fix(); - - // update solver manager - ceres_manager_ptr->update(); - - // check stateblock fixed - ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, AddUpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // Fix state block - sb_ptr->fix(); - - // update solver manager - ceres_manager_ptr->update(); - - // check stateblock fixed - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, RemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - - // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, AddRemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); - - // update solver - ceres_manager_ptr->update(); - - // check no stateblocks - ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, RemoveUpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); - - // update solver - ceres_manager_ptr->update(); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, DoubleRemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); - - // update solver - ceres_manager_ptr->update(); - - // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); - - // update solver manager - ceres_manager_ptr->update(); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, AddFactor) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 1); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, DoubleAddFactor) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); - - // add factor again - P->notifyFactor(c,ADD); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 1); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, RemoveFactor) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); - - // update solver - ceres_manager_ptr->update(); - - // remove factor - P->notifyFactor(c,REMOVE); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, AddRemoveFactor) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); - - // remove factor - P->notifyFactor(c,REMOVE); - - ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, DoubleRemoveFactor) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); - - // update solver - ceres_manager_ptr->update(); - - // remove factor - P->notifyFactor(c,REMOVE); - - // update solver - ceres_manager_ptr->update(); - - // remove factor - P->notifyFactor(c,REMOVE); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, AddStateBlockLocalParam) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector1d state; state << 1; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_param_ptr); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr)); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, RemoveLocalParam) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector1d state; state << 1; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_param_ptr); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // Remove local param - sb_ptr->removeLocalParametrization(); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, AddLocalParam) -{ - ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector1d state; state << 1; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->notifyStateBlock(sb_ptr,ADD); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - - // Local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_param_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr)); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, FactorsRemoveLocalParam) -{ - ProblemPtr P = Problem::create("PO", 3); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); - FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // remove local param - F->getO()->removeLocalParametrization(); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -TEST(CeresManager, FactorsUpdateLocalParam) -{ - ProblemPtr P = Problem::create("PO", 3); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); - FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // remove local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>(); - F->getO()->setLocalParametrization(local_param_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),local_param_ptr)); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 2c28c2bd442bc8675bf5e71fb82d97141cac8390..21ed3167006049866bcbc481ec5bfa81c760a23c 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -42,8 +42,8 @@ TEST(Emplace, Frame) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem()); + FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); } TEST(Emplace, Processor) @@ -67,12 +67,12 @@ TEST(Emplace, Capture) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem()); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem()); ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame()); } @@ -81,17 +81,17 @@ TEST(Emplace, Feature) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem()); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem()); ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame()); auto cov = Eigen::MatrixXd::Identity(2,2); FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXd(2), cov); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem()); ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture()); } TEST(Emplace, Factor) @@ -99,17 +99,17 @@ TEST(Emplace, Factor) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem()); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem()); ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame()); auto cov = Eigen::MatrixXd::Identity(2,2); auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem()); ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture()); auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false); ASSERT_NE(nullptr, ftr->getFactorList().front().get()); @@ -119,7 +119,7 @@ TEST(Emplace, EmplaceDerived) { ProblemPtr P = Problem::create("POV", 3); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d()); auto cov = Eigen::MatrixXd::Identity(2,2); auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr); @@ -130,7 +130,7 @@ TEST(Emplace, EmplaceDerived) auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov); ASSERT_EQ(sen, P->getHardware()->getSensorList().front()); - ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem()); + ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem()); } TEST(Emplace, Nullpointer) { @@ -141,7 +141,7 @@ TEST(Emplace, ReturnDerived) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); auto cov = Eigen::MatrixXd::Identity(2,2); auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov); diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp index 82b56da523c70dff134908ba3e63adf97493d914..dc3551d2c79d47973e4dca81d4046fa1f669661d 100644 --- a/test/gtest_factor_absolute.cpp +++ b/test/gtest_factor_absolute.cpp @@ -5,12 +5,12 @@ * \author: datchuth */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_block_absolute.h" #include "core/factor/factor_quaternion_absolute.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -32,10 +32,10 @@ Vector10d x0 = pose9toPose10(pose + Vector9d::Random()*0.25); // Problem and solver ProblemPtr problem_ptr = Problem::create("POV", 3); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, problem_ptr->stateZero() ); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, problem_ptr->stateZero() ); // Capture // auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr); @@ -59,7 +59,7 @@ TEST(FactorBlockAbs, fac_block_abs_p) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only orientation is constrained ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6); @@ -75,7 +75,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only orientation is constrained ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6); @@ -93,7 +93,7 @@ TEST(FactorBlockAbs, fac_block_abs_v) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only velocity is constrained ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6); @@ -111,7 +111,7 @@ TEST(FactorQuatAbs, fac_block_abs_o) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only velocity is constrained ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6); diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index 0acd77891bf1bae60fe41575edec629e9864999b..f4e3108d6bdf2e129358b851e82772dc5cffae3a 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -350,8 +350,11 @@ TEST(FactorAutodiff, AutodiffDummy12) TEST(FactorAutodiff, EmplaceOdom2d) { - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); + + ProblemPtr problem = Problem::create("PO", 2); + + FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1)); + FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2)); // SENSOR ParamsSensorOdom2d intrinsics_odo; @@ -377,12 +380,14 @@ TEST(FactorAutodiff, EmplaceOdom2d) TEST(FactorAutodiff, ResidualOdom2d) { + ProblemPtr problem = Problem::create("PO", 2); + Eigen::Vector3d f1_pose, f2_pose; f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); + FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose); + FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose); // SENSOR ParamsSensorOdom2d intrinsics_odo; @@ -421,12 +426,14 @@ TEST(FactorAutodiff, ResidualOdom2d) TEST(FactorAutodiff, JacobianOdom2d) { + ProblemPtr problem = Problem::create("PO", 2); + Eigen::Vector3d f1_pose, f2_pose; f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); + FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose); + FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose); // SENSOR ParamsSensorOdom2d intrinsics_odo; @@ -499,12 +506,14 @@ TEST(FactorAutodiff, JacobianOdom2d) TEST(FactorAutodiff, AutodiffVsAnalytic) { + ProblemPtr problem = Problem::create("PO", 2); + Eigen::Vector3d f1_pose, f2_pose; f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); + FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose); + FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose); // SENSOR ParamsSensorOdom2d intrinsics_odo; diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp index 1a3f492384290e2bb14e8f01af153aa61abc1193..fa3d365424b3d62aa56eb6e6d186163251a594ad 100644 --- a/test/gtest_factor_autodiff_distance_3d.cpp +++ b/test/gtest_factor_autodiff_distance_3d.cpp @@ -5,10 +5,10 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/factor/factor_autodiff_distance_3d.h" #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/math/rotations.h" #include "core/utils/utils_gtest.h" @@ -34,7 +34,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test Matrix1d dist_cov; ProblemPtr problem; - CeresManagerPtr ceres_manager; + SolverCeresPtr solver; void SetUp() override { @@ -53,11 +53,11 @@ class FactorAutodiffDistance3d_Test : public testing::Test dist_cov = Matrix1d(0.01); problem = Problem::create("PO", 3); - ceres_manager = std::make_shared<CeresManager>(problem); + solver = std::make_shared<SolverCeres>(problem); - F1 = problem->emplaceFrame (KEY, 1.0, pose1); + F1 = problem->emplaceFrame (1.0, pose1); - F2 = problem->emplaceFrame (KEY, 2.0, pose2); + F2 = problem->emplaceFrame (2.0, pose2); C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0); } @@ -96,7 +96,7 @@ TEST_F(FactorAutodiffDistance3d_Test, solve) f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov); c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE); - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); + std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // Check distance between F1 and F2 positions -- must match the measurement ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10); diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp index e1e035bf0ae084d309b51db84b4c49ac62440924..79663676a51ea74c3ea2fc23f5c32c5e18e1cf30 100644 --- a/test/gtest_factor_base.cpp +++ b/test/gtest_factor_base.cpp @@ -24,8 +24,8 @@ class FactorBaseTest : public testing::Test void SetUp() override { - F0 = std::make_shared<FrameBase>(KEY, 0.0, nullptr); - F1 = std::make_shared<FrameBase>(KEY, 1.0, nullptr); + F0 = std::make_shared<FrameBase>(0.0, nullptr); + F1 = std::make_shared<FrameBase>(1.0, nullptr); C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr); C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr); f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp index 9c739965701b6236dcff93fbafba1bd12f90eddb..1dbf21b97b609ae212b1e88cf16ef993c2e742dc 100644 --- a/test/gtest_factor_block_difference.cpp +++ b/test/gtest_factor_block_difference.cpp @@ -8,9 +8,9 @@ #include "core/utils/utils_gtest.h" #include <Eigen/Dense> -#include "core/problem/problem.h" -#include <core/ceres_wrapper/ceres_manager.h> +#include "../include/core/ceres_wrapper/solver_ceres.h" +#include "core/problem/problem.h" #include "core/frame/frame_base.h" #include "core/capture/capture_base.h" #include "core/feature/feature_base.h" @@ -30,7 +30,7 @@ class FixtureFactorBlockDifference : public testing::Test { public: ProblemPtr problem_; - CeresManagerPtr ceres_manager_; + SolverCeresPtr solver_; FrameBasePtr KF0_; FrameBasePtr KF1_; CaptureBasePtr Cap_; @@ -43,8 +43,7 @@ class FixtureFactorBlockDifference : public testing::Test { // Problem and solver problem_ = Problem::create("POV", 3); - ceres::Solver::Options ceres_options; - ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options); + solver_ = std::make_shared<SolverCeres>(problem_); TimeStamp t0(0); TimeStamp t1(1); @@ -59,7 +58,7 @@ class FixtureFactorBlockDifference : public testing::Test //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>()); //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false); - KF1_ = problem_->emplaceFrame(KEY, t1, problem_->stateZero()); + KF1_ = problem_->emplaceFrame(t1, problem_->stateZero()); Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1); } @@ -84,7 +83,7 @@ TEST_F(FixtureFactorBlockDifference, EqualP) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8); } @@ -98,7 +97,7 @@ TEST_F(FixtureFactorBlockDifference, EqualV) KF0_->getV()->setState(Vector3d::Random()); KF1_->getV()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8); } @@ -114,7 +113,7 @@ TEST_F(FixtureFactorBlockDifference, DiffP) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diff, 1e-8); } @@ -130,7 +129,7 @@ TEST_F(FixtureFactorBlockDifference, DiffV) KF0_->getV()->setState(Vector3d::Random()); KF1_->getV()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diff, 1e-8); } @@ -151,7 +150,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<1>() - KF0_->getP()->getState().head<1>(), diff.head<1>(), 1e-8); } @@ -171,7 +170,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<2>() - KF0_->getP()->getState().head<2>(), diff, 1e-8); } @@ -192,7 +191,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz) KF1_->getP()->setState(Vector3d::Random()); problem_->print(4,true,true,true); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); std::cout << report << std::endl; problem_->print(4,true,true,true); diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index f2e8e5f6014811ea3b99c2843fa4b0e8e94c5b72..4f3ece70d92af1deeb923ce811d7a09dd96ba543 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -5,13 +5,13 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/processor/processor_diff_drive.h" #include "core/sensor/sensor_diff_drive.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_diff_drive.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/frame/frame_base.h" #include "core/capture/capture_motion.h" #include "core/feature/feature_motion.h" @@ -114,7 +114,7 @@ class FactorDiffDriveTest : public testing::Test { public: ProblemPtr problem; - CeresManagerPtr solver; + SolverCeresPtr solver; TrajectoryBasePtr trajectory; ParamsSensorDiffDrivePtr intr; SensorDiffDrivePtr sensor; @@ -137,7 +137,7 @@ class FactorDiffDriveTest : public testing::Test problem = Problem::create("PO", 2); trajectory = problem->getTrajectory(); - solver = std::make_shared<CeresManager>(problem); + solver = std::make_shared<SolverCeres>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! intr = std::make_shared<ParamsSensorDiffDrive>(); @@ -160,12 +160,10 @@ class FactorDiffDriveTest : public testing::Test // frames F0 = FrameBase::emplace<FrameBase>(trajectory, - KEY, 0.0, "PO", std::list<VectorXd>{Vector2d(0,0), Vector1d(0)}); F1 = FrameBase::emplace<FrameBase>(trajectory, - KEY, 1.0, "PO", std::list<VectorXd>{Vector2d(1,0), Vector1d(0)}); @@ -436,7 +434,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) */ ProblemPtr problem = Problem::create("PO", 2); - CeresManagerPtr solver = std::make_shared<CeresManager>(problem); + SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>(); @@ -500,7 +498,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) C->process(); } - auto F2 = problem->getLastKeyFrame(); + auto F2 = problem->getLastFrame(); // Fix boundaries F0->fix(); @@ -569,7 +567,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) ProblemPtr problem = Problem::create("PO", 2); - CeresManagerPtr solver = std::make_shared<CeresManager>(problem); + SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>(); @@ -636,7 +634,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) C->process(); } - auto F2 = problem->getLastKeyFrame(); + auto F2 = problem->getLastFrame(); // VectorComposite x2c(x2, "PO", {2,1}); F2->setState(x2, "PO"); // Impose known final state regardless of integrated value. diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp index 8c0b11e3b428cfbfa99c91c29bffe53ceeeb1d92..740933a67158634dfd7c6194c8b6a46bd926c063 100644 --- a/test/gtest_factor_odom_2d.cpp +++ b/test/gtest_factor_odom_2d.cpp @@ -1,10 +1,10 @@ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_odom_2d.h" #include "core/capture/capture_odom_2d.h" #include "core/math/rotations.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -16,11 +16,11 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity(); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 2); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), Vector3d::Zero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), Vector3d::Zero()); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector3d::Zero()); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector3d::Zero()); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov); @@ -62,7 +62,7 @@ TEST(FactorOdom2d, fix_0_solve) frm1->perturb(5); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); @@ -103,7 +103,7 @@ TEST(FactorOdom2d, fix_1_solve) frm0->perturb(5); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp index 8001bbe70f25f774a55cf0c834950529934bdb36..7a57f0354a19c2a53e8ce6adac68f07713a93664 100644 --- a/test/gtest_factor_odom_3d.cpp +++ b/test/gtest_factor_odom_3d.cpp @@ -5,12 +5,12 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_odom_3d.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -34,11 +34,11 @@ Vector7d x1 = data2delta(data + Vector6d::Random()*0.25); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 3); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), problem_ptr->stateZero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), delta); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero()); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta); // Capture, feature and factor from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr); @@ -66,7 +66,7 @@ TEST(FactorOdom3d, fix_0_solve) frm1->setState(x1); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6); @@ -80,7 +80,7 @@ TEST(FactorOdom3d, fix_1_solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6); } diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp index e3107b7a5f976f5626f22016f64f60695e1fc982..7ecc6ab0848695e7922d9e9fb83f92a73a95e861 100644 --- a/test/gtest_factor_pose_2d.cpp +++ b/test/gtest_factor_pose_2d.cpp @@ -5,12 +5,12 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/factor/factor_pose_2d.h" #include "core/utils/utils_gtest.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -27,10 +27,10 @@ Vector3d x0 = pose + Vector3d::Random()*0.25; // Problem and solver ProblemPtr problem = Problem::create("PO", 2); -CeresManager ceres_mgr(problem); +SolverCeres solver(problem); // Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY, TimeStamp(0), problem->stateZero()); +FrameBasePtr frm0 = problem->emplaceFrame(TimeStamp(0), problem->stateZero()); // Capture, feature and factor from frm1 to frm0 auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr); @@ -57,7 +57,7 @@ TEST(FactorPose2d, solve) frm0->setState(x0); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose, 1e-6); diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp index d57aee85c27b47e41f9610040c3b8bcceb57ad35..c44374caa2a908080d36e5396a9e41db846b7fb7 100644 --- a/test/gtest_factor_pose_3d.cpp +++ b/test/gtest_factor_pose_3d.cpp @@ -5,12 +5,12 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/factor/factor_pose_3d.h" #include "core/utils/utils_gtest.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -33,10 +33,10 @@ Vector7d x0 = pose6toPose7(pose + Vector6d::Random()*0.25); // Problem and solver ProblemPtr problem = Problem::create("PO", 3); -CeresManager ceres_mgr(problem); +SolverCeres solver(problem); // Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY, 0, problem->stateZero() ); +FrameBasePtr frm0 = problem->emplaceFrame(0, problem->stateZero() ); // Capture, feature and factor auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr); @@ -63,7 +63,7 @@ TEST(FactorPose3d, solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL); + std::string brief_report = solver.solve(SolverManager::ReportVerbosity::FULL); ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose7, 1e-6); diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp index c461bbb94743c9ee8b3665650fb00686d979fc20..70cbc464716d50d60bb8ad05cfdb1a4bd6f79d95 100644 --- a/test/gtest_factor_relative_2d_analytic.cpp +++ b/test/gtest_factor_relative_2d_analytic.cpp @@ -1,10 +1,10 @@ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_relative_2d_analytic.h" #include "core/capture/capture_odom_2d.h" #include "core/math/rotations.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -16,11 +16,11 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity(); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 2); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, Vector3d::Zero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1.0, Vector3d::Zero()); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero()); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero()); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov); @@ -62,7 +62,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve) frm1->perturb(5); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); @@ -103,7 +103,7 @@ TEST(FactorRelative2dAnalytic, fix_1_solve) frm0->perturb(5); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index b9ad5bbe208d981a4ca69aa3eab8eac806f62e14..f6e8e18f10247e1c819121bc9e4494dd26ac6efe 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -1,3 +1,4 @@ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_relative_pose_2d_with_extrinsics.h" @@ -6,7 +7,6 @@ #include "core/processor/processor_odom_2d.h" #include "core/math/rotations.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -20,15 +20,15 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity(); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 2); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Sensor auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"); auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>()); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, Vector3d::Zero() ); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1, Vector3d::Zero() ); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() ); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() ); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov); @@ -40,7 +40,7 @@ TEST(FactorRelativePose2dWithExtrinsics, check_tree) TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve) { - for (int i = 0; i < 1e3; i++) + for (int i = 0; i < 1e2; i++) { // Random delta Vector3d delta = Vector3d::Random() * 10; @@ -77,7 +77,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve) frm1->perturb(5); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); @@ -88,7 +88,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve) TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve) { - for (int i = 0; i < 1e3; i++) + for (int i = 0; i < 1e2; i++) { // Random delta Vector3d delta = Vector3d::Random() * 10; @@ -125,7 +125,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve) frm0->perturb(5); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); @@ -136,7 +136,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve) TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve) { - for (int i = 0; i < 1e3; i++) + for (int i = 0; i < 1e2; i++) { // Random delta Vector3d delta = Vector3d::Random() * 10; @@ -173,7 +173,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve) sensor_odom2d->getP()->perturb(1); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6); @@ -184,7 +184,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve) TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve) { - for (int i = 0; i < 1e3; i++) + for (int i = 0; i < 1e2; i++) { // Random delta Vector3d delta = Vector3d::Random() * 10; @@ -223,7 +223,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve) //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl; // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6); diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index abdefc83d7d4a3e1f041f5355b3139c1276775cb..ac405e6fa1e9f4bcdc7976d8f0adc2014cb79c5f 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -22,7 +22,7 @@ using namespace wolf; TEST(FrameBase, GettersAndSetters) { - FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // getters ASSERT_EQ(F->id(), (unsigned int) 1); @@ -31,13 +31,11 @@ TEST(FrameBase, GettersAndSetters) F->getTimeStamp(t); ASSERT_EQ(t, 1); ASSERT_FALSE(F->isFixed()); - ASSERT_EQ(F->isKey(), false); - ASSERT_EQ(F->isKeyOrAux(), false); } TEST(FrameBase, StateBlocks) { - FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2); ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2); @@ -47,12 +45,10 @@ TEST(FrameBase, StateBlocks) TEST(FrameBase, LinksBasic) { - FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); ASSERT_FALSE(F->getTrajectory()); ASSERT_FALSE(F->getProblem()); - // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert() - // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected SensorOdom2dPtr S = make_shared<SensorOdom2d>(Vector3d::Zero(), ParamsSensorOdom2d()); ASSERT_FALSE(F->getCaptureOf(S)); ASSERT_TRUE(F->getCaptureList().empty()); @@ -69,8 +65,8 @@ TEST(FrameBase, LinksToTree) intrinsics_odo.k_disp_to_disp = 1; intrinsics_odo.k_rot_to_rot = 1; auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo); - auto F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - auto F2 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr); WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size()); auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>()); @@ -115,11 +111,6 @@ TEST(FrameBase, LinksToTree) ASSERT_FALSE(F1->isFixed()); F1->getO()->fix(); ASSERT_TRUE(F1->isFixed()); - - // set key - F1->setKey(); - ASSERT_TRUE(F1->isKey()); - ASSERT_TRUE(F1->isKeyOrAux()); } #include "core/state_block/state_quaternion.h" @@ -131,7 +122,7 @@ TEST(FrameBase, GetSetState) StateQuaternionPtr sbq = make_shared<StateQuaternion>(); // Create frame - FrameBase F(NON_ESTIMATED, 1, sbp, sbq, sbv); + FrameBase F(1, sbp, sbq, sbv); // Give values to vectors and vector blocks VectorXd x(10), x1(10), x2(10); @@ -155,28 +146,24 @@ TEST(FrameBase, GetSetState) TEST(FrameBase, Constructor_composite) { - FrameBase F = FrameBase(KEY, - 0.0, + FrameBase F = FrameBase(0.0, "POV", VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)})); - ASSERT_TRUE (F.isKey()); ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15); ASSERT_TRUE (F.getO()->hasLocalParametrization()); - ASSERT_EQ (F.getStateBlock("V")->getSize(), 3); + ASSERT_EQ (F.getStateBlock('V')->getSize(), 3); } TEST(FrameBase, Constructor_vectors) { - FrameBase F = FrameBase(KEY, - 0.0, + FrameBase F = FrameBase(0.0, "POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}); - ASSERT_TRUE (F.isKey()); ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15); ASSERT_TRUE (F.getO()->hasLocalParametrization()); - ASSERT_EQ (F.getStateBlock("V")->getSize(), 3); + ASSERT_EQ (F.getStateBlock('V')->getSize(), 3); } diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp index 1369fd5a87dc31f4d0a7af14a929f0d23637b041..0d42d813257b1b2b26aeefe3fb908a3b29b5b616 100644 --- a/test/gtest_has_state_blocks.cpp +++ b/test/gtest_has_state_blocks.cpp @@ -6,13 +6,13 @@ */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/frame/frame_base.h" #include "core/sensor/sensor_base.h" #include "core/landmark/landmark_base.h" #include "core/state_block/state_quaternion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace wolf; @@ -41,8 +41,8 @@ class HasStateBlocksTest : public testing::Test sbo1 = std::make_shared<StateQuaternion>(); sbv1 = std::make_shared<StateBlock>(3); // size 3 - F0 = std::make_shared<FrameBase>(NON_ESTIMATED, 0.0, sbp0, sbo0); // non KF - F1 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.0, nullptr); // non KF + F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF + F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF } void TearDown() override{} @@ -50,40 +50,25 @@ class HasStateBlocksTest : public testing::Test }; -TEST_F(HasStateBlocksTest, Notifications_setKey_add) -{ - Notification n; - ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n)); - - F0->link(problem->getTrajectory()); - - ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n)); - - F0->setKey(); - - ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n)); - ASSERT_EQ(n, ADD); -} - TEST_F(HasStateBlocksTest, Notifications_add_makeKF) { Notification n; - // First add SB, than make KF + // First add SB, then make KF ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n)); - F0->link(problem->getTrajectory()); + // F0->link(problem->getTrajectory()); - ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n)); + // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n)); - ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n)); + // ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n)); - F0->addStateBlock("V", sbv0); + F0->addStateBlock('V', sbv0); ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n)); - F0->setKey(); + F0->link(problem); ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n)); ASSERT_EQ(n, ADD); @@ -99,10 +84,10 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add) // first make KF, then add SB - F1->link(problem->getTrajectory()); - F1->setKey(); + // F1->link(problem->getTrajectory()); + F1->link(problem); - F1->addStateBlock("P", sbp1); + F1->addStateBlock('P', sbp1); ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n)); ASSERT_EQ(n, ADD); @@ -111,7 +96,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add) ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n)); - F1->addStateBlock("V", sbv1); + F1->addStateBlock('V', sbv1); ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n)); ASSERT_EQ(n, ADD); @@ -121,7 +106,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add) TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add) { - CeresManagerPtr solver = std::make_shared<CeresManager>(problem); + SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); Notification n; @@ -130,9 +115,8 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add) ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n)); - F0->link(problem->getTrajectory()); - F0->addStateBlock("V", sbv0); - F0->setKey(); + F0->addStateBlock('V', sbv0); + F0->link(problem); ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n)); ASSERT_EQ(n, ADD); @@ -158,7 +142,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add) // Add again the same SB. This should crash - ASSERT_DEATH( F0->addStateBlock("V", sbv0) , "" ); + ASSERT_DEATH( F0->addStateBlock('V', sbv0) , "" ); } @@ -170,51 +154,48 @@ TEST_F(HasStateBlocksTest, hasStateBlock) TEST_F(HasStateBlocksTest, stateBlockKey) { - std::string key; + char key; ASSERT_TRUE(F0->stateBlockKey(sbp0, key)); - ASSERT_TRUE(key == "P"); + ASSERT_TRUE(key == 'P'); ASSERT_FALSE(F0->stateBlockKey(sbp1, key)); - ASSERT_TRUE(key == ""); + ASSERT_TRUE(key == '0'); } TEST_F(HasStateBlocksTest, getState_structure) { - F0->addStateBlock("V", sbv0); // now KF0 is POV + F0->addStateBlock('V', sbv0); // now KF0 is POV WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure()); auto state0 = F0->getState(); WOLF_DEBUG("getState() = ", state0); ASSERT_EQ(state0.size(), 3); - ASSERT_TRUE(state0.count("P")); - ASSERT_TRUE(state0.count("O")); - ASSERT_TRUE(state0.count("V")); + ASSERT_TRUE(state0.count('P')); + ASSERT_TRUE(state0.count('O')); + ASSERT_TRUE(state0.count('V')); state0 = F0->getState("PO"); WOLF_DEBUG("getState(\"PO\") = ", state0); ASSERT_EQ(state0.size(), 2); - ASSERT_TRUE(state0.count("P")); - ASSERT_TRUE(state0.count("O")); - ASSERT_FALSE(state0.count("V")); + ASSERT_TRUE(state0.count('P')); + ASSERT_TRUE(state0.count('O')); + ASSERT_FALSE(state0.count('V')); state0 = F0->getState("PV"); WOLF_DEBUG("getState(\"PV\") = ", state0); ASSERT_EQ(state0.size(), 2); - ASSERT_TRUE(state0.count("P")); - ASSERT_FALSE(state0.count("O")); - ASSERT_TRUE(state0.count("V")); + ASSERT_TRUE(state0.count('P')); + ASSERT_FALSE(state0.count('O')); + ASSERT_TRUE(state0.count('V')); state0 = F0->getState("OW"); // W does not exist WOLF_DEBUG("getState(\"OW\") = ", state0); ASSERT_EQ(state0.size(), 1); - ASSERT_FALSE(state0.count("P")); - ASSERT_TRUE(state0.count("O")); - ASSERT_FALSE(state0.count("V")); - ASSERT_FALSE(state0.count("W")); - - - + ASSERT_FALSE(state0.count('P')); + ASSERT_TRUE(state0.count('O')); + ASSERT_FALSE(state0.count('V')); + ASSERT_FALSE(state0.count('W')); } diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index 8d97fc8d063ed1a08c36b25f259353cc74852f76..ab545f2eca3122493a20ac31024faab520f46c64 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -16,7 +16,6 @@ // Wolf includes #include "core/state_block/state_block.h" #include "core/common/wolf.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/capture/capture_pose.h" // STL includes @@ -28,6 +27,7 @@ // General includes #include <iostream> #include <iomanip> // std::setprecision +#include "../include/core/ceres_wrapper/solver_ceres.h" using namespace wolf; using namespace Eigen; @@ -74,29 +74,26 @@ void show(const ProblemPtr& problem) using std::endl; cout << std::setprecision(4); - for (FrameBasePtr F : problem->getTrajectory()->getFrameList()) + for (FrameBasePtr F : *problem->getTrajectory()) { - if (F->isKey()) + cout << "----- Key Frame " << F->id() << " -----" << endl; + if (!F->getCaptureList().empty()) { - cout << "----- Key Frame " << F->id() << " -----" << endl; - if (!F->getCaptureList().empty()) + auto C = F->getCaptureList().front(); + if (!C->getFeatureList().empty()) { - auto C = F->getCaptureList().front(); - if (!C->getFeatureList().empty()) - { - auto f = C->getFeatureList().front(); - cout << " feature measure: \n" - << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose() - << endl; - cout << " feature covariance: \n" - << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl; - } + auto f = C->getFeatureList().front(); + cout << " feature measure: \n" + << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose() + << endl; + cout << " feature covariance: \n" + << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl; } - cout << " state: \n" << F->getStateVector().transpose() << endl; - Eigen::MatrixXd cov; - problem->getFrameCovariance(F,cov); - cout << " covariance: \n" << cov << endl; } + cout << " state: \n" << F->getStateVector().transpose() << endl; + Eigen::MatrixXd cov; + problem->getFrameCovariance(F,cov); + cout << " covariance: \n" << cov << endl; } } @@ -125,21 +122,21 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02; ProblemPtr Pr = Problem::create("PO", 2); - CeresManager ceres_manager(Pr); + SolverCeres solver(Pr); // KF0 and absolute prior FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2); // KF1 and motion from KF0 t += dt; - FrameBasePtr F1 = Pr->emplaceFrame(KEY, t, Vector3d::Zero()); + FrameBasePtr F1 = Pr->emplaceFrame(t, Vector3d::Zero()); auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t); auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov); auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false); // KF2 and motion from KF1 t += dt; - FrameBasePtr F2 = Pr->emplaceFrame(KEY, t, Vector3d::Zero()); + FrameBasePtr F2 = Pr->emplaceFrame(t, Vector3d::Zero()); auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t); auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov); auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false); @@ -150,10 +147,10 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) F0->setState(Vector3d(1,2,3)); F1->setState(Vector3d(2,3,1)); F2->setState(Vector3d(3,1,2)); - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL); + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); // std::cout << report << std::endl; - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); // show(Pr); Matrix3d P1, P2; @@ -218,17 +215,17 @@ TEST(Odom2d, VoteForKfAndSolve) // KF - * -- * -- KF - * -- * -- KF - * // Ceres wrapper - CeresManager ceres_manager(problem); + SolverCeres solver(problem); // Origin Key Frame auto KF = problem->setPriorFactor(x0, s0, t, dt/2); processor_odom2d->setOrigin(KF); - ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + solver.solve(SolverManager::ReportVerbosity::BRIEF); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); // std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl; - // std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl; + // std::cout << "Initial covariance : " << std::endl << problem->getLastFrameCovariance() << std::endl; // std::cout << "Motion data : " << data.transpose() << std::endl; // Check covariance values @@ -297,8 +294,8 @@ TEST(Odom2d, VoteForKfAndSolve) } // Solve - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); problem->print(4,1,1,1); @@ -306,10 +303,8 @@ TEST(Odom2d, VoteForKfAndSolve) // Check the 3 KFs' states and covariances MatrixXd computed_cov; int n = 0; - for (auto F : problem->getTrajectory()->getFrameList()) + for (auto F : *problem->getTrajectory()) { - if (!F->isKey()) break; - ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6); ASSERT_TRUE (F->getCovariance(computed_cov)); ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6); @@ -358,7 +353,7 @@ TEST(Odom2d, KF_callback) processor_odom2d->setTimeTolerance(dt/2); // Ceres wrapper - CeresManager ceres_manager(problem); + SolverCeres solver(problem); // Origin Key Frame FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2); @@ -423,12 +418,12 @@ TEST(Odom2d, KF_callback) std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl; Vector3d x_split = processor_odom2d->getState(t_split).vector("PO"); - FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, t_split, x_split); + FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split); // there must be 2KF and one F - ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3); + ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2); // The last KF must have TS = 0.08 - ASSERT_EQ(problem->getLastKeyFrame()->getTimeStamp().getNanoSeconds(), 80000000); + ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000); ASSERT_TRUE(problem->check(0)); processor_odom2d->keyFrameCallback(keyframe_2, dt/2); @@ -442,13 +437,13 @@ TEST(Odom2d, KF_callback) MotionBuffer key_buffer_n = key_capture_n->getBuffer(); - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); // std::cout << report << std::endl; - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6); + ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6); MatrixXd computed_cov; - ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov)); + ASSERT_TRUE(problem->getLastFrameCovariance(computed_cov)); ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6); //////////////////////////////////////////////////////////////// @@ -460,7 +455,7 @@ TEST(Odom2d, KF_callback) problem->print(4,1,1,1); x_split = processor_odom2d->getState(t_split).vector("PO"); - FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, t_split, x_split); + FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split); ASSERT_TRUE(problem->check(0)); processor_odom2d->keyFrameCallback(keyframe_1, dt/2); @@ -479,8 +474,8 @@ TEST(Odom2d, KF_callback) keyframe_1->setState(Vector3d(2,3,1)); keyframe_2->setState(Vector3d(3,1,2)); - report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); problem->print(4,1,1,1); @@ -493,7 +488,7 @@ TEST(Odom2d, KF_callback) // check other KF in the future of the split KF MatrixXd KF2_cov; ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov)); - ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6); + ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6); ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6); // Check full trajectory diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp index 056fbcf0bb231962e109ea60f404a5a0cefd58f5..d1fce41e50c72c91fb520d748a8154d5d516a1e6 100644 --- a/test/gtest_pack_KF_buffer.cpp +++ b/test/gtest_pack_KF_buffer.cpp @@ -34,10 +34,10 @@ class BufferPackKeyFrameTest : public testing::Test void SetUp(void) override { - f10 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(10),nullptr,nullptr,nullptr); - f20 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(20),nullptr,nullptr,nullptr); - f21 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(21),nullptr,nullptr,nullptr); - f28 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(28),nullptr,nullptr,nullptr); + f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr); + f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr); + f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr); + f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr); tt10 = 1.0; tt20 = 1.0; @@ -230,7 +230,7 @@ TEST_F(BufferPackKeyFrameTest, removeUpTo) // Specifically, only f28 should remain pack_kf_buffer.add(f28, tt28); ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2); - FrameBasePtr f22 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(22),nullptr,nullptr,nullptr); + FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr); PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5); pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() ); ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1); diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp index a512601e2b75ed7035f3b70745294ccffe1a9596..8faac803c5ffe7e5780557fbc08155e43c3ca587 100644 --- a/test/gtest_param_prior.cpp +++ b/test/gtest_param_prior.cpp @@ -9,15 +9,15 @@ #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/sensor/sensor_odom_3d.h" #include <iostream> +#include "../include/core/ceres_wrapper/solver_ceres.h" using namespace wolf; ProblemPtr problem_ptr = Problem::create("PO", 3); -CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); +SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr); Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).finished()); Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished()); Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished()); @@ -37,7 +37,7 @@ TEST(ParameterPrior, prior_p) { odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3d::Identity()); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6); } @@ -46,7 +46,7 @@ TEST(ParameterPrior, prior_o) { odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3d::Identity()); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6); } @@ -55,7 +55,7 @@ TEST(ParameterPrior, prior_p_overwrite) { odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3d::Identity()); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6); } @@ -64,7 +64,7 @@ TEST(ParameterPrior, prior_p_segment) { odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6); } @@ -72,7 +72,7 @@ TEST(ParameterPrior, prior_p_segment) TEST(ParameterPrior, prior_o_segment) { // constraining segment of quaternion is not allowed - ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter("O", prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),""); + ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter('O', prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),""); } diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 1470052b849465705eb860b34600d8d624dfb61c..181fb1ce43bc80a8a04034191973d5ac60635b1e 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -113,7 +113,7 @@ TEST(Problem, SetOrigin_PO_2d) VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1}); P->setPriorFactor(x0, s0, t0, 1.0); - + WOLF_INFO("printing.-.."); P->print(4,1,1,1); // check that no sensor has been added @@ -132,7 +132,7 @@ TEST(Problem, SetOrigin_PO_2d) F->getFactorList(fac_list); // check that we have one frame (prior) - ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1); // check that we have one capture (prior) ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1); // check that we have two features (prior: one per state block) @@ -191,7 +191,7 @@ TEST(Problem, SetOrigin_PO_3d) F->getFactorList(fac_list); // check that we have one frame (prior) - ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1); // check that we have one capture (prior) ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1); // check that we have two features (prior: one per state block) @@ -224,12 +224,13 @@ TEST(Problem, emplaceFrame_factory) { ProblemPtr P = Problem::create("PO", 2); - FrameBasePtr f0 = P->emplaceFrame(KEY, 0, "PO" , 2, VectorXd(3) ); - FrameBasePtr f1 = P->emplaceFrame(KEY, 1, "PO" , 3, VectorXd(7) ); - FrameBasePtr f2 = P->emplaceFrame(KEY, 2, "POV", 3, VectorXd(10) ); + FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2, VectorXd(3) ); + FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, VectorXd(7) ); + FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, VectorXd(10) ); + // check that all frames are effectively in the trajectory - ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3); + ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3); ASSERT_EQ(f0->getStateVector().size(), 3 ); ASSERT_EQ(f1->getStateVector().size(), 7 ); @@ -239,6 +240,8 @@ TEST(Problem, emplaceFrame_factory) ASSERT_EQ(f0->getProblem(), P); ASSERT_EQ(f1->getProblem(), P); ASSERT_EQ(f2->getProblem(), P); + + ASSERT_EQ(P->getFrameStructure(), "POV"); } TEST(Problem, StateBlocks) @@ -246,7 +249,7 @@ TEST(Problem, StateBlocks) std::string wolf_root = _WOLF_ROOT_DIR; ProblemPtr P = Problem::create("PO", 3); - Eigen::Vector7d xs3d; + Eigen::Vector7d xs3d; xs3d << 1,2,3,0,0,0,1; // required normalized quaternion (solver manager checks this) Eigen::Vector3d xs2d; // 2 state blocks, fixed @@ -261,7 +264,7 @@ TEST(Problem, StateBlocks) auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml"); // 2 state blocks, estimated - auto KF = P->emplaceFrame(KEY, 0, "PO", 3, xs3d ); + auto KF = P->emplaceFrame(0, "PO", 3, xs3d ); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // Notifications @@ -320,7 +323,7 @@ TEST(Problem, Covariances) // 4 state blocks, estimated St->unfixExtrinsics(); - FrameBasePtr F = P->emplaceFrame(KEY, 0, "PO", 3, xs3d ); + FrameBasePtr F = P->emplaceFrame(0, "PO", 3, xs3d ); // set covariance (they are not computed without a solver) P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity()); @@ -349,14 +352,14 @@ TEST(Problem, perturb) intr->ticks_per_wheel_revolution = 100; Vector3d extr(0,0,0); auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr)); - sensor->setStateBlockDynamic("I", true); + sensor->setStateBlockDynamic('I', true); Vector3d pose; pose << 0,0,0; int i = 0; for (TimeStamp t = 0.0; t < 2.0; t += 1.0) { - auto F = problem->emplaceFrame(KEY, t, pose ); + auto F = problem->emplaceFrame(t, pose ); if (i==0) F->fix(); for (int j = 0; j< 2 ; j++) @@ -385,21 +388,22 @@ TEST(Problem, perturb) //// CHECK ALL STATE BLOCKS /// - double prec = 1e-2; +#define prec 1e-2 for (auto S : problem->getHardware()->getSensorList()) { - ASSERT_TRUE (S->getP()->getState().isApprox(Vector2d(0,0), prec) ); - ASSERT_TRUE (S->getO()->getState().isApprox(Vector1d(0), prec) ); + ASSERT_MATRIX_APPROX(S->getP()->getState(), Vector2d(0,0), prec ); + ASSERT_MATRIX_APPROX(S->getO()->getState(), Vector1d(0), prec ); ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) ); } - for (auto F : problem->getTrajectory()->getFrameList()) + for (auto pair_T_F : problem->getTrajectory()->getFrameMap()) { + auto F = pair_T_F.second; if (F->isFixed()) // fixed { - ASSERT_TRUE (F->getP()->getState().isApprox(Vector2d(0,0), prec) ); - ASSERT_TRUE (F->getO()->getState().isApprox(Vector1d(0), prec) ); + ASSERT_MATRIX_APPROX (F->getP()->getState(), Vector2d(0,0), prec ); + ASSERT_MATRIX_APPROX (F->getO()->getState(), Vector1d(0), prec ); } else { @@ -416,8 +420,8 @@ TEST(Problem, perturb) { if ( L->isFixed() ) // fixed { - ASSERT_TRUE (L->getP()->getState().isApprox(Vector2d(1,2), prec) ); - ASSERT_TRUE (L->getO()->getState().isApprox(Vector1d(3), prec) ); + ASSERT_MATRIX_APPROX (L->getP()->getState(), Vector2d(1,2), prec ); + ASSERT_MATRIX_APPROX (L->getO()->getState(), Vector1d(3), prec ); } else { @@ -439,14 +443,14 @@ TEST(Problem, check) intr->ticks_per_wheel_revolution = 100; Vector3d extr(0,0,0); auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr)); - sensor->setStateBlockDynamic("I", true); + sensor->setStateBlockDynamic('I', true); Vector3d pose; pose << 0,0,0; int i = 0; for (TimeStamp t = 0.0; t < 2.0; t += 1.0) { - auto F = problem->emplaceFrame(KEY, t, pose); + auto F = problem->emplaceFrame(t, pose); if (i==0) F->fix(); for (int j = 0; j< 2 ; j++) @@ -477,7 +481,7 @@ TEST(Problem, check) // remove stuff from problem, and re-check - auto F = problem->getLastKeyFrame(); + auto F = problem->getLastFrame(); auto cby = F->getConstrainedByList().front(); @@ -554,6 +558,9 @@ TEST(Problem, getState) WOLF_DEBUG("P () = ", P->getState("P")); WOLF_DEBUG("PO() = ", P->getState("PO")); WOLF_DEBUG("x () = ", P->getState()); + ASSERT_EQ(P->getState("P").size(), 1); + ASSERT_EQ(P->getState("PO").size(), 2); + ASSERT_EQ(P->getState().size(), 2); } @@ -561,6 +568,6 @@ TEST(Problem, getState) int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "Problem.getState"; +// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory"; return RUN_ALL_TESTS(); } diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index ed01245b29c903a7584e932196fb93a2c01c5c20..c3277078df9300439a6dee3721f0084df9751522 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -117,12 +117,16 @@ TEST(ProcessorBase, KeyFrameCallback) capt_odo->setTimeStamp(t); std::cout << "2\n"; +// auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo); +// auto last_ptr = proc_odo_motion->last_ptr_; +// auto last_ptr_frame = last_ptr->getFrame(); proc_odo->captureCallback(capt_odo); std::cout << "3\n"; // Track capt_trk = make_shared<CaptureVoid>(t, sens_trk); std::cout << "4\n"; + problem->print(4,1,1,1, std::cout); proc_trk->captureCallback(capt_trk); std::cout << "5\n"; @@ -130,7 +134,7 @@ TEST(ProcessorBase, KeyFrameCallback) std::cout << "6\n"; // Only odom creating KFs - ASSERT_TRUE( problem->getLastKeyFrame()->getStructure().compare("PO")==0 ); + ASSERT_TRUE( problem->getLastFrame()->getStructure().compare("PO")==0 ); } } diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index b13061e657badbd2ed81e9ea82b0f6078f3803f0..fc30487fe96765f36caea276defddc7abee9a6ac 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -352,9 +352,11 @@ TEST_F(ProcessorDiffDriveTest, linear) data(0) = 100.0 ; // one turn of the wheels data(1) = 100.0 ; + t += 1.0; auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front()); C->process(); + WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose()); // radius is 1.0m, 100 ticks per revolution, so advanced distance is @@ -377,6 +379,7 @@ TEST_F(ProcessorDiffDriveTest, angular) processor->setOrigin(F0); // Straight one turn of the wheels, in one go + t += 1.0; data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse data(1) = 20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp index 90d55137f88921c61c2e79bee1ea19d0f9047111..4e50c870348da1877b3a599c48158730e2df47ab 100644 --- a/test/gtest_processor_loopclosure.cpp +++ b/test/gtest_processor_loopclosure.cpp @@ -34,11 +34,10 @@ protected: bool detectFeatures(CaptureBasePtr cap) override { return true;}; CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override { - for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) - if (kf->isKey()) - for (CaptureBasePtr cap : kf->getCaptureList()) - if (cap != _capture) - return cap; + for (FrameBasePtr kf : *getProblem()->getTrajectory()) + for (CaptureBasePtr cap : kf->getCaptureList()) + if (cap != _capture) + return cap; return nullptr; }; void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override @@ -87,7 +86,7 @@ TEST(ProcessorLoopClosure, installProcessor) // new KF t += dt; - auto kf = problem->emplaceFrame(KEY, t, x); //KF2 + auto kf = problem->emplaceFrame(t, x); //KF2 // emplace a capture in KF auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc); proc_lc->captureCallback(capt_lc); diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 410ebdf1faea8175c8e73435670fdb46de85fd9e..e531defd5721e3374308d67e793a01f682701577 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -12,7 +12,6 @@ #include "core/sensor/sensor_odom_2d.h" #include "core/processor/processor_odom_2d.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -51,11 +50,6 @@ class ProcessorMotion_test : public testing::Test{ Vector2d data; Matrix2d data_cov; -// ProcessorMotion_test() : -// ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()), -// dt(0) -// { } - void SetUp() override { std::string wolf_root = _WOLF_ROOT_DIR; @@ -64,7 +58,7 @@ class ProcessorMotion_test : public testing::Test{ problem = Problem::create("PO", 2); sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml")); ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>()); - params->time_tolerance = 0.25; + params->time_tolerance = 0.5; params->dist_traveled = 100; params->angle_turned = 6.28; params->max_time_span = 2.5*dt; // force KF at every third process() @@ -121,12 +115,12 @@ TEST_F(ProcessorMotion_test, getState_structure) WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose()); } - ASSERT_TRUE (processor->getState("P").count("P")); - ASSERT_FALSE(processor->getState("P").count("O")); - ASSERT_FALSE(processor->getState("O").count("P")); - ASSERT_TRUE (processor->getState("O").count("O")); + ASSERT_TRUE (processor->getState("P").count('P')); + ASSERT_FALSE(processor->getState("P").count('O')); + ASSERT_FALSE(processor->getState("O").count('P')); + ASSERT_TRUE (processor->getState("O").count('O')); - WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState("V")); + WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState('V')); ASSERT_EQ (processor->getState("V").size(), 0); } @@ -147,14 +141,16 @@ TEST_F(ProcessorMotion_test, getState_time_structure) capture->setTimeStamp(t); capture->setData(data); capture->setDataCovariance(data_cov); - processor->captureCallback(capture); + capture->process(); WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose()); } - ASSERT_TRUE (processor->getState(7, "P").count("P")); - ASSERT_FALSE(processor->getState(7, "P").count("O")); - ASSERT_FALSE(processor->getState(7, "O").count("P")); - ASSERT_TRUE (processor->getState(7, "O").count("O")); + problem->print(2,1,1,1); + + ASSERT_TRUE (processor->getState(7, "P").count('P')); + ASSERT_FALSE(processor->getState(7, "P").count('O')); + ASSERT_FALSE(processor->getState(7, "O").count('P')); + ASSERT_TRUE (processor->getState(7, "O").count('O')); WOLF_DEBUG("processor->getState(7, \"V\") = ", processor->getState(7, "V")); ASSERT_EQ (processor->getState(7, "V").size(), 0); @@ -293,6 +289,83 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior) ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8); } +TEST_F(ProcessorMotion_test, mergeCaptures) +{ + // Prior + Vector3d x0; x0 << 0, 0, 0; + Matrix3d P0; P0.setIdentity(); + + data << 1, 2*M_PI/10; // advance in circle + data_cov.setIdentity(); + TimeStamp t(0.0); + + for (int i = 0; i<10; i++) // one full turn exactly + { + t += dt; + capture->setTimeStamp(t); + capture->setData(data); + capture->setDataCovariance(data_cov); + processor->captureCallback(capture); + WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose()); + } + + SensorBasePtr S = processor->getSensor(); + + TimeStamp t_target = 8.5; + FrameBasePtr F_target = problem->emplaceFrame(t_target); + CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast()); + CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target, + "ODOM 2d", + t_target, + sensor, + data, + nullptr); + + // copy initial buffer + MotionBuffer initial_buffer = C_source->getBuffer(); + + processor->splitBuffer(C_source, + t_target, + F_target, + C_target); + + C_target->getBuffer().print(1,1,1,0); + C_source->getBuffer().print(1,1,1,0); + + // merge captures + processor->mergeCaptures(C_target, C_source); + + C_target->getBuffer().print(1,1,1,0); + C_source->getBuffer().print(1,1,1,0); + + // Check that merged buffer is the initial one (before splitting) + EXPECT_EQ(C_source->getBuffer().size(), initial_buffer.size()); + + auto init_it = initial_buffer.begin(); + auto res_it = C_source->getBuffer().begin(); + while (init_it != initial_buffer.end()) + { + EXPECT_EQ(init_it->data_size_, init_it->data_size_); + EXPECT_EQ(init_it->delta_size_, init_it->delta_size_); + EXPECT_EQ(init_it->cov_size_, init_it->cov_size_); + EXPECT_EQ(init_it->calib_size_, init_it->calib_size_); + EXPECT_EQ(init_it->ts_, init_it->ts_); + + EXPECT_MATRIX_APPROX(init_it->data_ ,init_it->data_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->data_cov_,init_it->data_cov_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->delta_, init_it->delta_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->delta_cov_, init_it->delta_cov_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->delta_integr_, init_it->delta_integr_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->delta_integr_cov_, init_it->delta_integr_cov_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->jacobian_delta_, init_it->jacobian_delta_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->jacobian_delta_integr_, init_it->jacobian_delta_integr_, 1e-12); + EXPECT_MATRIX_APPROX(init_it->jacobian_calib_, init_it->jacobian_calib_, 1e-12); + + init_it++; + res_it++; + } +} + TEST_F(ProcessorMotion_test, splitBufferAutoPrior) { // Prior @@ -316,7 +389,7 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior) SensorBasePtr S = processor->getSensor(); TimeStamp t_target = 8.5; - FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target); + FrameBasePtr F_target = problem->emplaceFrame(t_target); CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast()); CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target, "ODOM 2d", @@ -361,7 +434,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior) SensorBasePtr S = processor->getSensor(); TimeStamp t_target = 8.5; - FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target); + FrameBasePtr F_target = problem->emplaceFrame(t_target); CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast()); CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target, "ODOM 2d", @@ -406,7 +479,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior) SensorBasePtr S = processor->getSensor(); TimeStamp t_target = 8.5; - FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target); + FrameBasePtr F_target = problem->emplaceFrame(t_target); CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast()); CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target, "ODOM 2d", @@ -424,6 +497,37 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior) C_source->getBuffer().print(1,1,1,0); } +TEST_F(ProcessorMotion_test, initOdometry) +{ + auto odometry = processor->getOdometry(); + + ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20); + ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20); +} + +TEST_F(ProcessorMotion_test, integrateOdometry) +{ + auto odometry = processor->getOdometry(); + + ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20); + ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20); + + data << 1,0; + capture->setData(data); + + capture->setTimeStamp(capture->getTimeStamp() + 1.0); + capture->process(); + odometry = processor->getOdometry(); + ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(1,0), 1e-20); + ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20); + + capture->setTimeStamp(capture->getTimeStamp() + 1.0); + capture->process(); + odometry = processor->getOdometry(); + ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(2,0), 1e-20); + ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20); +} + int main(int argc, char **argv) { diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp index 6238a5786533fc89df682364305db55661921e92..49593d64afa531c28a8a490de0ba562ed1c13bb2 100644 --- a/test/gtest_processor_tracker_feature_dummy.cpp +++ b/test/gtest_processor_tracker_feature_dummy.cpp @@ -287,8 +287,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process) CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor); cap1->process(); - ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr); - ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap1->getFrame()->id()); + ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr); + ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap1->getFrame()->id()); ASSERT_TRUE(problem->check(0)); //2ND TIME @@ -321,8 +321,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process) CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor); cap5->process(); - ASSERT_TRUE(cap4->getFrame()->isKey()); - ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id()); + ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap4->getFrame()->id()); ASSERT_TRUE(problem->check(0)); // check factors diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp index 44dfbdf14cc1f53ea4962cc7f2517d37ef5210b2..d0fe1b1d8a748df603db674cf7604720c69e4d71 100644 --- a/test/gtest_processor_tracker_landmark_dummy.cpp +++ b/test/gtest_processor_tracker_landmark_dummy.cpp @@ -321,8 +321,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process) CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor); cap1->process(); - ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr); - ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame()); + ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr); + ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap1->getFrame()); //2ND TIME WOLF_DEBUG("Second time..."); @@ -351,7 +351,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process) CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor); cap5->process(); - ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame()); + ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap4->getFrame()); ASSERT_EQ(processor->getOrigin(), cap4); ASSERT_EQ(processor->getLast(), cap5); diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e5efa2dff18a27131a13cb173e064edad2d0a3c2 --- /dev/null +++ b/test/gtest_solver_ceres.cpp @@ -0,0 +1,1522 @@ +/* + * gtest_solver_ptr.cpp + * + * Created on: Jun, 2018 + * Author: jvallve + */ + +#include "core/utils/utils_gtest.h" + +#include "core/problem/problem.h" +#include "core/sensor/sensor_base.h" +#include "core/state_block/state_block.h" +#include "core/capture/capture_void.h" +#include "core/factor/factor_pose_2d.h" +#include "core/factor/factor_quaternion_absolute.h" +#include "core/factor/factor_block_absolute.h" +#include "core/state_block/local_parametrization_angle.h" +#include "core/state_block/local_parametrization_quaternion.h" + +#include "core/solver/solver_manager.h" +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/ceres_wrapper/local_parametrization_wrapper.h" + +#include "ceres/ceres.h" + +#include <iostream> + +using namespace wolf; +using namespace Eigen; + +/* + * Following tests are the same as in gtest_solver_manager.cpp + * (modifications should be applied to both tests) + */ + +StateBlockPtr createStateBlock() +{ + Vector4d state; state << 1, 0, 0, 0; + return std::make_shared<StateBlock>(state); +} + +FactorBasePtr createFactor(StateBlockPtr sb_ptr) +{ + return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy", + VectorXd::Zero(sb_ptr->getSize()), + MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())), + sb_ptr, + nullptr, + false); +} + +TEST(SolverCeres, Create) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // check pointers + EXPECT_EQ(P, solver_ptr->getProblem()); + + // run solver check + EXPECT_TRUE(solver_ptr->check()); +} + +//////////////////////////////////////////////////////// +// FLOATING STATE BLOCKS +TEST(SolverCeres, FloatingStateBlock_Add) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check stateblock + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_DoubleAdd) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // notify stateblock again + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check stateblock + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_AddFix) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock unfixed + EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); + + // Fix frame + sb_ptr->fix(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_AddFixed) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Fix state block + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Local param + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); + sb_ptr->setLocalParametrization(local_ptr); + + // Fix state block + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver manager + solver_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Local param + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); + sb_ptr->setLocalParametrization(local_ptr); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check stateblock localparam + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Remove local param + sb_ptr->removeLocalParametrization(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check stateblock localparam + EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_Remove) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check stateblock + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_AddRemove) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check state block + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check state block + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_DoubleRemove) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // update solver + solver_ptr->update(); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // update solver + solver_ptr->update(); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, FloatingStateBlock_AddUpdated) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Fix + sb_ptr->fix(); + + // Set State + sb_ptr->setState(2*sb_ptr->getState()); + + // Check flags have been set true + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->stateUpdated()); + + // == When an ADD is notified: a ADD notification should be stored == + + // notify state block + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + // check notifications + Notification notif; + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(notif, ADD); + + // == Insert OTHER notifications == + + // Set State --> FLAG + sb_ptr->setState(2*sb_ptr->getState()); + + // Fix --> FLAG + sb_ptr->unfix(); + // Check flag has been set true + EXPECT_TRUE(sb_ptr->fixUpdated()); + + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) + + // == consume empties the notification map == + solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + // Check flags have been reset + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + + // == When an REMOVE is notified: a REMOVE notification should be stored == + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(notif, REMOVE); + + // == REMOVE + ADD: notification map should be empty == + P->notifyStateBlock(sb_ptr,ADD); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + + // == UPDATES should clear the list of notifications == + // notify state block + P->notifyStateBlock(sb_ptr,ADD); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // == ADD + REMOVE: notification map should be empty == + P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr,REMOVE); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); +} + +//////////////////////////////////////////////////////// +// STATE BLOCKS AND FACTOR +TEST(SolverCeres, StateBlockAndFactor_Add) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock (floating for the moment) + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + + // notify factor (state block now not floating) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_DoubleAdd) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // notify stateblock again + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check stateblock + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_Fix) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock unfixed + EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); + + // Fix frame + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_AddFixed) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Fix state block + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Local param + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); + sb_ptr->setLocalParametrization(local_ptr); + + // Fix state block + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // Local param + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); + sb_ptr->setLocalParametrization(local_ptr); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check solver + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->check()); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Remove local param + sb_ptr->removeLocalParametrization(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check solver + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list) + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_RemoveFactor) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating + + // check notifications + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added) + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check state block + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification + + // check notifications + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // state block should be automatically stored as floating + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // factor ADD is posponed + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver manager + solver_ptr->update(); // repeated REMOVE should be ignored + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check state block + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD + + // check notifications + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // state block should be automatically stored as floating + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + + // update solver + solver_ptr->update(); // repeated REMOVE should be ignored + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // Fix + sb_ptr->fix(); + + // Change State + sb_ptr->setState(2*sb_ptr->getState()); + + // Check flags have been set true + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->stateUpdated()); + + // == When an ADD is notified: a ADD notification should be stored == + + // notify state block + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // == Insert OTHER notifications == + + // Set State --> FLAG + sb_ptr->setState(2*sb_ptr->getState()); + + // Fix --> FLAG + sb_ptr->unfix(); + // Check flag has been set true + EXPECT_TRUE(sb_ptr->fixUpdated()); + + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) + + // == consume empties the notification map == + solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + // Check flags have been reset + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + + // == When an REMOVE is notified: a REMOVE notification should be stored == + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(notif, REMOVE); + + // == REMOVE + ADD: notification map should be empty == + P->notifyStateBlock(sb_ptr,ADD); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + + // == UPDATES should clear the list of notifications == + // notify state block + P->notifyStateBlock(sb_ptr,ADD); + + // update solver + solver_ptr->update(); + + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty + + // == ADD + REMOVE: notification map should be empty == + P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr,REMOVE); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); +} + +//////////////////////////////////////////////////////// +// ONLY FACTOR +TEST(SolverCeres, OnlyFactor_Add) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify factor (state block has not been notified) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // factor ADD should be posponed (in the notification list again) + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // checks + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, OnlyFactor_Remove) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify factor (state block has not been notified) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // ADD factor should be posponed (in the notification list again) + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // notify factor + P->notifyFactor(fac_ptr,REMOVE); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // update solver + solver_ptr->update(); // nothing to update + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // checks + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverCeres, OnlyFactor_AddRemove) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify factor (state block has not been notified) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // notify factor + P->notifyFactor(fac_ptr,REMOVE); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // update solver + solver_ptr->update(); // nothing to update + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // checks + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3cd41dde30bc72ecc16fc27d70b8cde5fe327a7b --- /dev/null +++ b/test/gtest_solver_ceres_multithread.cpp @@ -0,0 +1,81 @@ +/* + * gtest_solver_ptr.cpp + * + * Created on: Jun, 2018 + * Author: jvallve + */ + +#include "core/utils/utils_gtest.h" + +#include "core/problem/problem.h" +#include "core/sensor/sensor_base.h" +#include "core/state_block/state_block.h" +#include "core/capture/capture_void.h" +#include "core/factor/factor_pose_2d.h" +#include "core/factor/factor_quaternion_absolute.h" +#include "core/factor/factor_block_absolute.h" +#include "core/state_block/local_parametrization_angle.h" +#include "core/state_block/local_parametrization_quaternion.h" + +#include "core/solver/solver_manager.h" +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/ceres_wrapper/local_parametrization_wrapper.h" + +#include "ceres/ceres.h" + +#include <iostream> + +using namespace wolf; +using namespace Eigen; + +/* + * Following tests are the same as in gtest_solver_manager_multithread.cpp + * (modifications should be applied to both tests) + */ + +TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications) +{ + double Dt = 5.0; + ProblemPtr P = Problem::create("PO", 2); + auto solver_ptr = std::make_shared<SolverCeres>(P); + + // loop updating (without sleep) + std::thread t([&](){ + auto start_t = std::chrono::high_resolution_clock::now(); + while (true) + { + solver_ptr->update(); + ASSERT_TRUE(solver_ptr->check()); + if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt) + break; + }}); + + // loop emplacing and removing frames (window of 10 KF) + auto start = std::chrono::high_resolution_clock::now(); + TimeStamp ts(0); + while (true) + { + // Emplace Frame, Capture, feature and factor pose 2d + FrameBasePtr F = P->emplaceFrame(ts, P->stateZero()); + auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr); + auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity()); + auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); + + ts += 1.0; + + if (P->getTrajectory()->getFrameMap().size() > 10) + (*P->getTrajectory()->begin())->remove(); + + if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt) + break; + } + + t.join(); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 7e212b9971b5bf8fb1a46f94141ce61a1a5aab08..2eb3f46e927270a7e61abb03734d01898722f471 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -13,8 +13,8 @@ #include "core/state_block/state_block.h" #include "core/capture/capture_void.h" #include "core/factor/factor_pose_2d.h" -#include "core/state_block/local_parametrization_base.h" -#include "core/state_block/local_parametrization_angle.h" +#include "core/factor/factor_block_absolute.h" +#include "core/state_block/local_parametrization_quaternion.h" #include "dummy/solver_manager_dummy.h" #include <iostream> @@ -23,568 +23,1490 @@ using namespace wolf; using namespace Eigen; +/* + * Following tests are the same as in gtest_solver_ceres.cpp + * (modifications should be applied to both tests) + */ + +StateBlockPtr createStateBlock() +{ + Vector4d state; state << 1, 0, 0, 0; + return std::make_shared<StateBlock>(state); +} + +FactorBasePtr createFactor(StateBlockPtr sb_ptr) +{ + return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy", + VectorXd::Zero(sb_ptr->getSize()), + MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())), + sb_ptr, + nullptr, + false); +} + TEST(SolverManager, Create) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); - // check double pointers to branches - ASSERT_EQ(P, solver_manager_ptr->getProblem()); + // check pointers + EXPECT_EQ(P, solver_ptr->getProblem()); + + // run solver check + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddStateBlock) +//////////////////////////////////////////////////////// +// FLOATING STATE BLOCKS +TEST(SolverManager, FloatingStateBlock_Add) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check stateblock - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, DoubleAddStateBlock) +TEST(SolverManager, FloatingStateBlock_DoubleAdd) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // update solver - solver_manager_ptr->update(); + solver_ptr->update(); - // add stateblock again + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // notify stateblock again P->notifyStateBlock(sb_ptr,ADD); + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check stateblock - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, UpdateStateBlock) +TEST(SolverManager, FloatingStateBlock_AddFix) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // check stateblock unfixed - ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); // Fix frame sb_ptr->fix(); + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // update solver manager - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // check stateblock fixed - ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddUpdateStateBlock) +TEST(SolverManager, FloatingStateBlock_AddFixed) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // Fix state block sb_ptr->fix(); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // update solver manager - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // check stateblock fixed - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); - ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddUpdateLocalParamStateBlock) +TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // Local param - LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); sb_ptr->setLocalParametrization(local_ptr); // Fix state block sb_ptr->fix(); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // update solver manager - solver_manager_ptr->update(); + solver_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // check stateblock fixed - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); - ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); - ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock) +TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); // Local param - LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); sb_ptr->setLocalParametrization(local_ptr); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // update solver manager - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check stateblock localparam - ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); // Remove local param sb_ptr->removeLocalParametrization(); // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->localParamUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); // update solver manager - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check stateblock localparam - ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr)); + EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, RemoveStateBlock) +TEST(SolverManager, FloatingStateBlock_Remove) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // remove state_block P->notifyStateBlock(sb_ptr,REMOVE); + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check stateblock - ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddRemoveStateBlock) +TEST(SolverManager, FloatingStateBlock_AddRemove) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr,REMOVE); // should cancel out the ADD + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check state block - ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, RemoveUpdateStateBlock) +TEST(SolverManager, FloatingStateBlock_AddRemoveAdd) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add state_block + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check state block + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, DoubleRemoveStateBlock) +TEST(SolverManager, FloatingStateBlock_DoubleRemove) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // add stateblock + // notify stateblock P->notifyStateBlock(sb_ptr,ADD); + // update solver + solver_ptr->update(); + // remove state_block P->notifyStateBlock(sb_ptr,REMOVE); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); // remove state_block P->notifyStateBlock(sb_ptr,REMOVE); + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + // update solver manager - solver_manager_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddUpdatedStateBlock) +TEST(SolverManager, FloatingStateBlock_AddUpdated) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); // Fix sb_ptr->fix(); // Set State - Vector2d state_2 = 2*state; - sb_ptr->setState(state_2); + sb_ptr->setState(2*sb_ptr->getState()); // Check flags have been set true - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->stateUpdated()); // == When an ADD is notified: a ADD notification should be stored == - // add state_block + // notify state block P->notifyStateBlock(sb_ptr,ADD); + // check notifications + // check notifications Notification notif; - ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); - ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); - ASSERT_EQ(notif, ADD); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(notif, ADD); // == Insert OTHER notifications == // Set State --> FLAG - state_2 = 2*state; - sb_ptr->setState(state_2); + sb_ptr->setState(2*sb_ptr->getState()); // Fix --> FLAG sb_ptr->unfix(); // Check flag has been set true - ASSERT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); - ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) // == consume empties the notification map == - solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); - ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); + solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // Check flags have been reset - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); // == When an REMOVE is notified: a REMOVE notification should be stored == // remove state_block P->notifyStateBlock(sb_ptr,REMOVE); - ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); - ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); - ASSERT_EQ(notif, REMOVE); + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(notif, REMOVE); // == REMOVE + ADD: notification map should be empty == P->notifyStateBlock(sb_ptr,ADD); - ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // == UPDATES should clear the list of notifications == - // add state_block + // notify state block P->notifyStateBlock(sb_ptr,ADD); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); - ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // == ADD + REMOVE: notification map should be empty == P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,REMOVE); - ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); +} + +//////////////////////////////////////////////////////// +// STATE BLOCKS AND FACTOR +TEST(SolverManager, StateBlockAndFactor_Add) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock (floating for the moment) + P->notifyStateBlock(sb_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + + // notify factor (state block now not floating) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_DoubleAdd) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // notify stateblock again + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check stateblock + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_Fix) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock unfixed + EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); + + // Fix frame + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_AddFixed) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Fix state block + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Local param + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); + sb_ptr->setLocalParametrization(local_ptr); + + // Fix state block + sb_ptr->fix(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // check stateblock fixed + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // Local param + LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); + sb_ptr->setLocalParametrization(local_ptr); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver manager + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // check solver + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->check()); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->localParamUpdated()); + + // Remove local param + sb_ptr->removeLocalParametrization(); + + // check flags + EXPECT_FALSE(sb_ptr->stateUpdated()); + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->localParamUpdated()); + + // update solver manager + solver_ptr->update(); + + // check solver + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list) + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_RemoveFactor) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating + + // check notifications + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + + // update solver + solver_ptr->update(); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added) + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check state block + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddFactor) +TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); + // notify factor + P->notifyFactor(fac_ptr,ADD); - // notification + // check notifications Notification notif; - ASSERT_TRUE(P->getFactorNotification(c,notif)); - ASSERT_EQ(notif, ADD); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification + + // check notifications + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); // state block should be automatically stored as floating - // check factor - ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c)); + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); + + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, RemoveFactor) +TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); + + // notify factor + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); // factor ADD is posponed + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); + + // check notifications + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(REMOVE, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver manager + solver_ptr->update(); // repeated REMOVE should be ignored + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // check state block + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify stateblock + P->notifyStateBlock(sb_ptr,ADD); - // add factor - P->notifyFactor(c,REMOVE); + // notify factor + P->notifyFactor(fac_ptr,ADD); - // notification + // check notifications Notification notif; - ASSERT_TRUE(P->getFactorNotification(c,notif)); - ASSERT_EQ(notif, REMOVE); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD + + // check notifications + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // update solver + solver_ptr->update(); // state block should be automatically stored as floating + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // remove state_block + P->notifyFactor(fac_ptr,REMOVE); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(REMOVE, notif); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); // repeated REMOVE should be ignored - // check factor - ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); + // checks + EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, AddRemoveFactor) +TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // Fix + sb_ptr->fix(); + + // Change State + sb_ptr->setState(2*sb_ptr->getState()); + + // Check flags have been set true + EXPECT_TRUE(sb_ptr->fixUpdated()); + EXPECT_TRUE(sb_ptr->stateUpdated()); + + // == When an ADD is notified: a ADD notification should be stored == - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + // notify state block + P->notifyStateBlock(sb_ptr,ADD); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); + // notify factor + P->notifyFactor(fac_ptr,ADD); - // notification + // check notifications Notification notif; - ASSERT_TRUE(P->getFactorNotification(c,notif)); - ASSERT_EQ(notif, ADD); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(ADD, notif); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // == Insert OTHER notifications == + + // Set State --> FLAG + sb_ptr->setState(2*sb_ptr->getState()); + + // Fix --> FLAG + sb_ptr->unfix(); + // Check flag has been set true + EXPECT_TRUE(sb_ptr->fixUpdated()); + + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) + + // == consume empties the notification map == + solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + // Check flags have been reset + EXPECT_FALSE(sb_ptr->fixUpdated()); + EXPECT_FALSE(sb_ptr->stateUpdated()); + + // == When an REMOVE is notified: a REMOVE notification should be stored == + + // remove state_block + P->notifyStateBlock(sb_ptr,REMOVE); - // add factor - P->notifyFactor(c,REMOVE); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); + EXPECT_EQ(notif, REMOVE); - // notification - ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out - ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out + // == REMOVE + ADD: notification map should be empty == + P->notifyStateBlock(sb_ptr,ADD); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + + // == UPDATES should clear the list of notifications == + // notify state block + P->notifyStateBlock(sb_ptr,ADD); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); + + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty - // check factor - ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); + // == ADD + REMOVE: notification map should be empty == + P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr,REMOVE); + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); } -TEST(SolverManager, DoubleRemoveFactor) +//////////////////////////////////////////////////////// +// ONLY FACTOR +TEST(SolverManager, OnlyFactor_Add) { - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // Create State block - Vector2d state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + auto sb_ptr = createStateBlock(); - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero() ); + // Create Factor + auto fac_ptr = createFactor(sb_ptr); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); - auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); + // notify factor (state block has not been notified) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); // factor ADD should be posponed (in the notification list again) + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); - // remove factor - P->notifyFactor(c,REMOVE); + // checks + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); +} + +TEST(SolverManager, OnlyFactor_Remove) +{ + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify factor (state block has not been notified) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); // ADD factor should be posponed (in the notification list again) - // remove factor - P->notifyFactor(c,REMOVE); + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // notify factor + P->notifyFactor(fac_ptr,REMOVE); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); // update solver - solver_manager_ptr->update(); + solver_ptr->update(); // nothing to update + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); - // check factor - ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); + // checks + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); } -TEST(SolverManager, MultiThreadingTruncatedNotifications) +TEST(SolverManager, OnlyFactor_AddRemove) { - double Dt = 5.0; - ProblemPtr P = Problem::create("PO", 2); - SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); - - // loop updating (without sleep) - std::thread t([&](){ - auto start_t = std::chrono::high_resolution_clock::now(); - while (true) - { - solver_manager_ptr->update(); - if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt) - break; - }}); - - // loop emplacing and removing frames (window of 10 KF) - auto start = std::chrono::high_resolution_clock::now(); - while (true) - { - // Emplace Frame, Capture, feature and factor pose 2d - FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero()); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity()); - auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); - - if (P->getTrajectory()->getFrameList().size() > 10) - P->getTrajectory()->getFrameList().front()->remove(); - - if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt) - break; - } - - t.join(); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // Create State block + auto sb_ptr = createStateBlock(); + + // Create Factor + auto fac_ptr = createFactor(sb_ptr); + + // notify factor (state block has not been notified) + P->notifyFactor(fac_ptr,ADD); + + // check notifications + Notification notif; + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); + EXPECT_EQ(ADD, notif); + + // notify factor + P->notifyFactor(fac_ptr,REMOVE); + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // update solver + solver_ptr->update(); // nothing to update + + // check notifications + EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(),0); + + // checks + EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); + EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr)); + EXPECT_TRUE(solver_ptr->check()); } int main(int argc, char **argv) diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4e772c1bb74747d80a6ceda04d62a679f323a4e3 --- /dev/null +++ b/test/gtest_solver_manager_multithread.cpp @@ -0,0 +1,75 @@ +/* + * gtest_solver_manager.cpp + * + * Created on: Jun, 2018 + * Author: jvallve + */ + +#include "core/utils/utils_gtest.h" + + +#include "core/problem/problem.h" +#include "core/sensor/sensor_base.h" +#include "core/state_block/state_block.h" +#include "core/capture/capture_void.h" +#include "core/factor/factor_pose_2d.h" +#include "core/factor/factor_block_absolute.h" +#include "core/state_block/local_parametrization_quaternion.h" +#include "dummy/solver_manager_dummy.h" + +#include <iostream> +#include <thread> + +using namespace wolf; +using namespace Eigen; + +/* + * Following tests are the same as in gtest_solver_ceres_multithread.cpp + * (modifications should be applied to both tests) + */ + +TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications) +{ + double Dt = 5.0; + ProblemPtr P = Problem::create("PO", 2); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + + // loop updating (without sleep) + std::thread t([&](){ + auto start_t = std::chrono::high_resolution_clock::now(); + while (true) + { + solver_ptr->update(); + ASSERT_TRUE(solver_ptr->check()); + if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt) + break; + }}); + + // loop emplacing and removing frames (window of 10 KF) + auto start = std::chrono::high_resolution_clock::now(); + TimeStamp ts(0); + while (true) + { + // Emplace Frame, Capture, feature and factor pose 2d + FrameBasePtr F = P->emplaceFrame(ts, P->stateZero()); + auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr); + auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity()); + auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); + + ts += 1.0; + + if (P->getTrajectory()->getFrameMap().size() > 10) + (*P->getTrajectory()->begin())->remove(); + + if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt) + break; + } + + t.join(); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp index 56f705aa3b9c3689a076989fcd8e30896d80fe13..f4bcea67bd4e51af793e026f1496f2591fcfe98a 100644 --- a/test/gtest_state_composite.cpp +++ b/test/gtest_state_composite.cpp @@ -25,9 +25,9 @@ class StateBlockCompositeInit : public testing::Test void SetUp() override { - sbp = states.emplace("P", Vector3d(1,2,3)); - sbv = states.emplace("V", Vector3d(4,5,6)); - sbq = states.emplace<StateQuaternion>("Q", Vector4d(.5,.5,.5,.5)); + sbp = states.emplace('P', Vector3d(1,2,3)); + sbv = states.emplace('V', Vector3d(4,5,6)); + sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5)); sbx = std::make_shared<StateBlock>(Vector3d(7,8,9)); } @@ -57,8 +57,8 @@ TEST_F(StateBlockCompositeInit, emplace) TEST_F(StateBlockCompositeInit, has_key) { - ASSERT_TRUE(states.has("P")); - ASSERT_FALSE(states.has("X")); + ASSERT_TRUE(states.has('P')); + ASSERT_FALSE(states.has('X')); } TEST_F(StateBlockCompositeInit, has_sb) @@ -69,22 +69,22 @@ TEST_F(StateBlockCompositeInit, has_sb) TEST_F(StateBlockCompositeInit, at) { - ASSERT_EQ(states.at("P"), sbp); + ASSERT_EQ(states.at('P'), sbp); - ASSERT_EQ(states.at("X"), nullptr); + ASSERT_EQ(states.at('X'), nullptr); } TEST_F(StateBlockCompositeInit, set_sb) { - states.set("P", sbx); + states.set('P', sbx); - ASSERT_EQ(states.at("P"), sbx); + ASSERT_EQ(states.at('P'), sbx); - states.set("P", sbp); + states.set('P', sbp); - ASSERT_DEATH(states.set("X", sbx), ""); + ASSERT_DEATH(states.set('X', sbx), ""); - ASSERT_EQ(states.at("X"), nullptr); + ASSERT_EQ(states.at('X'), nullptr); } TEST_F(StateBlockCompositeInit, set_vec) @@ -92,13 +92,13 @@ TEST_F(StateBlockCompositeInit, set_vec) Vector3d p(11,22,33); Vector3d x(55,66,77); - states.set("P", p); + states.set('P', p); - ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20); + ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20); - ASSERT_DEATH(states.set("X", x), ""); + ASSERT_DEATH(states.set('X', x), ""); - ASSERT_EQ(states.at("X"), nullptr); + ASSERT_EQ(states.at('X'), nullptr); } TEST_F(StateBlockCompositeInit, set_vectors) @@ -109,31 +109,31 @@ TEST_F(StateBlockCompositeInit, set_vectors) states.setVectors("PQ", {p, q}); - ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20); - ASSERT_MATRIX_APPROX(states.at("Q")->getState(), q, 1e-20); + ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20); + ASSERT_MATRIX_APPROX(states.at('Q')->getState(), q, 1e-20); ASSERT_DEATH(states.setVectors("PX", {p,x}), ""); - ASSERT_EQ(states.at("X"), nullptr); + ASSERT_EQ(states.at('X'), nullptr); } TEST_F(StateBlockCompositeInit, key_ref) { - std::string key; + char key; ASSERT_TRUE(states.key(sbp, key)); - ASSERT_EQ(key, "P"); + ASSERT_EQ(key, 'P'); ASSERT_FALSE(states.key(sbx, key)); - ASSERT_EQ(key, ""); + ASSERT_EQ(key, '0'); } TEST_F(StateBlockCompositeInit, key_return) { // existing key - ASSERT_EQ(states.key(sbp), "P"); + ASSERT_EQ(states.key(sbp), 'P'); - // non existing key returns empty string - ASSERT_EQ(states.key(sbx), ""); + // non existing key returns zero char + ASSERT_EQ(states.key(sbx), '0'); } TEST_F(StateBlockCompositeInit, find) @@ -147,81 +147,81 @@ TEST_F(StateBlockCompositeInit, find) TEST_F(StateBlockCompositeInit, add) { - states.add("X", sbx); + states.add('X', sbx); - ASSERT_EQ(states.at("X"), sbx); + ASSERT_EQ(states.at('X'), sbx); } TEST_F(StateBlockCompositeInit, remove) { // remove existing block - states.remove("V"); + states.remove('V'); ASSERT_EQ(states.getStateBlockMap().size(), 2); // remove non existing block -- no effect - states.remove("X"); + states.remove('X'); ASSERT_EQ(states.getStateBlockMap().size(), 2); } TEST_F(StateBlockCompositeInit, perturb) { - ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3)); - ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3)); - ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3)); + ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3)); + ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3)); + ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3)); states.perturb(0.1); // values have moved wrt original - ASSERT_FALSE(states.at("P")->getState().isApprox(Vector3d(1,2,3), 1e-3)); - ASSERT_FALSE(states.at("V")->getState().isApprox(Vector3d(4,5,6), 1e-3)); - ASSERT_FALSE(states.at("Q")->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3)); + ASSERT_FALSE(states.at('P')->getState().isApprox(Vector3d(1,2,3), 1e-3)); + ASSERT_FALSE(states.at('V')->getState().isApprox(Vector3d(4,5,6), 1e-3)); + ASSERT_FALSE(states.at('Q')->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3)); } TEST_F(StateBlockCompositeInit, setIdentity) { - ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3)); - ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3)); - ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3)); + ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3)); + ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3)); + ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3)); states.setIdentity(); // values have moved wrt original - ASSERT_TRUE(states.at("P")->getState().isApprox(Vector3d(0,0,0), 1e-10)); - ASSERT_TRUE(states.at("V")->getState().isApprox(Vector3d(0,0,0), 1e-10)); - ASSERT_TRUE(states.at("Q")->getState().isApprox(Vector4d(0,0,0,1), 1e-10)); + ASSERT_TRUE(states.at('P')->getState().isApprox(Vector3d(0,0,0), 1e-10)); + ASSERT_TRUE(states.at('V')->getState().isApprox(Vector3d(0,0,0), 1e-10)); + ASSERT_TRUE(states.at('Q')->getState().isApprox(Vector4d(0,0,0,1), 1e-10)); } TEST_F(StateBlockCompositeInit, identity) { VectorComposite v = states.identity(); - ASSERT_TRUE(v.at("P").isApprox(Vector3d(0,0,0), 1e-10)); - ASSERT_TRUE(v.at("V").isApprox(Vector3d(0,0,0), 1e-10)); - ASSERT_TRUE(v.at("Q").isApprox(Vector4d(0,0,0,1), 1e-10)); + ASSERT_TRUE(v.at('P').isApprox(Vector3d(0,0,0), 1e-10)); + ASSERT_TRUE(v.at('V').isApprox(Vector3d(0,0,0), 1e-10)); + ASSERT_TRUE(v.at('Q').isApprox(Vector4d(0,0,0,1), 1e-10)); } TEST_F(StateBlockCompositeInit, fix) { states.fix(); - ASSERT_TRUE(states.at("P")->isFixed()); - ASSERT_TRUE(states.at("V")->isFixed()); - ASSERT_TRUE(states.at("Q")->isFixed()); + ASSERT_TRUE(states.at('P')->isFixed()); + ASSERT_TRUE(states.at('V')->isFixed()); + ASSERT_TRUE(states.at('Q')->isFixed()); } TEST_F(StateBlockCompositeInit, unfix) { states.fix(); - ASSERT_TRUE(states.at("P")->isFixed()); - ASSERT_TRUE(states.at("V")->isFixed()); - ASSERT_TRUE(states.at("Q")->isFixed()); + ASSERT_TRUE(states.at('P')->isFixed()); + ASSERT_TRUE(states.at('V')->isFixed()); + ASSERT_TRUE(states.at('Q')->isFixed()); states.unfix(); - ASSERT_FALSE(states.at("P")->isFixed()); - ASSERT_FALSE(states.at("V")->isFixed()); - ASSERT_FALSE(states.at("Q")->isFixed()); + ASSERT_FALSE(states.at('P')->isFixed()); + ASSERT_FALSE(states.at('V')->isFixed()); + ASSERT_FALSE(states.at('Q')->isFixed()); } TEST_F(StateBlockCompositeInit, isFixed) @@ -230,7 +230,7 @@ TEST_F(StateBlockCompositeInit, isFixed) ASSERT_TRUE(states.isFixed()); - states.at("P")->unfix(); + states.at('P')->unfix(); ASSERT_FALSE(states.isFixed()); } @@ -245,23 +245,23 @@ TEST(VectorComposite, constructor_empty) TEST(VectorComposite, constructor_copy) { VectorComposite u; - u.emplace("a", Vector2d(1,2)); - u.emplace("b", Vector3d(3,4,5)); + u.emplace('a', Vector2d(1,2)); + u.emplace('b', Vector3d(3,4,5)); VectorComposite v(u); ASSERT_FALSE(v.empty()); - ASSERT_MATRIX_APPROX(u["a"], v["a"], 1e-20); - ASSERT_MATRIX_APPROX(u["b"], v["b"], 1e-20); + ASSERT_MATRIX_APPROX(u['a'], v['a'], 1e-20); + ASSERT_MATRIX_APPROX(u['b'], v['b'], 1e-20); } TEST(VectorComposite, constructor_from_list) { VectorComposite v(Vector4d(1,2,3,4), "ab", {3,1}); - ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20); - ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20); + ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20); + ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20); } TEST(VectorComposite, set) @@ -270,8 +270,8 @@ TEST(VectorComposite, set) v.set(Vector4d(1,2,3,4), "ab", {3,1}); - ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20); - ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20); + ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20); + ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20); } TEST(VectorComposite, operatorStream) @@ -280,8 +280,8 @@ TEST(VectorComposite, operatorStream) VectorComposite x; - x.emplace("P", Vector2d(1,1)); - x.emplace("O", Vector3d(2,2,2)); + x.emplace('P', Vector2d(1,1)); + x.emplace('O', Vector3d(2,2,2)); WOLF_DEBUG("X = ", x); } @@ -290,54 +290,54 @@ TEST(VectorComposite, operatorPlus) { VectorComposite x, y; - x.emplace("P", Vector2d(1,1)); - x.emplace("O", Vector3d(2,2,2)); - y.emplace("P", Vector2d(1,1)); - y.emplace("O", Vector3d(2,2,2)); + x.emplace('P', Vector2d(1,1)); + x.emplace('O', Vector3d(2,2,2)); + y.emplace('P', Vector2d(1,1)); + y.emplace('O', Vector3d(2,2,2)); WOLF_DEBUG("x + y = ", x + y); - ASSERT_MATRIX_APPROX((x+y).at("P"), Vector2d(2,2), 1e-20); - ASSERT_MATRIX_APPROX((x+y).at("O"), Vector3d(4,4,4), 1e-20); + ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20); + ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20); } TEST(VectorComposite, operatorMinus) { VectorComposite x, y; - x.emplace("P", Vector2d(3,3)); - x.emplace("O", Vector3d(6,6,6)); - y.emplace("P", Vector2d(1,1)); - y.emplace("O", Vector3d(2,2,2)); + x.emplace('P', Vector2d(3,3)); + x.emplace('O', Vector3d(6,6,6)); + y.emplace('P', Vector2d(1,1)); + y.emplace('O', Vector3d(2,2,2)); WOLF_DEBUG("x = ", x); WOLF_DEBUG("y = ", y); WOLF_DEBUG("x - y = ", x - y); - ASSERT_MATRIX_APPROX((x-y).at("P"), Vector2d(2,2), 1e-20); - ASSERT_MATRIX_APPROX((x-y).at("O"), Vector3d(4,4,4), 1e-20); + ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20); + ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20); } TEST(VectorComposite, unary_Minus) { VectorComposite x, y; - x.emplace("P", Vector2d(1,1)); - x.emplace("O", Vector3d(2,2,2)); + x.emplace('P', Vector2d(1,1)); + x.emplace('O', Vector3d(2,2,2)); WOLF_DEBUG("-x = ", -x); - ASSERT_MATRIX_APPROX((-x).at("P"), Vector2d(-1,-1), 1e-20); - ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20); + ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20); + ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20); } TEST(VectorComposite, stateVector) { VectorComposite x; - x.emplace("P", Vector2d(1,1)); - x.emplace("O", Vector3d(2,2,2)); - x.emplace("V", Vector4d(3,3,3,3)); + x.emplace('P', Vector2d(1,1)); + x.emplace('O', Vector3d(2,2,2)); + x.emplace('V', Vector4d(3,3,3,3)); VectorXd y(5); y<<1,1,2,2,2; ASSERT_MATRIX_APPROX(x.vector("PO"), y, 1e-20); @@ -365,7 +365,7 @@ TEST(MatrixComposite, Constructor_structure) MatrixComposite M("PO", "POV"); ASSERT_EQ(M.size() , 2); - ASSERT_EQ(M.at("P").size() , 3); + ASSERT_EQ(M.at('P').size() , 3); ASSERT_TRUE(M.check()); } @@ -375,10 +375,10 @@ TEST(MatrixComposite, Constructor_structure_sizes) MatrixComposite M("PO", {3,4}, "POV", {3,4,3}); ASSERT_EQ(M.size() , 2); - ASSERT_EQ(M.at("P").size() , 3); + ASSERT_EQ(M.at('P').size() , 3); - ASSERT_EQ(M.at("P","O").rows() , 3); - ASSERT_EQ(M.at("P","O").cols() , 4); + ASSERT_EQ(M.at('P','O').rows() , 3); + ASSERT_EQ(M.at('P','O').cols() , 4); ASSERT_EQ(M.matrix("PO","POV").rows() , 7); ASSERT_EQ(M.matrix("PO","POV").cols() , 10); @@ -393,16 +393,16 @@ TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes) MatrixComposite M(m, "PO", {3,4}, "POV", {3,4,3}); ASSERT_EQ(M.size() , 2); - ASSERT_EQ(M.at("P").size() , 3); + ASSERT_EQ(M.at('P').size() , 3); - ASSERT_EQ(M.at("P","O").rows() , 3); - ASSERT_EQ(M.at("P","O").cols() , 4); + ASSERT_EQ(M.at('P','O').rows() , 3); + ASSERT_EQ(M.at('P','O').cols() , 4); ASSERT_EQ(M.matrix("PO","POV").rows() , 7); ASSERT_EQ(M.matrix("PO","POV").cols() , 10); - ASSERT_MATRIX_APPROX(M.at("P","O"), m.block(0,3,3,4), 1e-20); - ASSERT_MATRIX_APPROX(M.at("O","V"), m.block(3,7,4,3), 1e-20); + ASSERT_MATRIX_APPROX(M.at('P','O'), m.block(0,3,3,4), 1e-20); + ASSERT_MATRIX_APPROX(M.at('O','V'), m.block(3,7,4,3), 1e-20); ASSERT_TRUE(M.check()); } @@ -462,10 +462,10 @@ TEST(MatrixComposite, emplace_operatorStream) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - ASSERT_TRUE(M.emplace("P", "P", Mpp)); - ASSERT_TRUE(M.emplace("P", "O", Mpo)); - ASSERT_TRUE(M.emplace("O", "P", Mop)); - ASSERT_TRUE(M.emplace("O", "O", Moo)); + ASSERT_TRUE(M.emplace('P', 'P', Mpp)); + ASSERT_TRUE(M.emplace('P', 'O', Mpo)); + ASSERT_TRUE(M.emplace('O', 'P', Mop)); + ASSERT_TRUE(M.emplace('O', 'O', Moo)); cout << "M = " << M << endl; } @@ -485,14 +485,14 @@ TEST(MatrixComposite, emplace_operatorStream) // Mop.setOnes(); Mop *= 3; // Moo.setOnes(); Moo *= 4; // -// M.emplace("P", "P", Mpp); -// ASSERT_MATRIX_APPROX( M["P"]["P"], Mpp, 1e-20); +// M.emplace('P', 'P', Mpp); +// ASSERT_MATRIX_APPROX( M['P']['P'], Mpp, 1e-20); // -// M.emplace("P", "O", Mpo); -// ASSERT_MATRIX_APPROX( M["P"]["O"], Mpo, 1e-20); +// M.emplace('P', 'O', Mpo); +// ASSERT_MATRIX_APPROX( M['P']['O'], Mpo, 1e-20); // // // return default empty matrix if block not present -// MatrixXd N = M["O"]["O"]; +// MatrixXd N = M['O']['O']; // ASSERT_EQ(N.rows(), 0); // ASSERT_EQ(N.cols(), 0); //} @@ -512,14 +512,14 @@ TEST(MatrixComposite, emplace_operatorStream) // Mop.setOnes(); Mop *= 3; // Moo.setOnes(); Moo *= 4; // -// M.emplace("P", "P", Mpp); -// ASSERT_MATRIX_APPROX( M("P", "P"), Mpp, 1e-20); +// M.emplace('P', 'P', Mpp); +// ASSERT_MATRIX_APPROX( M('P', 'P'), Mpp, 1e-20); // -// M.emplace("P", "O", Mpo); -// ASSERT_MATRIX_APPROX( M("P", "O"), Mpo, 1e-20); +// M.emplace('P', 'O', Mpo); +// ASSERT_MATRIX_APPROX( M('P', 'O'), Mpo, 1e-20); // // // return default empty matrix if block not present -// MatrixXd N = M("O", "O"); +// MatrixXd N = M('O', 'O'); // ASSERT_EQ(N.rows(), 0); // ASSERT_EQ(N.cols(), 0); //} @@ -539,14 +539,14 @@ TEST(MatrixComposite, operatorAt) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - M.emplace("P", "P", Mpp); - ASSERT_MATRIX_APPROX( M.at("P", "P"), Mpp, 1e-20); + M.emplace('P', 'P', Mpp); + ASSERT_MATRIX_APPROX( M.at('P', 'P'), Mpp, 1e-20); - M.emplace("P", "O", Mpo); - ASSERT_MATRIX_APPROX( M.at("P", "O"), Mpo, 1e-20); + M.emplace('P', 'O', Mpo); + ASSERT_MATRIX_APPROX( M.at('P', 'O'), Mpo, 1e-20); // error if block not present - ASSERT_DEATH(MatrixXd N = M.at("O", "O"), ""); + ASSERT_DEATH(MatrixXd N = M.at('O', 'O'), ""); } TEST(MatrixComposite, productScalar) @@ -563,10 +563,10 @@ TEST(MatrixComposite, productScalar) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - M.emplace("P", "P", Mpp); - M.emplace("P", "O", Mpo); - M.emplace("O", "P", Mop); - M.emplace("O", "O", Moo); + M.emplace('P', 'P', Mpp); + M.emplace('P', 'O', Mpo); + M.emplace('O', 'P', Mop); + M.emplace('O', 'O', Moo); WOLF_DEBUG("M = " , M); @@ -589,8 +589,8 @@ TEST(MatrixComposite, productVector) osize = 3; VectorComposite x; - x.emplace("P", Vector2d(1,1)); - x.emplace("O", Vector3d(2,2,2)); + x.emplace('P', Vector2d(1,1)); + x.emplace('O', Vector3d(2,2,2)); cout << "x= " << x << endl; @@ -604,10 +604,10 @@ TEST(MatrixComposite, productVector) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - M.emplace("P", "P", Mpp); - M.emplace("P", "O", Mpo); - M.emplace("O", "P", Mop); - M.emplace("O", "O", Moo); + M.emplace('P', 'P', Mpp); + M.emplace('P', 'O', Mpo); + M.emplace('O', 'P', Mop); + M.emplace('O', 'O', Moo); WOLF_DEBUG("M = " , M); VectorComposite y; @@ -628,15 +628,15 @@ TEST(MatrixComposite, productVector) Vector2d yp(14,14); Vector3d yo(30,30,30); - ASSERT_MATRIX_APPROX(y.at("P"), yp, 1e-20); - ASSERT_MATRIX_APPROX(y.at("O"), yo, 1e-20); + ASSERT_MATRIX_APPROX(y.at('P'), yp, 1e-20); + ASSERT_MATRIX_APPROX(y.at('O'), yo, 1e-20); // throw if x has extra blocks - // x.emplace("V", Vector2d(3,3)); + // x.emplace('V', Vector2d(3,3)); // ASSERT_DEATH(y = M * x , ""); // M * x --> does not die if x has extra blocks wrt M // throw if x has missing blocks - // x.erase("O"); + // x.erase('O'); // cout << "x = " << x << endl; // ASSERT_DEATH(y = M * x , ""); // M * x --> exception if x has missing blocks wrt M, not caught by ASSERT_DEATH @@ -657,10 +657,10 @@ TEST(MatrixComposite, product) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - M.emplace("P", "P", Mpp); - M.emplace("P", "O", Mpo); - M.emplace("O", "P", Mop); - M.emplace("O", "O", Moo); + M.emplace('P', 'P', Mpp); + M.emplace('P', 'O', Mpo); + M.emplace('O', 'P', Mop); + M.emplace('O', 'O', Moo); WOLF_DEBUG("M = " , M); MatrixXd Noo(osize,osize), Nov(osize, vsize), Npo(psize,osize), Npv(psize,vsize); @@ -669,10 +669,10 @@ TEST(MatrixComposite, product) Npo.setOnes(); Npo *= 3; Npv.setOnes(); Npv *= 4; - N.emplace("O", "O", Noo); - N.emplace("O", "V", Nov); - N.emplace("P", "O", Npo); - N.emplace("P", "V", Npv); + N.emplace('O', 'O', Noo); + N.emplace('O', 'V', Nov); + N.emplace('P', 'O', Npo); + N.emplace('P', 'V', Npv); WOLF_DEBUG("N = " , N); MatrixComposite MN; @@ -693,10 +693,10 @@ TEST(MatrixComposite, product) MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22); MatrixXd MNov(MatrixXd::Ones(osize,vsize) * 32); - ASSERT_MATRIX_APPROX(MN.at("P","O"), MNpo, 1e-20); - ASSERT_MATRIX_APPROX(MN.at("P","V"), MNpv, 1e-20); - ASSERT_MATRIX_APPROX(MN.at("O","O"), MNoo, 1e-20); - ASSERT_MATRIX_APPROX(MN.at("O","V"), MNov, 1e-20); + ASSERT_MATRIX_APPROX(MN.at('P','O'), MNpo, 1e-20); + ASSERT_MATRIX_APPROX(MN.at('P','V'), MNpv, 1e-20); + ASSERT_MATRIX_APPROX(MN.at('O','O'), MNoo, 1e-20); + ASSERT_MATRIX_APPROX(MN.at('O','V'), MNov, 1e-20); ASSERT_TRUE(MN.check()); } @@ -717,10 +717,10 @@ TEST(MatrixComposite, propagate) Qop.setOnes(); Qop *= 2; Qoo.setOnes(); Qoo *= 3; - Q.emplace("P", "P", Qpp); - Q.emplace("P", "O", Qpo); - Q.emplace("O", "P", Qop); - Q.emplace("O", "O", Qoo); + Q.emplace('P', 'P', Qpp); + Q.emplace('P', 'O', Qpo); + Q.emplace('O', 'P', Qop); + Q.emplace('O', 'O', Qoo); WOLF_DEBUG("Q = " , Q); MatrixXd Jvp(vsize,psize), Jvo(vsize, osize), Jwp(wsize,psize), Jwo(wsize,osize); @@ -729,10 +729,10 @@ TEST(MatrixComposite, propagate) Jwp.setOnes(); Jwp *= 3; Jwo.setOnes(); Jwo *= 4; - J.emplace("V", "P", Jvp); - J.emplace("V", "O", Jvo); - J.emplace("W", "P", Jwp); - J.emplace("W", "O", Jwo); + J.emplace('V', 'P', Jvp); + J.emplace('V', 'O', Jvo); + J.emplace('W', 'P', Jwp); + J.emplace('W', 'O', Jwo); WOLF_DEBUG("J = " , J); MatrixComposite JQJt; @@ -763,15 +763,15 @@ TEST(MatrixComposite, sum) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - M.emplace("P", "P", Mpp); - M.emplace("P", "O", Mpo); - M.emplace("O", "P", Mop); - M.emplace("O", "O", Moo); + M.emplace('P', 'P', Mpp); + M.emplace('P', 'O', Mpo); + M.emplace('O', 'P', Mop); + M.emplace('O', 'O', Moo); WOLF_DEBUG("M = " , M); - N.emplace("P", "P", Mpp); - N.emplace("P", "O", Mpo); - N.emplace("O", "P", Mop); - N.emplace("O", "O", Moo); + N.emplace('P', 'P', Mpp); + N.emplace('P', 'O', Mpo); + N.emplace('O', 'P', Mop); + N.emplace('O', 'O', Moo); WOLF_DEBUG("N = " , N); MatrixComposite MpN; @@ -780,10 +780,10 @@ TEST(MatrixComposite, sum) WOLF_DEBUG("MpN = M + N = " , MpN); - ASSERT_MATRIX_APPROX(MpN.at("P","P"), 2 * Mpp, 1e-10); - ASSERT_MATRIX_APPROX(MpN.at("P","O"), 2 * Mpo, 1e-10); - ASSERT_MATRIX_APPROX(MpN.at("O","P"), 2 * Mop, 1e-10); - ASSERT_MATRIX_APPROX(MpN.at("O","O"), 2 * Moo, 1e-10); + ASSERT_MATRIX_APPROX(MpN.at('P','P'), 2 * Mpp, 1e-10); + ASSERT_MATRIX_APPROX(MpN.at('P','O'), 2 * Mpo, 1e-10); + ASSERT_MATRIX_APPROX(MpN.at('O','P'), 2 * Mop, 1e-10); + ASSERT_MATRIX_APPROX(MpN.at('O','O'), 2 * Moo, 1e-10); } @@ -801,15 +801,15 @@ TEST(MatrixComposite, difference) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - M.emplace("P", "P", Mpp); - M.emplace("P", "O", Mpo); - M.emplace("O", "P", Mop); - M.emplace("O", "O", Moo); + M.emplace('P', 'P', Mpp); + M.emplace('P', 'O', Mpo); + M.emplace('O', 'P', Mop); + M.emplace('O', 'O', Moo); WOLF_DEBUG("M = " , M); - N.emplace("P", "P", Mpp); - N.emplace("P", "O", Mpo); - N.emplace("O", "P", Mop); - N.emplace("O", "O", Moo); + N.emplace('P', 'P', Mpp); + N.emplace('P', 'O', Mpo); + N.emplace('O', 'P', Mop); + N.emplace('O', 'O', Moo); WOLF_DEBUG("N = " , N); MatrixComposite MmN; @@ -818,10 +818,10 @@ TEST(MatrixComposite, difference) WOLF_DEBUG("MmN = M - N = " , MmN); - ASSERT_MATRIX_APPROX(MmN.at("P","P"), Mpp * 0, 1e-10); - ASSERT_MATRIX_APPROX(MmN.at("P","O"), Mpo * 0, 1e-10); - ASSERT_MATRIX_APPROX(MmN.at("O","P"), Mop * 0, 1e-10); - ASSERT_MATRIX_APPROX(MmN.at("O","O"), Moo * 0, 1e-10); + ASSERT_MATRIX_APPROX(MmN.at('P','P'), Mpp * 0, 1e-10); + ASSERT_MATRIX_APPROX(MmN.at('P','O'), Mpo * 0, 1e-10); + ASSERT_MATRIX_APPROX(MmN.at('O','P'), Mop * 0, 1e-10); + ASSERT_MATRIX_APPROX(MmN.at('O','O'), Moo * 0, 1e-10); } @@ -839,10 +839,10 @@ TEST(MatrixComposite, unary_minus) Mop.setOnes(); Mop *= 3; Moo.setOnes(); Moo *= 4; - M.emplace("P", "P", Mpp); - M.emplace("P", "O", Mpo); - M.emplace("O", "P", Mop); - M.emplace("O", "O", Moo); + M.emplace('P', 'P', Mpp); + M.emplace('P', 'O', Mpo); + M.emplace('O', 'P', Mop); + M.emplace('O', 'O', Moo); WOLF_DEBUG("M = " , M); MatrixComposite m; @@ -851,10 +851,10 @@ TEST(MatrixComposite, unary_minus) WOLF_DEBUG("m = - M = " , m); - ASSERT_MATRIX_APPROX(m.at("P","P"), - M.at("P","P"), 1e-10); - ASSERT_MATRIX_APPROX(m.at("P","O"), - M.at("P","O"), 1e-10); - ASSERT_MATRIX_APPROX(m.at("O","P"), - M.at("O","P"), 1e-10); - ASSERT_MATRIX_APPROX(m.at("O","O"), - M.at("O","O"), 1e-10); + ASSERT_MATRIX_APPROX(m.at('P','P'), - M.at('P','P'), 1e-10); + ASSERT_MATRIX_APPROX(m.at('P','O'), - M.at('P','O'), 1e-10); + ASSERT_MATRIX_APPROX(m.at('O','P'), - M.at('O','P'), 1e-10); + ASSERT_MATRIX_APPROX(m.at('O','O'), - M.at('O','O'), 1e-10); } int main(int argc, char **argv) diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp index 4e539e24b7554a7a2e5cfe88096cc4c861de4018..a14c436ddc0864f7b55e1ef9db1fd451ecf494ea 100644 --- a/test/gtest_track_matrix.cpp +++ b/test/gtest_track_matrix.cpp @@ -22,9 +22,11 @@ class TrackMatrixTest : public testing::Test FrameBasePtr F0, F1, F2, F3, F4; CaptureBasePtr C0, C1, C2, C3, C4; FeatureBasePtr f0, f1, f2, f3, f4; + ProblemPtr problem; void SetUp() override { + problem = Problem::create("PO", 2); // unlinked captures // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0); @@ -35,11 +37,11 @@ class TrackMatrixTest : public testing::Test // unlinked frames // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer - F0 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 0.0, nullptr); - F1 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 1.0, nullptr); - F2 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 2.0, nullptr); - F3 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 3.0, nullptr); - F4 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 4.0, nullptr); + F0 = std::make_shared<FrameBase>(0.0, nullptr); + F1 = std::make_shared<FrameBase>(1.0, nullptr); + F2 = std::make_shared<FrameBase>(2.0, nullptr); + F3 = std::make_shared<FrameBase>(3.0, nullptr); + F4 = std::make_shared<FrameBase>(4.0, nullptr); // unlinked features // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer @@ -50,8 +52,8 @@ class TrackMatrixTest : public testing::Test f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); // F0 and F4 are keyframes - F0->setKey(); - F4->setKey(); + F0->link(problem); + F4->link(problem); // link captures C0->link(F0); diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index 00f7d83fff99ff39b16fc8a67c2eee9b767cfbe1..bd05620f6fb13bdaa20a97343c12141f44d60c97 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -32,62 +32,34 @@ TEST(TrajectoryBase, ClosestKeyFrame) // 1 2 3 4 time stamps // --+-----+-----+-----+---> time - FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero() ); - FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero() ); - FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() ); - FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, 4, Eigen::Vector3d::Zero() ); + FrameBasePtr F1 = P->emplaceFrame( 1, Eigen::Vector3d::Zero() ); + FrameBasePtr F2 = P->emplaceFrame( 2, Eigen::Vector3d::Zero() ); + // FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() ); + FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(), + // P->getDim(), + std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) ); + FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameStructure(), + // P->getDim(), + std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) ); FrameBasePtr KF; // closest key-frame queried - KF = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return F1 + KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1 ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return F1 + KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1 ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return F2 + KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2 + KF = T->closestFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestKeyFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2 + KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestKeyFrameToTimeStamp(4.2); // after the last frame --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! -} - -TEST(TrajectoryBase, ClosestKeyOrAuxFrame) -{ - - ProblemPtr P = Problem::create("PO", 2); - TrajectoryBasePtr T = P->getTrajectory(); - - // Trajectory status: - // KF1 KF2 F3 frames - // 1 2 3 time stamps - // --+-----+-----+---> time - - FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero() ); - FrameBasePtr F2 = P->emplaceFrame(AUXILIARY, 2, Eigen::Vector3d::Zero() ); - FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero() ); - - FrameBasePtr KF; // closest key-frame queried - - KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8); // before all keyframes --> return f0 - ASSERT_EQ(KF->id(), F1->id()); // same id! - - KF = T->closestKeyOrAuxFrameToTimeStamp(1.1); // between keyframes --> return F1 - ASSERT_EQ(KF->id(), F1->id()); // same id! - - KF = T->closestKeyOrAuxFrameToTimeStamp(1.9); // between keyframes --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! - - KF = T->closestKeyOrAuxFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! - - KF = T->closestKeyOrAuxFrameToTimeStamp(3.2); // after the last frame --> return F2 + KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! } @@ -108,28 +80,29 @@ TEST(TrajectoryBase, Add_Remove_Frame) std::cout << __LINE__ << std::endl; // add F1 - FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero()); // 2 non-fixed + FrameBasePtr F1 = P->emplaceFrame(1, Eigen::Vector3d::Zero()); // 2 non-fixed if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1); + ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); std::cout << __LINE__ << std::endl; // add F2 - FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not + FrameBasePtr F2 = P->emplaceFrame(2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2); + ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4); std::cout << __LINE__ << std::endl; // add F3 - FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero()); + FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(), + // P->getDim(), + std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()})); if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3); + ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4); std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastFrame()->id(), F3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastFrame()->id(), F2->id()); std::cout << __LINE__ << std::endl; N.update(); @@ -139,24 +112,23 @@ TEST(TrajectoryBase, Add_Remove_Frame) // remove frames and keyframes F2->remove(); // KF if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2); + ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastFrame()->id(), F3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id()); + ASSERT_EQ(T->getLastFrame()->id(), F1->id()); std::cout << __LINE__ << std::endl; F3->remove(); // F if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1); + ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1); std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id()); + ASSERT_EQ(T->getLastFrame()->id(), F1->id()); F1->remove(); // KF if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 0); + ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 0); std::cout << __LINE__ << std::endl; N.update(); @@ -165,123 +137,6 @@ TEST(TrajectoryBase, Add_Remove_Frame) std::cout << __LINE__ << std::endl; } -TEST(TrajectoryBase, KeyFramesAreSorted) -{ - using std::make_shared; - - ProblemPtr P = Problem::create("PO", 2); - TrajectoryBasePtr T = P->getTrajectory(); - - // Trajectory status: - // KF1 KF2 F3 frames - // 1 2 3 time stamps - // --+-----+-----+---> time - - // add frames and keyframes in random order --> keyframes must be sorted after that - FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero()); - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - - FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero()); // non-key-frame - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - - FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero()); - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - - F3->setKey(); // set KF3 - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); - - auto F4 = P->emplaceFrame(NON_ESTIMATED, 1.5, Eigen::Vector3d::Zero()); - // Trajectory status: - // KF1 KF2 KF3 F4 frames - // 1 2 3 1.5 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), F4->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); - - F4->setKey(); - // Trajectory status: - // KF1 KF4 KF2 KF3 frames - // 1 1.5 2 3 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); - - F2->setTimeStamp(4); - // Trajectory status: - // KF1 KF4 KF3 KF2 frames - // 1 1.5 3 4 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - - F4->setTimeStamp(0.5); - // Trajectory status: - // KF4 KF2 KF3 KF2 frames - // 0.5 1 3 4 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getFrameList().front()->id(), F4->id()); - - auto F5 = P->emplaceFrame(AUXILIARY, 1.5, Eigen::Vector3d::Zero()); - // Trajectory status: - // KF4 KF2 AuxF5 KF3 KF2 frames - // 0.5 1 1.5 3 4 time stamps - // --+-----+-----+-----+-----+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - - F5->setTimeStamp(5); - // Trajectory status: - // KF4 KF2 KF3 KF2 AuxF5 frames - // 0.5 1 3 4 5 time stamps - // --+-----+-----+-----+-----+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), F5->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - - auto F6 = P->emplaceFrame(NON_ESTIMATED, 6, Eigen::Vector3d::Zero()); - // Trajectory status: - // KF4 KF2 KF3 KF2 AuxF5 F6 frames - // 0.5 1 3 4 5 6 time stamps - // --+-----+-----+-----+-----+-----+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), F6->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - - auto F7 = P->emplaceFrame(NON_ESTIMATED, 5.5, Eigen::Vector3d::Zero()); - // Trajectory status: - // KF4 KF2 KF3 KF2 AuxF5 F7 F6 frames - // 0.5 1 3 4 5 5.5 6 time stamps - // --+-----+-----+-----+-----+-----+-----+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), F7->id()); //Only auxiliary and key-frames are sorted - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id()); - ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - -} - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp index 1d97e2925487f9e363e1292b6ef2c2a82c29fc96..9c684c7bc0f3f30199a8d86e2ca9bf4b8aacb87f 100644 --- a/test/gtest_tree_manager.cpp +++ b/test/gtest_tree_manager.cpp @@ -94,7 +94,7 @@ TEST(TreeManager, keyFrameCallback) ASSERT_EQ(GM->n_KF_, 0); - auto F0 = P->emplaceFrame(KEY, 0, "PO", 3, VectorXd(7) ); + auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) ); P->keyFrameCallback(F0, nullptr, 0); ASSERT_EQ(GM->n_KF_, 1); diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index 5cbc9550cc9a751a57046807f236486f4d5cbb92..e8ffeb407df4df15c00715b21a3e86d554b38da8 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/test/gtest_tree_manager_sliding_window.cpp @@ -24,7 +24,7 @@ TEST(TreeManagerSlidingWindow, make_shared) P->setTreeManager(GM); - ASSERT_EQ(P->getTreeManager(), GM); + EXPECT_EQ(P->getTreeManager(), GM); } TEST(TreeManagerSlidingWindow, createParams) @@ -35,12 +35,12 @@ TEST(TreeManagerSlidingWindow, createParams) auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM); - ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr); + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr); P->setTreeManager(GM); - ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr); - ASSERT_EQ(P->getTreeManager(), GM); + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr); + EXPECT_EQ(P->getTreeManager(), GM); } TEST(TreeManagerSlidingWindow, createParamServer) @@ -52,12 +52,12 @@ TEST(TreeManagerSlidingWindow, createParamServer) auto GM = TreeManagerSlidingWindow::create("tree_manager", server); - ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr); + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr); P->setTreeManager(GM); - ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr); - ASSERT_EQ(P->getTreeManager(), GM); + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr); + EXPECT_EQ(P->getTreeManager(), GM); } TEST(TreeManagerSlidingWindow, autoConf) @@ -67,7 +67,7 @@ TEST(TreeManagerSlidingWindow, autoConf) ProblemPtr P = Problem::autoSetup(server); - ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr); + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr); } TEST(TreeManagerSlidingWindow, slidingWindowFixViral) @@ -79,15 +79,15 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) P->applyPriorOptions(0); // FRAME 1 ---------------------------------------------------------- - auto F1 = P->getTrajectory()->getLastKeyFrame(); - ASSERT_TRUE(F1 != nullptr); + auto F1 = P->getTrajectory()->getLastFrame(); + EXPECT_TRUE(F1 != nullptr); Vector7d state = F1->getStateVector(); Vector7d zero_disp(state); Matrix6d cov = Matrix6d::Identity(); // FRAME 2 ---------------------------------------------------------- - auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state); + auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state); P->keyFrameCallback(F2, nullptr, 0); // absolute factor @@ -100,10 +100,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false); // Check no frame removed - ASSERT_FALSE(F1->isRemoving()); + EXPECT_FALSE(F1->isRemoving()); // FRAME 3 ---------------------------------------------------------- - auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state); + auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state); P->keyFrameCallback(F3, nullptr, 0); // absolute factor @@ -116,10 +116,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false); // Check no frame removed - ASSERT_FALSE(F1->isRemoving()); + EXPECT_FALSE(F1->isRemoving()); // FRAME 4 ---------------------------------------------------------- - auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state); + auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state); P->keyFrameCallback(F4, nullptr, 0); // absolute factor @@ -132,14 +132,14 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false); // Checks - ASSERT_TRUE(F1->isRemoving()); - ASSERT_TRUE(c12->isRemoving()); - ASSERT_TRUE(C12->isRemoving()); //Virally removed - ASSERT_TRUE(f12->isRemoving()); //Virally removed - ASSERT_TRUE(F2->isFixed()); //Fixed + EXPECT_TRUE(F1->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C12->isRemoving()); // Virally removed + EXPECT_TRUE(f12->isRemoving()); // Virally removed + EXPECT_TRUE(F2->isFixed()); //Fixed // FRAME 5 ---------------------------------------------------------- - auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state); + auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state); P->keyFrameCallback(F5, nullptr, 0); // absolute factor @@ -152,18 +152,19 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false); // Checks - ASSERT_TRUE(F1->isRemoving()); - ASSERT_TRUE(c12->isRemoving()); - ASSERT_TRUE(C12->isRemoving()); //Virally removed - ASSERT_TRUE(f12->isRemoving()); //Virally removed - ASSERT_TRUE(F2->isRemoving()); - ASSERT_TRUE(c2->isRemoving()); - ASSERT_TRUE(C2->isRemoving()); //Virally removed - ASSERT_TRUE(f2->isRemoving()); //Virally removed - ASSERT_TRUE(c23->isRemoving()); - ASSERT_TRUE(C23->isRemoving()); //Virally removed - ASSERT_TRUE(f23->isRemoving()); //Virally removed - ASSERT_TRUE(F3->isFixed()); //Fixed + EXPECT_TRUE(F1->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C12->isRemoving()); // Virally removed + EXPECT_TRUE(f12->isRemoving()); // Virally removed + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C2->isRemoving()); // Virally removed + EXPECT_TRUE(f2->isRemoving()); // Virally removed + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_TRUE(F3->isFixed()); //Fixed } TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) @@ -175,15 +176,15 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) P->applyPriorOptions(0); // FRAME 1 (prior) ---------------------------------------------------------- - auto F1 = P->getTrajectory()->getLastKeyFrame(); - ASSERT_TRUE(F1 != nullptr); + auto F1 = P->getTrajectory()->getLastFrame(); + EXPECT_TRUE(F1 != nullptr); Vector7d state = F1->getStateVector(); Vector7d zero_disp(state); Matrix6d cov = Matrix6d::Identity(); // FRAME 2 ---------------------------------------------------------- - auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state); + auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state); P->keyFrameCallback(F2, nullptr, 0); // absolute factor @@ -196,10 +197,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false); // Check no frame removed - ASSERT_FALSE(F1->isRemoving()); + EXPECT_FALSE(F1->isRemoving()); // FRAME 3 ---------------------------------------------------------- - auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state); + auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state); P->keyFrameCallback(F3, nullptr, 0); // absolute factor @@ -212,10 +213,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false); // Check no frame removed - ASSERT_FALSE(F1->isRemoving()); + EXPECT_FALSE(F1->isRemoving()); // FRAME 4 ---------------------------------------------------------- - auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state); + auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state); P->keyFrameCallback(F4, nullptr, 0); // absolute factor @@ -228,14 +229,14 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false); // Checks - ASSERT_TRUE(F1->isRemoving()); - ASSERT_TRUE(c12->isRemoving()); - ASSERT_FALSE(C12->isRemoving()); //Not virally removed - ASSERT_FALSE(f12->isRemoving()); //Not virally removed - ASSERT_FALSE(F2->isFixed()); //Not fixed + EXPECT_TRUE(F1->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C12->isRemoving()); //Not virally removed + EXPECT_FALSE(f12->isRemoving()); //Not virally removed + EXPECT_FALSE(F2->isFixed()); //Not fixed // FRAME 5 ---------------------------------------------------------- - auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state); + auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state); P->keyFrameCallback(F5, nullptr, 0); // absolute factor @@ -248,18 +249,18 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false); // Checks - ASSERT_TRUE(F1->isRemoving()); - ASSERT_TRUE(c12->isRemoving()); - ASSERT_TRUE(C12->isRemoving()); - ASSERT_TRUE(f12->isRemoving()); - ASSERT_TRUE(F2->isRemoving()); - ASSERT_TRUE(c2->isRemoving()); - ASSERT_TRUE(C2->isRemoving()); - ASSERT_TRUE(f2->isRemoving()); - ASSERT_TRUE(c23->isRemoving()); - ASSERT_FALSE(C23->isRemoving()); //Not virally removed - ASSERT_FALSE(f23->isRemoving()); //Not virally removed - ASSERT_FALSE(F3->isFixed()); //Not fixed + EXPECT_TRUE(F1->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C23->isRemoving()); //Not virally removed + EXPECT_FALSE(f23->isRemoving()); //Not virally removed + EXPECT_FALSE(F3->isFixed()); //Not fixed } int main(int argc, char **argv) diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fa1414aafe81283cbc931b20498e3b737b13b533 --- /dev/null +++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp @@ -0,0 +1,1066 @@ +#include "core/utils/utils_gtest.h" + + +#include "core/problem/problem.h" +#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h" +#include "core/yaml/parser_yaml.h" +#include "core/capture/capture_void.h" +#include "core/capture/capture_odom_3d.h" +#include "core/feature/feature_base.h" +#include "core/factor/factor_odom_3d.h" +#include "core/factor/factor_pose_3d.h" +#include "core/solver/factory_solver.h" + +using namespace wolf; +using namespace Eigen; + +std::string wolf_root = _WOLF_ROOT_DIR; + +TEST(TreeManagerSlidingWindowDualRate, make_shared) +{ + ProblemPtr P = Problem::create("PO", 2); + + auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>(); + + auto GM = std::make_shared<TreeManagerSlidingWindowDualRate>(ParamsGM); + + P->setTreeManager(GM); + + EXPECT_EQ(P->getTreeManager(), GM); +} + +TEST(TreeManagerSlidingWindowDualRate, createParams) +{ + ProblemPtr P = Problem::create("PO", 2); + + auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>(); + + auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", ParamsGM); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr); + + P->setTreeManager(GM); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr); + EXPECT_EQ(P->getTreeManager(), GM); +} + +TEST(TreeManagerSlidingWindowDualRate, createParamServer) +{ + ProblemPtr P = Problem::create("PO", 2); + + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", server); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr); + + P->setTreeManager(GM); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr); + EXPECT_EQ(P->getTreeManager(), GM); +} + +TEST(TreeManagerSlidingWindowDualRate, autoConf) +{ + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + ProblemPtr P = Problem::autoSetup(server); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr); +} + +TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral) +{ + /* sliding window dual rate: + * n_frames: 5 + * n_frames_recent: 3 + * rate_old_frames: 2 + */ + + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + ProblemPtr P = Problem::autoSetup(server); + P->applyPriorOptions(0); + + /* FRAME 1 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )( )(F1) + */ + auto F1 = P->getTrajectory()->getLastFrame(); + ASSERT_TRUE(F1 != nullptr); + ASSERT_FALSE(F1->getCaptureList().empty()); + auto C1 = F1->getCaptureList().front(); + ASSERT_FALSE(C1->getFeatureList().empty()); + auto f1 = C1->getFeatureList().front(); + ASSERT_FALSE(f1->getFactorList().empty()); + auto c1 = f1->getFactorList().front(); + + Vector7d state = F1->getStateVector(); + Vector7d zero_disp(state); + Matrix6d cov = Matrix6d::Identity(); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + + /* FRAME 2 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )(F1)(F2) + */ + auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state); + P->keyFrameCallback(F2, nullptr, 0); + + // absolute factor + auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov); + auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false); + // displacement + auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov); + auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + + /* FRAME 3 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) (F1)(F2)(F3) + */ + auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state); + P->keyFrameCallback(F3, nullptr, 0); + + // absolute factor + auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov); + auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false); + // displacement + auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov); + auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + + /* FRAME 4 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1)(F2)(F3)(F4) + */ + auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state); + P->keyFrameCallback(F4, nullptr, 0); + + // absolute factor + auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov); + auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false); + // displacement + auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov); + auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + + /* FRAME 5 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1) (F3)(F4)(F5) + */ + auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state); + P->keyFrameCallback(F5, nullptr, 0); + + // absolute factor + auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov); + auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false); + // displacement + auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov); + auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + + /* FRAME 6 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3)(F4)(F5)(F6) + */ + auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state); + P->keyFrameCallback(F6, nullptr, 0); + + // absolute factor + auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov); + auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false); + // displacement + auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov); + auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + + /* FRAME 7 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3) (F5)(F6)(F7) + */ + auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state); + P->keyFrameCallback(F7, nullptr, 0); + + // absolute factor + auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov); + auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false); + // displacement + auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov); + auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C45->isRemoving()); // Virally removed + EXPECT_TRUE(f45->isRemoving()); // Virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + + /* FRAME 8 ---------------------------------------------------------- + * + * Sliding window: + * (F3) (F5)(F6)(F7)(F8) + */ + auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state); + P->keyFrameCallback(F8, nullptr, 0); + + // absolute factor + auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov); + auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false); + // displacement + auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov); + auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false); + + // Checks + EXPECT_TRUE(F1->isRemoving()); // First frame removed + EXPECT_TRUE(c1->isRemoving()); + EXPECT_TRUE(C1->isRemoving()); + EXPECT_TRUE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C45->isRemoving()); // Virally removed + EXPECT_TRUE(f45->isRemoving()); // Virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F8->isRemoving()); + EXPECT_FALSE(c8->isRemoving()); + EXPECT_FALSE(C8->isRemoving()); + EXPECT_FALSE(f8->isRemoving()); + EXPECT_FALSE(c78->isRemoving()); + EXPECT_FALSE(C78->isRemoving()); + EXPECT_FALSE(f78->isRemoving()); + + EXPECT_TRUE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + EXPECT_FALSE(F8->isFixed()); +} + +TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral) +{ + /* sliding window dual rate: + * n_frames: 5 + * n_frames_recent: 3 + * rate_old_frames: 2 + */ + + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + ProblemPtr P = Problem::autoSetup(server); + P->applyPriorOptions(0); + + /* FRAME 1 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )( )(F1) + */ + auto F1 = P->getTrajectory()->getLastFrame(); + ASSERT_TRUE(F1 != nullptr); + ASSERT_FALSE(F1->getCaptureList().empty()); + auto C1 = F1->getCaptureList().front(); + ASSERT_FALSE(C1->getFeatureList().empty()); + auto f1 = C1->getFeatureList().front(); + ASSERT_FALSE(f1->getFactorList().empty()); + auto c1 = f1->getFactorList().front(); + + Vector7d state = F1->getStateVector(); + Vector7d zero_disp(state); + Matrix6d cov = Matrix6d::Identity(); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + + /* FRAME 2 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )(F1)(F2) + */ + auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state); + P->keyFrameCallback(F2, nullptr, 0); + + // absolute factor + auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov); + auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false); + // displacement + auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov); + auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + + /* FRAME 3 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) (F1)(F2)(F3) + */ + auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state); + P->keyFrameCallback(F3, nullptr, 0); + + // absolute factor + auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov); + auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false); + // displacement + auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov); + auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + + /* FRAME 4 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1)(F2)(F3)(F4) + */ + auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state); + P->keyFrameCallback(F4, nullptr, 0); + + // absolute factor + auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov); + auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false); + // displacement + auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov); + auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + + /* FRAME 5 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1) (F3)(F4)(F5) + */ + auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state); + P->keyFrameCallback(F5, nullptr, 0); + + // absolute factor + auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov); + auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false); + // displacement + auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov); + auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C23->isRemoving()); // Not virally removed + EXPECT_FALSE(f23->isRemoving()); // Not virally removed + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + + /* FRAME 6 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3)(F4)(F5)(F6) + */ + auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state); + P->keyFrameCallback(F6, nullptr, 0); + + // absolute factor + auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov); + auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false); + // displacement + auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov); + auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C23->isRemoving()); // Not virally removed + EXPECT_FALSE(f23->isRemoving()); // Not virally removed + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + + /* FRAME 7 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3) (F5)(F6)(F7) + */ + auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state); + P->keyFrameCallback(F7, nullptr, 0); + + // absolute factor + auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov); + auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false); + // displacement + auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov); + auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C23->isRemoving()); // Not virally removed + EXPECT_FALSE(f23->isRemoving()); // Not virally removed + + EXPECT_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C45->isRemoving()); // Not virally removed + EXPECT_FALSE(f45->isRemoving()); // Not virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + + /* FRAME 8 ---------------------------------------------------------- + * + * Sliding window: + * (F3) (F5)(F6)(F7)(F8) + */ + auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state); + P->keyFrameCallback(F8, nullptr, 0); + + // absolute factor + auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov); + auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false); + // displacement + auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov); + auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false); + + // Checks + EXPECT_TRUE(F1->isRemoving()); // First frame removed + EXPECT_TRUE(c1->isRemoving()); + EXPECT_TRUE(C1->isRemoving()); + EXPECT_TRUE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C23->isRemoving()); // Not virally removed + EXPECT_FALSE(f23->isRemoving()); // Not virally removed + + EXPECT_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C45->isRemoving()); // Not virally removed + EXPECT_FALSE(f45->isRemoving()); // Not virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F8->isRemoving()); + EXPECT_FALSE(c8->isRemoving()); + EXPECT_FALSE(C8->isRemoving()); + EXPECT_FALSE(f8->isRemoving()); + EXPECT_FALSE(c78->isRemoving()); + EXPECT_FALSE(C78->isRemoving()); + EXPECT_FALSE(f78->isRemoving()); + + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + EXPECT_FALSE(F8->isFixed()); +} + +TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor) +{ + // SLIDING WINDOW DUAL RATE + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + ProblemPtr problem = Problem::autoSetup(server); + SolverManagerPtr solver = FactorySolver::create("SolverCeres", problem, server); + + // BASELINE + ParserYaml parser_bl = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml", wolf_root); + ParamsServer server_bl = ParamsServer(parser_bl.getParams()); + ProblemPtr problem_bl = Problem::autoSetup(server_bl); + SolverManagerPtr solver_bl = FactorySolver::create("SolverCeres", problem_bl, server); + + // aux variables + Vector7d data; + Matrix6d data_cov = Matrix6d::Identity(); + TimeStamp t(0.0); + double dt = 1; + CaptureMotionPtr capture = std::make_shared<CaptureOdom3d>(t, + problem->getSensor("odom"), + data, + data_cov); + CaptureMotionPtr capture_bl = std::make_shared<CaptureOdom3d>(t, + problem_bl->getSensor("odom"), + data, + data_cov); + + // START MOTION CAPTURES + for (int i = 0; i<20; i++) + { + t += dt; + WOLF_INFO("-------------------------- t: ", t); + + // random movement + data.setRandom(); data.tail<4>().normalize(); + + // sliding window process + capture->setTimeStamp(t); + capture->setData(data); + capture->process(); + + // baseline process + capture_bl->setTimeStamp(t); + capture_bl->setData(data); + capture_bl->process(); + + WOLF_INFO("state: ", problem->getState().vector("PO").transpose()); + + ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(), + problem_bl->getState().vector("PO").transpose(), + 1e-12); + + // Solve + solver->solve(); + solver_bl->solve(); + + ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(), + problem_bl->getState().vector("PO").transpose(), + 1e-12); + + ASSERT_TRUE(problem->check()); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml index 96ad94f72af7e8de1b9200a8e5ea44e3bc952a85..22242febd640b616bebe4ab0410867b0e373a5be 100644 --- a/test/yaml/params_tree_manager_sliding_window1.yaml +++ b/test/yaml/params_tree_manager_sliding_window1.yaml @@ -4,8 +4,6 @@ config: dimension: 3 prior: mode: "factor" - # state: [0,0,0,0,0,0,1] - # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1] $state: P: [0,0,0] O: [0,0,0,1] @@ -15,8 +13,8 @@ config: time_tolerance: 0.1 tree_manager: type: "TreeManagerSlidingWindow" - n_key_frames: 3 - fix_first_key_frame: true + n_frames: 3 + fix_first_frame: true viral_remove_empty_parent: true sensors: - diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml index 9ae33a2af5ab0695bd44948d33d2f9aa104cbc74..add5eff760050fecbde63c827bb75e77c531f86e 100644 --- a/test/yaml/params_tree_manager_sliding_window2.yaml +++ b/test/yaml/params_tree_manager_sliding_window2.yaml @@ -4,8 +4,6 @@ config: dimension: 3 prior: mode: "factor" - # state: [0,0,0,0,0,0,1] - # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1] $state: P: [0,0,0] O: [0,0,0,1] @@ -15,8 +13,8 @@ config: time_tolerance: 0.1 tree_manager: type: "TreeManagerSlidingWindow" - n_key_frames: 3 - fix_first_key_frame: false + n_frames: 3 + fix_first_frame: false viral_remove_empty_parent: false sensors: - diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..32ef665e0a6d92b94e9342d22c5e4ca9952613f1 --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml @@ -0,0 +1,20 @@ +config: + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "TreeManagerSlidingWindowDualRate" + n_frames: 5 + n_frames_recent: 3 + rate_old_frames: 2 + fix_first_frame: true + viral_remove_empty_parent: true diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6b4795fb3ad70c77b02c9cffc74b964abfe64141 --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml @@ -0,0 +1,20 @@ +config: + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "TreeManagerSlidingWindowDualRate" + n_frames: 5 + n_frames_recent: 3 + rate_old_frames: 2 + fix_first_frame: false + viral_remove_empty_parent: false diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b59022518aaf931890368185fb9b489611cda257 --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml @@ -0,0 +1,55 @@ +config: + solver: + period: 1 + verbose: 2 + update_immediately: false + max_num_iterations: 10 + n_threads: 2 + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "TreeManagerSlidingWindowDualRate" + n_frames: 5 + n_frames_recent: 3 + rate_old_frames: 2 + fix_first_frame: true + viral_remove_empty_parent: true + sensors: + - + type: "SensorOdom3d" + name: "odom" + plugin: "core" + k_disp_to_disp: 0.1 + k_disp_to_rot: 0.1 + k_rot_to_rot: 0.1 + min_disp_var: 0.1 + min_rot_var: 0.1 + extrinsic: + pose: [1,2,3,0,0,0,1] + processors: + - + type: "ProcessorOdom3d" + name: "my_proc_odom3d" + sensor_name: "odom" + plugin: "core" + apply_loss_function: false + time_tolerance: 0.01 # seconds + keyframe_vote: + voting_active: true + voting_aux_active: false + max_time_span: 0.2 # seconds + max_buff_length: 10 # motion deltas + dist_traveled: 10 # meters + angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg) + + unmeasured_perturbation_std: 0.00111 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml new file mode 100644 index 0000000000000000000000000000000000000000..eba780939b0e4521ba7a5d571ed47d63749adc6e --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml @@ -0,0 +1,49 @@ +config: + solver: + period: 1 + verbose: 2 + update_immediately: false + max_num_iterations: 10 + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "None" + sensors: + - + type: "SensorOdom3d" + name: "odom" + plugin: "core" + k_disp_to_disp: 0.1 + k_disp_to_rot: 0.1 + k_rot_to_rot: 0.1 + min_disp_var: 0.1 + min_rot_var: 0.1 + extrinsic: + pose: [1,2,3,0,0,0,1] + processors: + - + type: "ProcessorOdom3d" + name: "my_proc_odom3d" + sensor_name: "odom" + plugin: "core" + apply_loss_function: false + time_tolerance: 0.01 # seconds + keyframe_vote: + voting_active: true + voting_aux_active: false + max_time_span: 0.2 # seconds + max_buff_length: 10 # motion deltas + dist_traveled: 10 # meters + angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg) + + unmeasured_perturbation_std: 0.00111 diff --git a/wolf_scripts/wolf_uninstall.sh b/wolf_scripts/wolf_uninstall.sh index b381616ecf9c37423d5cd11e18bb3f5502cec641..ff022e0b25ebc31bc0d7d0e50124569896c8f6ba 100755 --- a/wolf_scripts/wolf_uninstall.sh +++ b/wolf_scripts/wolf_uninstall.sh @@ -1,20 +1,42 @@ #!/bin/bash -echo “=†-echo “====================== UNINSTALLING WOLF ======================†-echo “=†+if [ -z "$1" ] +then + echo “=†+ echo “====================== UNINSTALLING WOLF ======================†+ echo “=†-echo "cd /usr/local/include/iri-algorithms/" -cd /usr/local/include/iri-algorithms/ -echo "sudo rm -rf wolf" -sudo rm -rf wolf + echo "cd /usr/local/include/iri-algorithms/" + cd /usr/local/include/iri-algorithms/ + echo "sudo rm -rf wolf" + sudo rm -rf wolf -echo "cd /usr/local/lib/iri-algorithms/" -echo "sudo rm libwolf*.*" -cd /usr/local/lib/iri-algorithms/ -sudo rm libwolf*.* + echo "cd /usr/local/lib/iri-algorithms/" + echo "sudo rm libwolf*.*" + cd /usr/local/lib/iri-algorithms/ + sudo rm libwolf*.* -echo "cd /usr/local/lib/cmake/" -echo "sudo rm -rf wolf*" -cd /usr/local/lib/cmake/ -sudo rm -rf wolf* \ No newline at end of file + echo "cd /usr/local/lib/cmake/" + echo "sudo rm -rf wolf*" + cd /usr/local/lib/cmake/ + sudo rm -rf wolf* +else + echo “=†+ echo “====================== UNINSTALLING PLUGIN $1 ======================†+ echo “=†+ + echo "cd /usr/local/include/iri-algorithms/wolf" + cd /usr/local/include/iri-algorithms/wolf/ + echo "sudo rm -rf plugin_$1" + sudo rm -rf plugin_$1 + + echo "cd /usr/local/lib/iri-algorithms/" + echo "sudo rm libwolf$1.*" + cd /usr/local/lib/iri-algorithms/ + sudo rm libwolf$1.* + + echo "cd /usr/local/lib/cmake/" + echo "sudo rm -rf wolf$1" + cd /usr/local/lib/cmake/ + sudo rm -rf wolf$1 +fi