diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000000000000000000000000000000000000..bc951c7539c78c4fa00efde0030dcf12b126686a
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,115 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+IndentAccessModifiers: false
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: true
+AlignEscapedNewlines: Left
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: true
+BinPackParameters: false
+BreakBeforeBraces: Custom
+BraceWrapping:
+ AfterClass: true
+ AfterControlStatement: Always
+ AfterEnum: true
+ AfterFunction: true
+ AfterNamespace: true
+ AfterObjCDeclaration: true
+ AfterStruct: true
+ AfterUnion: true
+ AfterExternBlock: false
+ BeforeCatch: true
+ BeforeElse: true
+ IndentBraces: false
+ SplitEmptyFunction: true
+ SplitEmptyRecord: true
+ SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit: 119
+# CommentPragmas: "^ IWYU pragma: ^\\.+"
+CommentPragmas: '^\\.+'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+ - foreach
+ - Q_FOREACH
+ - BOOST_FOREACH
+IncludeBlocks: Preserve
+# IncludeCategories:
+# - Regex: '^<pinocchio/fwd\.hpp>'
+# Priority: 1
+# - Regex: '^<ext/.*\.h>'
+# Priority: 3
+# - Regex: '^<.*\.h>'
+# Priority: 2
+# - Regex: "^<.*"
+# Priority: 3
+# - Regex: ".*"
+# Priority: 4
+IncludeIsMainRegex: "([-_](test|unittest))?$"
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+TabWidth: 4
+UseTab: Never
diff --git a/.gitignore b/.gitignore
index 77906100d4ae252720e456622103b13d8c6371e5..2a19e53e62aaa9c251b2f92657392e589597f1d6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,7 +4,10 @@
README.txt
bin/
build/
+build_debug/
build_release/
+build-debug/
+build-release/
lib/
.idea/
./Wolf.user
@@ -30,7 +33,6 @@ src/CMakeFiles/cmake.check_cache
src/examples/map_apriltag_save.yaml
\.vscode/
-build_release/
.clangd
wolfcore.found
/wolf.found
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 249b84e5c1902743a60b5ad0665318b4b622bc1a..8337cb1148e9f8669aecc9a6ab7558c5a08446b9 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -6,6 +6,26 @@ stages:
- deploy_ros
############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+ # Print variables
+ - echo $CI_COMMIT_BRANCH
+ - echo $WOLF_IMU_BRANCH
+ - echo $WOLF_GNSS_BRANCH
+ - echo $WOLF_LASER_BRANCH
+ - echo $WOLF_VISION_BRANCH
+ - echo $WOLF_APRILTAG_BRANCH
+ - echo $WOLF_BODYDYNAMICS_BRANCH
+ - echo $GNSSUTILS_BRANCH
+ - echo $LASERSCANUTILS_BRANCH
+ - echo $DEPLOY_CI_ROS
+ - echo $WOLF_ROS_NODE_BRANCH
+ - echo $WOLF_ROS_IMU_BRANCH
+ - echo $WOLF_ROS_GNSS_BRANCH
+ - echo $WOLF_ROS_LASER_BRANCH
+ - echo $WOLF_ROS_VISION_BRANCH
+ - echo $WOLF_ROS_APRILTAG_BRANCH
+ - echo $WOLF_ROS_BODYDYNAMICS_BRANCH
+
.preliminaries_template: &preliminaries_definition
## Install ssh-agent if not already installed, it is required by Docker.
## (change apt-get to yum if you use an RPM-based image)
@@ -80,6 +100,7 @@ license_header:
stage: license
image: labrobotica/wolf_deps:20.04
before_script:
+ - *print_variables_definition
- *preliminaries_definition
script:
- *license_header_definition
@@ -89,6 +110,7 @@ build_and_test:bionic:
stage: build_and_test
image: labrobotica/wolf_deps:18.04
script:
+ - *print_variables_definition
- *build_and_test_definition
############ UBUNTU 20.04 TESTS ############
@@ -96,11 +118,14 @@ build_and_test:focal:
stage: build_and_test
image: labrobotica/wolf_deps:20.04
script:
+ - *print_variables_definition
- *build_and_test_definition
-############ DEPLOY PLUGINS ############
+############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############
deploy_imu:
stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH != "main"
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
DEPLOY_CI_ROS: "false"
@@ -111,8 +136,11 @@ deploy_imu:
deploy_gnss:
stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH != "main"
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
@@ -121,6 +149,8 @@ deploy_gnss:
deploy_vision:
stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH != "main"
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
DEPLOY_CI_ROS: "false"
@@ -131,8 +161,11 @@ deploy_vision:
deploy_laser:
stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH != "main"
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
@@ -141,8 +174,11 @@ deploy_laser:
deploy_apriltag:
stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH != "main"
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
@@ -151,26 +187,105 @@ deploy_apriltag:
deploy_bodydynamics:
stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH != "main"
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
branch: $WOLF_BODYDYNAMICS_BRANCH
strategy: depend
+############ DEPLOY PLUGINS FOR main ############
+deploy_imu_main:
+ stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH == "main"
+ variables:
+ WOLF_CORE_BRANCH: main
+ DEPLOY_CI_ROS: "false"
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
+ branch: main
+ strategy: depend
+
+deploy_gnss_main:
+ stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH == "main"
+ variables:
+ WOLF_CORE_BRANCH: main
+ GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
+ DEPLOY_CI_ROS: "false"
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
+ branch: main
+ strategy: depend
+
+deploy_vision_main:
+ stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH == "main"
+ variables:
+ WOLF_CORE_BRANCH: main
+ DEPLOY_CI_ROS: "false"
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
+ branch: main
+ strategy: depend
+
+deploy_laser_main:
+ stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH == "main"
+ variables:
+ WOLF_CORE_BRANCH: main
+ LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
+ DEPLOY_CI_ROS: "false"
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
+ branch: main
+ strategy: depend
+
+deploy_apriltag_main:
+ stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH == "main"
+ variables:
+ WOLF_CORE_BRANCH: main
+ WOLF_VISION_BRANCH: main
+ DEPLOY_CI_ROS: "false"
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
+ branch: main
+ strategy: depend
+
+deploy_bodydynamics_main:
+ stage: deploy_plugins
+ rules:
+ - if: $CI_COMMIT_BRANCH == "main"
+ variables:
+ WOLF_CORE_BRANCH: main
+ WOLF_IMU_BRANCH: main
+ DEPLOY_CI_ROS: "false"
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
+ branch: main
+ strategy: depend
+
############ WAIT FOR PLUGINS ############
final_all_plugins:
stage: final_plugins
script:
- echo "ALL PLUGINS PIPELINES SUCCEED!!!"
-############ DEPLOY WOLF_ROS_NODE ############
+############ DEPLOY WOLF_ROS_NODE EXCEPT FOR main ############
deploy_wolf_ros_node:
stage: deploy_ros
- only:
- variables:
- - $DEPLOY_CI_ROS == "true"
+ rules:
+ - if: $CI_COMMIT_BRANCH != "main" && $DEPLOY_CI_ROS == "true"
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
@@ -185,14 +300,40 @@ deploy_wolf_ros_node:
WOLF_ROS_VISION_BRANCH: $WOLF_ROS_VISION_BRANCH
WOLF_ROS_APRILTAG_BRANCH: $WOLF_ROS_APRILTAG_BRANCH
WOLF_ROS_BODYDYNAMICS_BRANCH: $WOLF_ROS_BODYDYNAMICS_BRANCH
+ GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
+ LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
+ branch: $WOLF_ROS_NODE_BRANCH
+
+############ DEPLOY WOLF_ROS_NODE FOR main ############
+deploy_wolf_ros_node_main:
+ stage: deploy_ros
+ rules:
+ - if: $CI_COMMIT_BRANCH == "main" && $DEPLOY_CI_ROS == "true"
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_IMU_BRANCH: main
+ WOLF_GNSS_BRANCH: main
+ WOLF_LASER_BRANCH: main
+ WOLF_VISION_BRANCH: main
+ WOLF_APRILTAG_BRANCH: main
+ WOLF_BODYDYNAMICS_BRANCH: main
+ WOLF_ROS_IMU_BRANCH: main
+ WOLF_ROS_GNSS_BRANCH: main
+ WOLF_ROS_LASER_BRANCH: main
+ WOLF_ROS_VISION_BRANCH: main
+ WOLF_ROS_APRILTAG_BRANCH: main
+ WOLF_ROS_BODYDYNAMICS_BRANCH: main
+ GNSSUTILS_BRANCH: main
+ LASERSCANUTILS_BRANCH: main
trigger:
project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
- branch: $WOLF_ROS_CORE_BRANCH
+ branch: main
no_deploy_wolf_ros_node:
stage: deploy_ros
script:
- - echo "NOT deploying CI for wolf_ros_node, since DEPLOY_CI_ROS is $DEPLOY_CI_ROS"
- except:
- variables:
- - $DEPLOY_CI_ROS == "true"
+ - echo "NOT deploying CI for wolf_ros_node, since DEPLOY_CI_ROS is $DEPLOY_CI_ROS (not true)"
+ rules:
+ - if: $DEPLOY_CI_ROS != "true"
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c94b33bfb04c9751c727f62bb16557cd1ef8f508..e69a3926a1a15adc529a45d8c37c7aa3a5437ff5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,12 +2,7 @@
MESSAGE(STATUS "Starting WOLF CMakeLists ...")
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-if(COMMAND cmake_policy)
- cmake_policy(SET CMP0005 NEW)
- cmake_policy(SET CMP0003 NEW)
-endif(COMMAND cmake_policy)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
@@ -17,10 +12,11 @@ PROJECT(core)
set(PLUGIN_NAME "wolf${PROJECT_NAME}")
# Paths
+SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE)
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)
+SET(LIB_INSTALL_DIR lib)
+SET(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
# Build type
IF (NOT CMAKE_BUILD_TYPE)
@@ -80,21 +76,18 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-#CMAKE modules
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS "Cmake modules at: " ${CMAKE_MODULE_PATH})
-
# ============ DEPENDENCIES ============
-FIND_PACKAGE(Threads REQUIRED)
-FIND_PACKAGE(Ceres REQUIRED)
-FIND_PACKAGE(Eigen3 3.3 REQUIRED)
+FIND_PACKAGE(Threads REQUIRED MODULE)
+FIND_PACKAGE(Ceres REQUIRED CONFIG)
+FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
endif()
-FIND_PACKAGE(YamlCpp REQUIRED)
+FIND_PACKAGE(yaml-cpp REQUIRED CONFIG)
# ============ config.h ============
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+set(_WOLF_LIB_DIR ${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR})
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal)
@@ -124,9 +117,7 @@ ENDIF (SPDLOG_INCLUDE_DIR)
# ============ INCLUDES ============
INCLUDE_DIRECTORIES("include") # In this same project
-INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
+
# ============ HEADERS ============
SET(HDRS_CAPTURE
include/core/capture/capture_base.h
@@ -382,10 +373,6 @@ ELSE(Ceres_FOUND)
SET(SRCS_CERES_WRAPPER)
ENDIF(Ceres_FOUND)
-IF (Suitesparse_FOUND)
- #ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-
# create the shared library
ADD_LIBRARY(${PLUGIN_NAME}
SHARED
@@ -423,21 +410,22 @@ endif()
#Link the created libraries
#=============================================================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ${CMAKE_THREAD_LIBS_INIT} dl)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC yaml-cpp)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ceres)
ENDIF(Ceres_FOUND)
#Build tests
-#===============EXAMPLE=========================
+#========================================
IF(BUILD_TESTS)
MESSAGE(STATUS "Will build tests.")
add_subdirectory(test)
ENDIF(BUILD_TESTS)
#Build demos
-#===============EXAMPLE=========================
+#========================================
IF(BUILD_DEMOS)
MESSAGE(STATUS "Will build demos.")
ADD_SUBDIRECTORY(demos)
@@ -447,73 +435,85 @@ ENDIF(BUILD_DEMOS)
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib
- ARCHIVE DESTINATION lib)
+ LIBRARY DESTINATION ${LIB_INSTALL_DIR}
+ ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
+)
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
+
+
+#Â Configure the package installation
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+ ${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
+ ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+ INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+ PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
+)
+
+install(
+ FILES
+ ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+ DESTINATION
+ ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+)
+
+# Specifies include directories to use when compiling the plugin target
+# This way, include_directories does not need to be called in plugins depending on this one
+target_include_directories(${PLUGIN_NAME} INTERFACE
+ $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
+)
-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)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_CERES_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/ceres_wrapper)
INSTALL(FILES ${HDRS_COMMON}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/common)
INSTALL(FILES ${HDRS_FACTOR}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/factor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
INSTALL(FILES ${HDRS_FRAME}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/frame)
INSTALL(FILES ${HDRS_HARDWARE}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/hardware)
INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/landmark)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/landmark)
INSTALL(FILES ${HDRS_MAP}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/map)
INSTALL(FILES ${HDRS_MATH}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/math)
INSTALL(FILES ${HDRS_PROBLEM}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/problem)
INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver)
#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
+# DESTINATION ${INCLUDE_INSTALL_DIR}/core/solver_suitesparse)
INSTALL(FILES ${HDRS_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
INSTALL(FILES ${HDRS_TRAJECTORY}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/trajectory)
INSTALL(FILES ${HDRS_TREE_MANAGER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager)
INSTALL(FILES ${HDRS_UTILS}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils)
INSTALL(FILES ${HDRS_YAML}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/yaml)
-FILE(WRITE ${PLUGIN_NAME}.found "")
-INSTALL(FILES ${PLUGIN_NAME}.found
- DESTINATION include/iri-algorithms/wolf/plugin_core)
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
-
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake"
- "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" @ONLY)
-
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
- "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
-INSTALL(FILES "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
-INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
export(PACKAGE ${PLUGIN_NAME})
-FIND_PACKAGE(Doxygen)
+FIND_PACKAGE(Doxygen MODULE)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
IF (IRI_DOC_DIR)
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
deleted file mode 100644
index 9e969786089ca8ea3801be8b084c51a5782f09b5..0000000000000000000000000000000000000000
--- a/cmake_modules/FindEigen3.cmake
+++ /dev/null
@@ -1,97 +0,0 @@
-# - Try to find Eigen3 lib
-#
-# 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.
-#
-# Once done this will define
-#
-# EIGEN3_FOUND - system has eigen lib with correct version
-# EIGEN3_INCLUDE_DIR - the eigen include directory
-# EIGEN3_VERSION - eigen version
-#
-# 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)
-
diff --git a/cmake_modules/FindGlog.cmake b/cmake_modules/FindGlog.cmake
deleted file mode 100644
index 979dceda4b8ec5e1a5457f4e32dbdc9a27834a1e..0000000000000000000000000000000000000000
--- a/cmake_modules/FindGlog.cmake
+++ /dev/null
@@ -1,346 +0,0 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * 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.
-#
-# 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.
-#
-# Author: alexs.mac@gmail.com (Alex Stewart)
-#
-
-# FindGlog.cmake - Find Google glog logging library.
-#
-# This module defines the following variables:
-#
-# GLOG_FOUND: TRUE iff glog is found.
-# GLOG_INCLUDE_DIRS: Include directories for glog.
-# GLOG_LIBRARIES: Libraries required to link glog.
-# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found
-# was built & installed / exported
-# as a CMake package.
-#
-# The following variables control the behaviour of this module:
-#
-# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then
-# then prefer using an exported CMake configuration
-# generated by glog > 0.3.4 over searching for the
-# glog components manually. Otherwise (FALSE)
-# ignore any exported glog 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 either
-# GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS
-# otherwise FALSE.
-# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to
-# search for glog includes, e.g: /timbuktu/include.
-# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to
-# search for glog libraries, e.g: /timbuktu/lib.
-#
-# 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.
-#
-# GLOG_INCLUDE_DIR: Include directory for glog, not including the
-# include directory of any dependencies.
-# GLOG_LIBRARY: glog library, not including the libraries of any
-# dependencies.
-
-# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when
-# FindGlog was invoked.
-macro(GLOG_RESET_FIND_LIBRARY_PREFIX)
- if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES)
- set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}")
- endif()
-endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)
-
-# Called if we failed to find glog 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(GLOG_REPORT_NOT_FOUND REASON_MSG)
- unset(GLOG_FOUND)
- unset(GLOG_INCLUDE_DIRS)
- unset(GLOG_LIBRARIES)
- # Make results of search visible in the CMake GUI if glog has not
- # been found so that user does not have to toggle to advanced view.
- mark_as_advanced(CLEAR GLOG_INCLUDE_DIR
- GLOG_LIBRARY)
-
- glog_reset_find_library_prefix()
-
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
- # use the camelcase library name, not uppercase.
- if (Glog_FIND_QUIETLY)
- message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN})
- elseif (Glog_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find glog - " ${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 glog - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(GLOG_REPORT_NOT_FOUND)
-
-# Protect against any alternative find_package scripts for this library having
-# been called previously (in a client project) which set GLOG_FOUND, but not
-# the other variables we require / set here which could cause the search logic
-# here to fail.
-unset(GLOG_FOUND)
-
-# -----------------------------------------------------------------
-# By default, if the user has expressed no preference for using an exported
-# glog CMake configuration over performing a search for the installed
-# components, and has not specified any hints for the search locations, then
-# prefer a glog exported configuration if available.
-if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION
- AND NOT GLOG_INCLUDE_DIR_HINTS
- AND NOT GLOG_LIBRARY_DIR_HINTS)
- message(STATUS "No preference for use of exported glog CMake configuration "
- "set, and no hints for include/library directories provided. "
- "Defaulting to preferring an installed/exported glog CMake configuration "
- "if available.")
- set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE)
-endif()
-
-if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
- # Try to find an exported CMake configuration for glog, as generated by
- # glog versions > 0.3.4
- #
- # 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.
- #
- # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its
- # project name when built with CMake, but exports itself as just 'glog'.
- # On Linux/OS X this does not break detection as the project name is
- # not used as part of the install path for the CMake package files,
- # e.g. /usr/local/lib/cmake/glog, where the <glog> suffix is hardcoded
- # in glog's CMakeLists. However, on Windows the project name *is*
- # part of the install prefix: C:/Program Files/google-glog/[include,lib].
- # However, by default CMake checks:
- # C:/Program Files/<FIND_PACKAGE_ARGUMENT_NAME='glog'> which does not
- # exist and thus detection fails. Thus we use the NAMES to force the
- # search to use both google-glog & glog.
- #
- # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package
- find_package(glog QUIET
- NAMES google-glog glog
- NO_MODULE
- NO_CMAKE_PACKAGE_REGISTRY
- NO_CMAKE_BUILDS_PATH)
- if (glog_FOUND)
- message(STATUS "Found installed version of glog: ${glog_DIR}")
- else()
- # Failed to find an installed version of glog, repeat search allowing
- # exported build directories.
- message(STATUS "Failed to find installed glog CMake configuration, "
- "searching for glog build directories exported with CMake.")
- # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and
- # do not want to treat projects built with the CMake GUI preferentially.
- find_package(glog QUIET
- NAMES google-glog glog
- NO_MODULE
- NO_CMAKE_BUILDS_PATH)
- if (glog_FOUND)
- message(STATUS "Found exported glog build directory: ${glog_DIR}")
- endif(glog_FOUND)
- endif(glog_FOUND)
-
- set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND})
-
- if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
- message(STATUS "Detected glog version: ${glog_VERSION}")
- set(GLOG_FOUND ${glog_FOUND})
- # glog wraps the include directories into the exported glog::glog target.
- set(GLOG_INCLUDE_DIR "")
- set(GLOG_LIBRARY glog::glog)
- else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
- message(STATUS "Failed to find an installed/exported CMake configuration "
- "for glog, will perform search for installed glog components.")
- endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
-endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
-
-if (NOT GLOG_FOUND)
- # Either failed to find an exported glog CMake configuration, or user
- # told us not to use one. Perform a manual search for all glog components.
-
- # Handle possible presence of lib prefix for libraries on MSVC, see
- # also GLOG_RESET_FIND_LIBRARY_PREFIX().
- if (MSVC)
- # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES
- # s/t we can set it back before returning.
- set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}")
- # The empty string in this list is important, it represents the case when
- # the libraries have no prefix (shared libraries / DLLs).
- set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}")
- endif (MSVC)
-
- # Search user-installed locations first, so that we prefer user installs
- # to system installs where both exist.
- list(APPEND GLOG_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)
- # Windows (for C:/Program Files prefix).
- list(APPEND GLOG_CHECK_PATH_SUFFIXES
- glog/include
- glog/Include
- Glog/include
- Glog/Include
- google-glog/include # CMake installs with project name prefix.
- google-glog/Include)
-
- list(APPEND GLOG_CHECK_LIBRARY_DIRS
- /usr/local/lib
- /usr/local/homebrew/lib # Mac OS X.
- /opt/local/lib
- /usr/lib)
- # Windows (for C:/Program Files prefix).
- list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES
- glog/lib
- glog/Lib
- Glog/lib
- Glog/Lib
- google-glog/lib # CMake installs with project name prefix.
- google-glog/Lib)
-
- # Search supplied hint directories first if supplied.
- find_path(GLOG_INCLUDE_DIR
- NAMES glog/logging.h
- HINTS ${GLOG_INCLUDE_DIR_HINTS}
- PATHS ${GLOG_CHECK_INCLUDE_DIRS}
- PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES})
- if (NOT GLOG_INCLUDE_DIR OR
- NOT EXISTS ${GLOG_INCLUDE_DIR})
- glog_report_not_found(
- "Could not find glog include directory, set GLOG_INCLUDE_DIR "
- "to directory containing glog/logging.h")
- endif (NOT GLOG_INCLUDE_DIR OR
- NOT EXISTS ${GLOG_INCLUDE_DIR})
-
- find_library(GLOG_LIBRARY NAMES glog
- HINTS ${GLOG_LIBRARY_DIR_HINTS}
- PATHS ${GLOG_CHECK_LIBRARY_DIRS}
- PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES})
- if (NOT GLOG_LIBRARY OR
- NOT EXISTS ${GLOG_LIBRARY})
- glog_report_not_found(
- "Could not find glog library, set GLOG_LIBRARY "
- "to full path to libglog.")
- endif (NOT GLOG_LIBRARY OR
- NOT EXISTS ${GLOG_LIBRARY})
-
- # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets
- # if called.
- set(GLOG_FOUND TRUE)
-
- # Glog does not seem to provide any record of the version in its
- # source tree, thus cannot extract version.
-
- # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and
- # thus FIND_[PATH/LIBRARY] are not called, but specified locations are
- # invalid, otherwise we would report the library as found.
- if (GLOG_INCLUDE_DIR AND
- NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
- glog_report_not_found(
- "Caller defined GLOG_INCLUDE_DIR:"
- " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.")
- endif (GLOG_INCLUDE_DIR AND
- NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
- # TODO: This regex for glog library is pretty primitive, we use lowercase
- # for comparison to handle Windows using CamelCase library names, could
- # this check be better?
- string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY)
- if (GLOG_LIBRARY AND
- NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
- glog_report_not_found(
- "Caller defined GLOG_LIBRARY: "
- "${GLOG_LIBRARY} does not match glog.")
- endif (GLOG_LIBRARY AND
- NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
-
- glog_reset_find_library_prefix()
-
-endif(NOT GLOG_FOUND)
-
-# Set standard CMake FindPackage variables if found.
-if (GLOG_FOUND)
- set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
- set(GLOG_LIBRARIES ${GLOG_LIBRARY})
-endif (GLOG_FOUND)
-
-# If we are using an exported CMake glog target, the include directories are
-# wrapped into the target itself, and do not have to be (and are not)
-# separately specified. In which case, we should not add GLOG_INCLUDE_DIRS
-# to the list of required variables in order that glog be reported as found.
-if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
- set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES)
-else()
- set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES)
-endif()
-
-# Handle REQUIRED / QUIET optional arguments.
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(Glog DEFAULT_MSG
- ${GLOG_REQUIRED_VARIABLES})
-
-# Only mark internal variables as advanced if we found glog, otherwise
-# leave them visible in the standard GUI for the user to set manually.
-if (GLOG_FOUND)
- mark_as_advanced(FORCE GLOG_INCLUDE_DIR
- GLOG_LIBRARY
- glog_DIR) # Autogenerated by find_package(glog)
-endif (GLOG_FOUND)
diff --git a/cmake_modules/FindYamlCpp.cmake b/cmake_modules/FindYamlCpp.cmake
deleted file mode 100644
index 196c4754e2787a9fa4f47eb8716fd2525bc84fd4..0000000000000000000000000000000000000000
--- a/cmake_modules/FindYamlCpp.cmake
+++ /dev/null
@@ -1,99 +0,0 @@
-# Locate yaml-cpp
-#
-# This module defines
-# YAMLCPP_FOUND, if false, do not try to link to yaml-cpp
-# YAMLCPP_LIBNAME, name of yaml library
-# YAMLCPP_LIBRARY, where to find yaml-cpp
-# YAMLCPP_LIBRARY_RELEASE, where to find Release or RelWithDebInfo yaml-cpp
-# YAMLCPP_LIBRARY_DEBUG, where to find Debug yaml-cpp
-# YAMLCPP_INCLUDE_DIR, where to find yaml.h
-# YAMLCPP_LIBRARY_DIR, the directories to find YAMLCPP_LIBRARY
-#
-# By default, the dynamic libraries of yaml-cpp will be found. To find the static ones instead,
-# you must set the YAMLCPP_USE_STATIC_LIBS variable to TRUE before calling find_package(YamlCpp ...)
-
-# attempt to find static library first if this is set
-if(YAMLCPP_USE_STATIC_LIBS)
- set(YAMLCPP_STATIC libyaml-cpp.a)
- set(YAMLCPP_STATIC_DEBUG libyaml-cpp-dbg.a)
-endif()
-
-if(${CMAKE_SYSTEM_NAME} MATCHES "Windows") ### Set Yaml libary name for Windows
- set(YAMLCPP_LIBNAME "libyaml-cppmd" CACHE STRING "Name of YAML library")
- set(YAMLCPP_LIBNAME optimized ${YAMLCPP_LIBNAME} debug ${YAMLCPP_LIBNAME}d)
-else() ### Set Yaml libary name for Unix, Linux, OS X, etc
- set(YAMLCPP_LIBNAME "yaml-cpp" CACHE STRING "Name of YAML library")
-endif()
-
-# find the yaml-cpp include directory
-find_path(YAMLCPP_INCLUDE_DIR
- NAMES yaml-cpp/yaml.h
- PATH_SUFFIXES include
- PATHS
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/include
- ~/Library/Frameworks/yaml-cpp/include/
- /Library/Frameworks/yaml-cpp/include/
- /usr/local/include/
- /usr/include/
- /sw/yaml-cpp/ # Fink
- /opt/local/yaml-cpp/ # DarwinPorts
- /opt/csw/yaml-cpp/ # Blastwave
- /opt/yaml-cpp/)
-
-# find the release yaml-cpp library
-find_library(YAMLCPP_LIBRARY_RELEASE
- NAMES ${YAMLCPP_STATIC} yaml-cpp libyaml-cppmd.lib
- PATH_SUFFIXES lib64 lib Release RelWithDebInfo
- PATHS
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/build
- ~/Library/Frameworks
- /Library/Frameworks
- /usr/local
- /usr
- /sw
- /opt/local
- /opt/csw
- /opt)
-
-# find the debug yaml-cpp library
-find_library(YAMLCPP_LIBRARY_DEBUG
- NAMES ${YAMLCPP_STATIC_DEBUG} yaml-cpp-dbg libyaml-cppmdd.lib
- PATH_SUFFIXES lib64 lib Debug
- PATHS
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/build
- ~/Library/Frameworks
- /Library/Frameworks
- /usr/local
- /usr
- /sw
- /opt/local
- /opt/csw
- /opt)
-
-# set library vars
-set(YAMLCPP_LIBRARY ${YAMLCPP_LIBRARY_RELEASE})
-if(CMAKE_BUILD_TYPE MATCHES Debug AND EXISTS ${YAMLCPP_LIBRARY_DEBUG})
- set(YAMLCPP_LIBRARY ${YAMLCPP_LIBRARY_DEBUG})
-endif()
-
-get_filename_component(YAMLCPP_LIBRARY_RELEASE_DIR ${YAMLCPP_LIBRARY_RELEASE} PATH)
-get_filename_component(YAMLCPP_LIBRARY_DEBUG_DIR ${YAMLCPP_LIBRARY_DEBUG} PATH)
-set(YAMLCPP_LIBRARY_DIR ${YAMLCPP_LIBRARY_RELEASE_DIR} ${YAMLCPP_LIBRARY_DEBUG_DIR})
-
-# handle the QUIETLY and REQUIRED arguments and set YAMLCPP_FOUND to TRUE if all listed variables are TRUE
-include(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(YamlCpp DEFAULT_MSG
- YAMLCPP_INCLUDE_DIR
- YAMLCPP_LIBRARY
- YAMLCPP_LIBRARY_DIR)
-mark_as_advanced(
- YAMLCPP_INCLUDE_DIR
- YAMLCPP_LIBRARY_DIR
- YAMLCPP_LIBRARY
- YAMLCPP_LIBRARY_RELEASE
- YAMLCPP_LIBRARY_RELEASE_DIR
- YAMLCPP_LIBRARY_DEBUG
- YAMLCPP_LIBRARY_DEBUG_DIR)
-
diff --git a/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake
deleted file mode 100644
index 5204cd493ca0740f38d99aef942a7df32ad7f71d..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfcoreConfig.cmake
+++ /dev/null
@@ -1,104 +0,0 @@
-#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)
-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})
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolfcore_INCLUDE_DIR ${wolfcore_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES
-SET(wolfcore_LIBRARY ${wolfcore_LIBRARIES})
diff --git a/cmake_modules/wolfcoreConfig.cmake.in b/cmake_modules/wolfcoreConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..735adef5984b0989b98947873a15e22a910172b4
--- /dev/null
+++ b/cmake_modules/wolfcoreConfig.cmake.in
@@ -0,0 +1,14 @@
+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+@PACKAGE_INIT@
+
+# forwards the correct parameters given to FIND_DEPENDENCIES
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(Threads REQUIRED)
+FIND_DEPENDENCY(Ceres REQUIRED)
+FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
+FIND_DEPENDENCY(yaml-cpp REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 0625a61b5fce5bd66d240e3ac1da89e5d584e455..29a27f5d9c1662a6e6ab7bd4c9d7c02515476c4e 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -295,8 +295,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFirstFrame();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFirstFrame();
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 037ff094a158592c24adee70c28ddfb8cddb1d35..bc8982ca2ce696719aab35105f059639d6c72f23 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -322,8 +322,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFirstFrame();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFirstFrame();
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/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
index d06ace3a796002e87d33a2e9546ad280bea49416..f790a24839bb6c63f1a09703fe283a6f48c11949 100644
--- a/demos/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
@@ -36,18 +36,28 @@ namespace wolf
using namespace Eigen;
-class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
+class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
{
public:
- FactorBearing(const LandmarkBasePtr& _landmark_other_ptr,
+ FactorBearing(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, FactorStatus _status) :
- FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr,
- _landmark_other_ptr, _processor_ptr,
- _apply_loss_function, _status,
- getCapture()->getFrame()->getP(),
- getCapture()->getFrame()->getO(),
- _landmark_other_ptr->getP())
+ FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING",
+ TOP_LMK,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feature_ptr->getCapture()->getFrame()->getP(),
+ _feature_ptr->getCapture()->getFrame()->getO(),
+ _feature_ptr->getCapture()->getSensor()->getP(),
+ _feature_ptr->getCapture()->getSensor()->getO(),
+ _landmark_other_ptr->getP())
{
//
}
@@ -58,9 +68,11 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
}
template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _p2,
+ bool operator ()(const T* const _p_w_r,
+ const T* const _o_w_r,
+ const T* const _p_r_s, // sensor position
+ const T* const _o_r_s, // sensor orientation
+ const T* const _lmk,
T* _res) const;
};
@@ -73,24 +85,30 @@ namespace wolf
{
template<typename T>
-inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
- const T* const _p2, T* _res) const
+inline bool FactorBearing::operator ()( const T* const _p_w_r,
+ const T* const _o_w_r,
+ const T* const _p_r_s, // sensor position
+ const T* const _o_r_s, // sensor orientation
+ const T* const _lmk,
+ T* _res) const
{
// 1. produce a transformation matrix to transform from robot frame to world frame
- Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform
+ Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
+ Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform
+
// Map input pointers into meaningful Eigen elements
- Map<const Matrix<T, 2, 1>> point_w(_p2);
+ Map<const Matrix<T, 2, 1>> point_w(_lmk);
Map<const Matrix<T, 1, 1>> res(_res);
- // 2. Transform world point to robot-referenced point
- Matrix<T, 2, 1> point_r = H_w_r.inverse() * point_w;
+ // 2. Transform world point to sensor-referenced point
+ Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w;
// 3. Get the expected bearing of the point
- T bearing = atan2(point_r(1), point_r(0));
+ T bearing = atan2(point_s(1), point_s(0));
// 4. Get the measured range-and-bearing to the point
- Matrix<T, 2, 1> range_bearing = getMeasurement();
+ Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
// 5. Get the bearing error by comparing against the bearing measurement
T er = range_bearing(1) - bearing;
@@ -100,7 +118,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
er -= T(2*M_PI);
// 6. Compute the residual by scaling according to the standard deviation of the bearing part
- *_res = er * T(getMeasurementSquareRootInformationUpper()(1,1));
+ *_res = er * getMeasurementSquareRootInformationUpper()(1,1);
return true;
}
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 5500b724a95cf12984d12b1327749833a5ff8acc..793d409e4c7522ee2ebcc18ad01ec61cbbf1bbbc 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -253,11 +253,11 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : *problem->getTrajectory())
+ for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
{
Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ kf_pair.second->getCovariance(cov);
+ WOLF_TRACE("KF", kf_pair.second->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 27214245096f91fefad1351f611613516010f40a..39492e1975dbe66cabf86ac9c751d3bd72584deb 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -243,11 +243,11 @@ int main()
// GET COVARIANCES of all states
WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : *problem->getTrajectory())
+ for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
{
Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_INFO("KF", kf->id(), "_cov = \n", cov);
+ kf_pair.second->getCovariance(cov);
+ WOLF_INFO("KF", kf_pair.second->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 821ffefeb052acb75f81ec5d3327ebdaf04c4cd7..e634dac6f2de62d79296525dd6ca2631b4041108 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -309,9 +309,9 @@ int main(int argc, char *argv[])
// draw landmarks
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
- double* position_ptr = (*landmark_it)->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -364,9 +364,9 @@ int main(int argc, char *argv[])
// Draw Final result -------------------------
std::cout << "Drawing final results..." << std::endl;
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
- double* position_ptr = (*landmark_it)->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -384,13 +384,13 @@ 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()->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++)
+ Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->size() * 3);
+ for (auto frame : wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap())
{
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));
+ state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), atan2(*frame->getO()->get(), *(frame->getO()->get() + 1));
else
- state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
+ state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), *frame->getO()->get();
i += 3;
}
@@ -398,10 +398,10 @@ int main(int argc, char *argv[])
std::cout << "Landmarks..." << std::endl;
i = 0;
Eigen::VectorXd landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
- for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
- Eigen::Map<Eigen::Vector2d> landmark((*landmark_it)->getP()->get());
- landmarks.segment(i, 2) = landmark;
+ Eigen::Map<Eigen::Vector2d> landmark_p(landmark->getP()->get());
+ landmarks.segment(i, 2) = landmark_p;
i += 2;
}
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index d73f17d8ec26cd8765ad1dcbeddaee8b66c91944..b946f35b770aabb02f7f237933221131b30afdbc 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -80,34 +80,45 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
- FrameBasePtr getFrame() const;
+ FrameBaseConstPtr getFrame() const;
+ FrameBasePtr getFrame();
private:
void setFrame(const FrameBasePtr _frm_ptr);
public:
- const FeatureBasePtrList& getFeatureList() const;
-
- void getFactorList(FactorBasePtrList& _fac_list) const;
- FactorBasePtrList getFactorList() const;
-
- SensorBasePtr getSensor() const;
+ FeatureBaseConstPtrList getFeatureList() const;
+ FeatureBasePtrList getFeatureList();
+ bool hasFeature(const FeatureBaseConstPtr &_feature) const;
+
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBasePtrList& _fac_list);
+
+ SensorBaseConstPtr getSensor() const;
+ SensorBasePtr getSensor();
virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
private:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+
public:
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
-
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
// State blocks
- StateBlockPtr getStateBlock(const char& _key) const;
- StateBlockPtr getSensorP() const;
- StateBlockPtr getSensorO() const;
- StateBlockPtr getSensorIntrinsic() const;
+ StateBlockConstPtr getStateBlock(const char& _key) const;
+ StateBlockPtr getStateBlock(const char& _key);
+ StateBlockConstPtr getSensorP() const;
+ StateBlockPtr getSensorP();
+ StateBlockConstPtr getSensorO() const;
+ StateBlockPtr getSensorO();
+ StateBlockConstPtr getSensorIntrinsic() const;
+ StateBlockPtr getSensorIntrinsic();
void fix() override;
void unfix() override;
@@ -133,8 +144,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
@@ -158,17 +169,32 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
return cpt;
}
-inline StateBlockPtr CaptureBase::getSensorP() const
+inline StateBlockConstPtr CaptureBase::getSensorP() const
{
return getStateBlock('P');
}
-inline StateBlockPtr CaptureBase::getSensorO() const
+inline StateBlockPtr CaptureBase::getSensorP()
+{
+ return getStateBlock('P');
+}
+
+inline StateBlockConstPtr CaptureBase::getSensorO() const
{
return getStateBlock('O');
}
-inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
+inline StateBlockPtr CaptureBase::getSensorO()
+{
+ return getStateBlock('O');
+}
+
+inline StateBlockConstPtr CaptureBase::getSensorIntrinsic() const
+{
+ return getStateBlock('I');
+}
+
+inline StateBlockPtr CaptureBase::getSensorIntrinsic()
{
return getStateBlock('I');
}
@@ -178,7 +204,13 @@ inline unsigned int CaptureBase::id() const
return capture_id_;
}
-inline FrameBasePtr CaptureBase::getFrame() const
+inline FrameBaseConstPtr CaptureBase::getFrame() const
+{
+ if(frame_ptr_.expired()) return nullptr;
+ else return frame_ptr_.lock();
+}
+
+inline FrameBasePtr CaptureBase::getFrame()
{
if(frame_ptr_.expired()) return nullptr;
else return frame_ptr_.lock();
@@ -189,7 +221,15 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
frame_ptr_ = _frm_ptr;
}
-inline const FeatureBasePtrList& CaptureBase::getFeatureList() const
+inline FeatureBaseConstPtrList CaptureBase::getFeatureList() const
+{
+ FeatureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : feature_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FeatureBasePtrList CaptureBase::getFeatureList()
{
return feature_list_;
}
@@ -199,18 +239,30 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
+inline FactorBaseConstPtrList CaptureBase::getConstrainedByList() const
{
- return constrained_by_list_;
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
+inline FactorBasePtrList CaptureBase::getConstrainedByList()
+{
+ return constrained_by_list_;
+}
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
}
-inline SensorBasePtr CaptureBase::getSensor() const
+inline SensorBaseConstPtr CaptureBase::getSensor() const
+{
+ return sensor_ptr_.lock();
+}
+
+inline SensorBasePtr CaptureBase::getSensor()
{
return sensor_ptr_.lock();
}
@@ -230,7 +282,6 @@ inline void CaptureBase::setTimeStampToNow()
time_stamp_.setToNow();
}
-
} // namespace wolf
#endif
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index a70a54b2aba7e19d8078a209c5b43cad63693671..719c1f800cec4ae0e5d019f56a09b656b2c3f9f5 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -112,17 +112,17 @@ class CaptureMotion : public CaptureBase
// Origin frame and capture
CaptureBasePtr getOriginCapture();
- CaptureBasePtr getOriginCapture() const;
+ CaptureBaseConstPtr getOriginCapture() const;
void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
- bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance);
+ bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const;
void printHeader(int depth, //
- bool constr_by, //
- bool metric, //
- bool state_blocks,
- std::ostream& stream ,
- std::string _tabs = "") const override;
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
// member data:
private:
@@ -181,7 +181,7 @@ inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
return capture_origin_ptr_.lock();
}
-inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture() const
+inline wolf::CaptureBaseConstPtr CaptureMotion::getOriginCapture() const
{
return capture_origin_ptr_.lock();
}
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index d533ff0da8b627dd0d12ca9a32637956da871af5..790ec306644c12e607a4657859bedc8e360284aa 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -307,6 +307,8 @@ class Factory
static bool unregisterCreator(const std::string& _type);
static TypeBasePtr create(const std::string& _type, TypeInput... _input);
std::string getClass() const;
+ static void printAddress();
+ static void printCallbacks();
private:
CallbackMap callbacks_;
@@ -320,10 +322,22 @@ class Factory
Factory(const Factory&) = delete;
void operator=(Factory const&) = delete;
private:
- Factory() { }
- ~Factory() { }
+ Factory();
+ ~Factory();
};
+template<class TypeBase, typename... TypeInput>
+inline Factory<TypeBase, TypeInput...>::Factory()
+{
+ //std::cout << " Factory constructor " << this->getClass() << std::endl;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline Factory<TypeBase, TypeInput...>::~Factory()
+{
+ //std::cout << " Factory destructor " << this->getClass() << std::endl;
+}
+
template<class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
{
@@ -332,7 +346,7 @@ inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string&
std::cout << std::setw(26) << std::left << get().getClass() << " <-- registered " << _type << std::endl;
else
std::cout << std::setw(26) << std::left << get().getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
-
+
return reg;
}
@@ -358,7 +372,7 @@ inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, T
template<class TypeBase, typename... TypeInput>
inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
{
- static Factory instance_;
+ static Factory<TypeBase, TypeInput...> instance_;
return instance_;
}
@@ -368,6 +382,21 @@ inline std::string Factory<TypeBase, TypeInput...>::getClass() const
return "Factory<class TypeBase>";
}
+template<class TypeBase, typename... TypeInput>
+inline void Factory<TypeBase, TypeInput...>::printAddress()
+{
+ std::cout << get().getClass() << " address: " << &get() << std::endl;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline void Factory<TypeBase, TypeInput...>::printCallbacks()
+{
+ std::cout << get().getClass() << " callbacks size: " << get().callbacks_.size() << std::endl;
+ for (auto cb: get().callbacks_){
+ std::cout << "\t" << cb.first << std::endl;
+ }
+}
+
} // namespace wolf
namespace wolf
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index f450b60deb50b1ffb65e53c87cf22bac28052f84..1f7cf9eaff36bdbae80a8802981ea88f9ba019a6 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -105,7 +105,8 @@ class NodeBase
void setType(const std::string& _type);
void setName(const std::string& _name);
- ProblemPtr getProblem() const;
+ ProblemConstPtr getProblem() const;
+ ProblemPtr getProblem();
};
} // namespace wolf
@@ -160,7 +161,12 @@ inline void NodeBase::setName(const std::string& _name)
node_name_ = _name;
}
-inline ProblemPtr NodeBase::getProblem() const
+inline ProblemConstPtr NodeBase::getProblem() const
+{
+ return problem_ptr_.lock();
+}
+
+inline ProblemPtr NodeBase::getProblem()
{
return problem_ptr_.lock();
}
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 197a6bfb1b30d79a64376f9468b9481c0a4bc3e3..25e9325fd3b61f425f085bd47e9b788ebd494aa9 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -197,27 +197,39 @@ struct MatrixSizeCheck
// TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
/////////////////////////////////////////////////////////////////////////
+#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \
+ typedef std::shared_ptr<Name> Name##Ptr; \
+ typedef std::shared_ptr<const Name> Name##ConstPtr; \
+ typedef std::weak_ptr<Name> Name##WPtr; \
+ typedef std::weak_ptr<const Name> Name##ConstWPtr; \
+
#define WOLF_PTR_TYPEDEFS(ClassName) \
class ClassName; \
- typedef std::shared_ptr<ClassName> ClassName##Ptr; \
- typedef std::shared_ptr<const ClassName> ClassName##ConstPtr; \
- typedef std::weak_ptr<ClassName> ClassName##WPtr;
+ WOLF_DECLARED_PTR_TYPEDEFS(ClassName); \
+
+#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
+ struct StructName; \
+ WOLF_DECLARED_PTR_TYPEDEFS(StructName); \
#define WOLF_LIST_TYPEDEFS(ClassName) \
class ClassName; \
- typedef std::list<ClassName##Ptr> ClassName##PtrList; \
- typedef ClassName##PtrList::iterator ClassName##Iter; \
- typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
- typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
- typedef ClassName##WPtrList::iterator ClassName##WIter; \
- typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
- typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
+ typedef std::list<ClassName##Ptr> ClassName##PtrList; \
+ typedef ClassName##PtrList::iterator ClassName##PtrListIter; \
+ typedef ClassName##PtrList::const_iterator ClassName##PtrListConstIter; \
+ typedef ClassName##PtrList::reverse_iterator ClassName##PtrListRevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WPtrListIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WPtrListConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WPtrListRevIter; \
+ typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \
+ typedef ClassName##ConstPtrList::iterator ClassName##ConstPtrListIter; \
+ typedef ClassName##ConstPtrList::const_iterator ClassName##ConstPtrListConstIter; \
+ typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstPtrListRevIter; \
+ typedef std::list<ClassName##ConstWPtr> ClassName##ConstWPtrList; \
+ typedef ClassName##ConstWPtrList::iterator ClassName##ConstWPtrListIter; \
+ typedef ClassName##ConstWPtrList::const_iterator ClassName##ConstWPtrListConstIter; \
+ typedef ClassName##ConstWPtrList::reverse_iterator ClassName##ConstWPtrListRevIter; \
-#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
- struct StructName; \
- typedef std::shared_ptr<StructName> StructName##Ptr; \
- typedef std::shared_ptr<const StructName> StructName##ConstPtr; \
// NodeBase
WOLF_PTR_TYPEDEFS(NodeBase);
@@ -258,7 +270,8 @@ WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
class TimeStamp;
-typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
+typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap;
+typedef std::map<TimeStamp, FrameBaseConstPtr> FrameConstPtrMap;
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index b2c0d0e4f6f2d589278a243e4cc34de6f6640c3e..88f28a3880d513ace93166d6e38cc80a178455be 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -35,6 +35,7 @@ class FactorAnalytic: public FactorBase
{
protected:
std::vector<StateBlockPtr> state_ptr_vector_;
+ std::vector<StateBlockConstPtr> state_ptr_const_vector_;
std::vector<unsigned int> state_block_sizes_vector_;
public:
@@ -60,6 +61,27 @@ class FactorAnalytic: public FactorBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
+ FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr = nullptr,
+ StateBlockPtr _state2Ptr = nullptr,
+ StateBlockPtr _state3Ptr = nullptr,
+ StateBlockPtr _state4Ptr = nullptr,
+ StateBlockPtr _state5Ptr = nullptr,
+ StateBlockPtr _state6Ptr = nullptr,
+ StateBlockPtr _state7Ptr = nullptr,
+ StateBlockPtr _state8Ptr = nullptr,
+ StateBlockPtr _state9Ptr = nullptr );
+
~FactorAnalytic() override = default;
/** \brief Returns a vector of pointers to the states
@@ -67,7 +89,8 @@ class FactorAnalytic: public FactorBase
* Returns a vector of pointers to the state in which this factor depends
*
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override;
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override;
/** \brief Returns a vector of sizes of the state blocks
*
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 0b8f9e111d007024ff457b2303cd704604a4fb10..c10c0b58b799f6171a418c9a6217b4d69a9e6f88 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -64,7 +64,8 @@ class FactorAutodiff : public FactorBase
protected:
- std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
static const std::vector<unsigned int> jacobian_locations_;
mutable std::array<WolfJet, RES> residuals_jets_;
@@ -106,10 +107,58 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state9Ptr,
StateBlockPtr _state10Ptr,
StateBlockPtr _state11Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
{
// asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
assert(_state0Ptr != nullptr && "nullptr state block");
assert(_state1Ptr != nullptr && "nullptr state block");
assert(_state2Ptr != nullptr && "nullptr state block");
@@ -301,9 +350,9 @@ class FactorAutodiff : public FactorBase
jacobians_.push_back(Ji);
}
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
}
/** \brief Returns a vector of pointers to the states
@@ -311,7 +360,11 @@ class FactorAutodiff : public FactorBase
* Returns a vector of pointers to the state in which this factor depends
*
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
{
return state_ptrs_;
}
@@ -341,265 +394,316 @@ class FactorAutodiff : public FactorBase
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = B10;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 11;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9 + B10> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
- mutable std::array<WolfJet, B8> jets_8_;
- mutable std::array<WolfJet, B9> jets_9_;
- mutable std::array<WolfJet, B10> jets_10_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr,
- StateBlockPtr _state10Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
- assert(_state8Ptr != nullptr && "nullptr state block");
- assert(_state9Ptr != nullptr && "nullptr state block");
- assert(_state10Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B10; i++)
- jets_10_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = B10;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 11;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 + B10> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- parameters[9],
- parameters[10],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i].a = parameters[8][i];
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i].a = parameters[9][i];
- for (unsigned int i = 0; i < B10; i++)
- jets_10_[i].a = parameters[10][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////// SPECIALIZATION 10 BLOCKS ////////////////////////////////////////////////////////////////////////
@@ -607,2096 +711,2488 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 10;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
- mutable std::array<WolfJet, B8> jets_8_;
- mutable std::array<WolfJet, B9> jets_9_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
- assert(_state8Ptr != nullptr && "nullptr state block");
- assert(_state9Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- parameters[9],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i].a = parameters[8][i];
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i].a = parameters[9][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 9;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
- mutable std::array<WolfJet, B8> jets_8_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
- assert(_state8Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i].a = parameters[8][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 8;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 7;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 6;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 5;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 4;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 10;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 9;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 8;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 7;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 6;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 5;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 4;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 3;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 3;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
-
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 2;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = 0;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 2;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
-
template <class FacT,unsigned int RES,unsigned int B0>
class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = 0;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 1;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = 0;
+ static const unsigned int block2Size = 0;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 1;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr}),
+ state_ptrs_const_({_state0Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 05508895ca766ee28ae61bd4bd615cbb27288edf..1894148de2ea576614b14d6de81a4f138c4e037c 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -25,8 +25,8 @@
// Forward declarations for node templates
namespace wolf{
class FeatureBase;
-class TrajectoryIter;
-class TrajectoryRevIter;
+//class TrajectoryIter;
+//class TrajectoryRevIter;
}
//Wolf includes
@@ -77,43 +77,28 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
{
friend FeatureBase;
private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
static unsigned int factor_id_count_;
+ FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
+ CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
+ FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
+ LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
+
protected:
unsigned int factor_id_;
- FactorTopology topology_; ///< topology of factor
- FactorStatus status_; ///< status of factor
- bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
- CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
- FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
- LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
- ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
- Eigen::VectorXd measurement_; ///< the measurement vector
- Eigen::MatrixXd measurement_sqrt_information_upper_;///< the squared root information matrix ///< ProcessorBase pointer list
+ FactorTopology topology_; ///< topology of factor
+ FactorStatus status_; ///< status of factor
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
+
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
void setProblem(ProblemPtr) override final;
public:
- /** \brief Default constructor.
- *
- * IMPORTANT: "other" means "of another branch of the wolf tree".
- * You should only provide a non-nullptr in frame/capture/feature_other_ptr in case of a frame/capture/feature involved in this factor
- * that does not located in the same branch.
- **/
- FactorBase(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
@@ -125,8 +110,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
-
-
~FactorBase() override = default;
virtual void remove(bool viral_remove_empty_parent=false);
@@ -154,7 +137,8 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
+ virtual std::vector<StateBlockConstPtr> getStateBlockPtrVector() const = 0;
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() = 0;
/** \brief Returns a vector of the states sizes
**/
@@ -170,19 +154,23 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the feature constrained from
**/
- FeatureBasePtr getFeature() const;
+ FeatureBaseConstPtr getFeature() const;
+ FeatureBasePtr getFeature();
/** \brief Returns a pointer to its capture
**/
- CaptureBasePtr getCapture() const;
+ CaptureBaseConstPtr getCapture() const;
+ CaptureBasePtr getCapture();
/** \brief Returns a pointer to its frame
**/
- FrameBasePtr getFrame() const;
+ FrameBaseConstPtr getFrame() const;
+ FrameBasePtr getFrame();
/** \brief Returns a pointer to its capture's sensor
**/
- SensorBasePtr getSensor() const;
+ SensorBaseConstPtr getSensor() const;
+ SensorBasePtr getSensor();
/** \brief Returns the factor residual size
**/
@@ -202,37 +190,45 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the first frame constrained to
**/
- FrameBasePtr getFrameOther() const;
+ FrameBaseConstPtr getFrameOther() const;
+ FrameBasePtr getFrameOther();
/** \brief Returns a pointer to the first capture constrained to
**/
- CaptureBasePtr getCaptureOther() const;
+ CaptureBaseConstPtr getCaptureOther() const;
+ CaptureBasePtr getCaptureOther();
/** \brief Returns a pointer to the first feature constrained to
**/
- FeatureBasePtr getFeatureOther() const;
+ FeatureBaseConstPtr getFeatureOther() const;
+ FeatureBasePtr getFeatureOther();
/** \brief Returns a pointer to the first landmark constrained to
**/
- LandmarkBasePtr getLandmarkOther() const;
+ LandmarkBaseConstPtr getLandmarkOther() const;
+ LandmarkBasePtr getLandmarkOther();
// get pointer lists to other nodes
- FrameBaseWPtrList getFrameOtherList() const { return frame_other_list_; }
- CaptureBaseWPtrList getCaptureOtherList() const { return capture_other_list_; }
- FeatureBaseWPtrList getFeatureOtherList() const { return feature_other_list_; }
- LandmarkBaseWPtrList getLandmarkOtherList() const { return landmark_other_list_; }
-
- bool hasFrameOther(const FrameBasePtr& _frm_other) const;
- bool hasCaptureOther(const CaptureBasePtr& _cap_other) const;
- bool hasFeatureOther(const FeatureBasePtr& _ftr_other) const;
- bool hasLandmarkOther(const LandmarkBasePtr& _lmk_other) const;
+ FrameBaseWPtrList getFrameOtherList() { return frame_other_list_; }
+ CaptureBaseWPtrList getCaptureOtherList() { return capture_other_list_; }
+ FeatureBaseWPtrList getFeatureOtherList() { return feature_other_list_; }
+ LandmarkBaseWPtrList getLandmarkOtherList() { return landmark_other_list_; }
+ FrameBaseConstWPtrList getFrameOtherList() const;
+ CaptureBaseConstWPtrList getCaptureOtherList() const;
+ FeatureBaseConstWPtrList getFeatureOtherList() const;
+ LandmarkBaseConstWPtrList getLandmarkOtherList() const;
+
+ bool hasFrameOther(const FrameBaseConstPtr& _frm_other) const;
+ bool hasCaptureOther(const CaptureBaseConstPtr& _cap_other) const;
+ bool hasFeatureOther(const FeatureBaseConstPtr& _ftr_other) const;
+ bool hasLandmarkOther(const LandmarkBaseConstPtr& _lmk_other) const;
- public:
/**
* @brief getProcessor
* @return
*/
- ProcessorBasePtr getProcessor() const;
+ ProcessorBaseConstPtr getProcessor() const;
+ ProcessorBasePtr getProcessor();
void link(FeatureBasePtr ftr);
template<typename classType, typename... T>
@@ -252,8 +248,8 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
@@ -291,7 +287,12 @@ inline unsigned int FactorBase::id() const
return factor_id_;
}
-inline FeatureBasePtr FactorBase::getFeature() const
+inline FeatureBaseConstPtr FactorBase::getFeature() const
+{
+ return feature_ptr_.lock();
+}
+
+inline FeatureBasePtr FactorBase::getFeature()
{
return feature_ptr_.lock();
}
@@ -306,7 +307,15 @@ inline bool FactorBase::getApplyLossFunction() const
return apply_loss_function_;
}
-inline FrameBasePtr FactorBase::getFrameOther() const
+inline FrameBaseConstPtr FactorBase::getFrameOther() const
+{
+ if (frame_other_list_.empty()) return nullptr;
+ if (frame_other_list_.front().expired()) return nullptr;
+
+ return frame_other_list_.front().lock();
+}
+
+inline FrameBasePtr FactorBase::getFrameOther()
{
if (frame_other_list_.empty()) return nullptr;
if (frame_other_list_.front().expired()) return nullptr;
@@ -314,7 +323,15 @@ inline FrameBasePtr FactorBase::getFrameOther() const
return frame_other_list_.front().lock();
}
-inline CaptureBasePtr FactorBase::getCaptureOther() const
+inline CaptureBaseConstPtr FactorBase::getCaptureOther() const
+{
+ if (capture_other_list_.empty()) return nullptr;
+ if (capture_other_list_.front().expired()) return nullptr;
+
+ return capture_other_list_.front().lock();
+}
+
+inline CaptureBasePtr FactorBase::getCaptureOther()
{
if (capture_other_list_.empty()) return nullptr;
if (capture_other_list_.front().expired()) return nullptr;
@@ -322,7 +339,7 @@ inline CaptureBasePtr FactorBase::getCaptureOther() const
return capture_other_list_.front().lock();
}
-inline FeatureBasePtr FactorBase::getFeatureOther() const
+inline FeatureBaseConstPtr FactorBase::getFeatureOther() const
{
if (feature_other_list_.empty()) return nullptr;
if (feature_other_list_.front().expired()) return nullptr;
@@ -330,7 +347,15 @@ inline FeatureBasePtr FactorBase::getFeatureOther() const
return feature_other_list_.front().lock();
}
-inline LandmarkBasePtr FactorBase::getLandmarkOther() const
+inline FeatureBasePtr FactorBase::getFeatureOther()
+{
+ if (feature_other_list_.empty()) return nullptr;
+ if (feature_other_list_.front().expired()) return nullptr;
+
+ return feature_other_list_.front().lock();
+}
+
+inline LandmarkBaseConstPtr FactorBase::getLandmarkOther() const
{
if (landmark_other_list_.empty()) return nullptr;
if (landmark_other_list_.front().expired()) return nullptr;
@@ -338,7 +363,20 @@ inline LandmarkBasePtr FactorBase::getLandmarkOther() const
return landmark_other_list_.front().lock();
}
-inline ProcessorBasePtr FactorBase::getProcessor() const
+inline LandmarkBasePtr FactorBase::getLandmarkOther()
+{
+ if (landmark_other_list_.empty()) return nullptr;
+ if (landmark_other_list_.front().expired()) return nullptr;
+
+ return landmark_other_list_.front().lock();
+}
+
+inline ProcessorBaseConstPtr FactorBase::getProcessor() const
+{
+ return processor_ptr_.lock();
+}
+
+inline ProcessorBasePtr FactorBase::getProcessor()
{
return processor_ptr_.lock();
}
@@ -358,5 +396,37 @@ inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpp
return measurement_sqrt_information_upper_;
}
+inline FrameBaseConstWPtrList FactorBase::getFrameOtherList() const
+{
+ FrameBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : frame_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline CaptureBaseConstWPtrList FactorBase::getCaptureOtherList() const
+{
+ CaptureBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : capture_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FeatureBaseConstWPtrList FactorBase::getFeatureOtherList() const
+{
+ FeatureBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : feature_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline LandmarkBaseConstWPtrList FactorBase::getLandmarkOtherList() const
+{
+ LandmarkBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : landmark_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index a132c3c0ac105fc0476f18ab2c625ee5a5801bad..6003c446732399f1abe6c4127dfb257543c82c2c 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -166,8 +166,8 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_current,
inline Eigen::VectorXd FactorRelativePose3d::expectation() const
{
Eigen::VectorXd exp(7);
- FrameBasePtr frm_current = getFeature()->getFrame();
- FrameBasePtr frm_past = getFrameOther();
+ auto frm_current = getFeature()->getFrame();
+ auto frm_past = getFrameOther();
const Eigen::VectorXd frame_current_pos = frm_current->getP()->getState();
const Eigen::VectorXd frame_current_ori = frm_current->getO()->getState();
const Eigen::VectorXd frame_past_pos = frm_past->getP()->getState();
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 5259e5c51f9d5dece25eb3b3f35d7cb5570541d7..beb5eae22b8667750d43c2a51ade0a24857c6483 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -116,20 +116,22 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void setExpectation(const Eigen::VectorXd& expectation);
// wolf tree access
- FrameBasePtr getFrame() const;
+ FrameBaseConstPtr getFrame() const;
+ FrameBasePtr getFrame();
- CaptureBasePtr getCapture() const;
+ CaptureBaseConstPtr getCapture() const;
+ CaptureBasePtr getCapture();
- const FactorBasePtrList& getFactorList() const;
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList & _fac_list) const;
+ void getFactorList(FactorBasePtrList & _fac_list);
+ bool hasFactor(FactorBaseConstPtr _fac) const;
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
-
-
-
- // all factors
- void getFactorList(FactorBasePtrList & _fac_list) const;
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
@@ -148,8 +150,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, FeatureBasePtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FeatureBaseConstPtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
@@ -167,20 +169,41 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
namespace wolf{
- template<typename classType, typename... T>
- std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
- {
- std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
- ftr->link(_cpt_ptr);
- return ftr;
- }
+template<typename classType, typename... T>
+std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
+{
+ std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
+ ftr->link(_cpt_ptr);
+ return ftr;
+}
inline unsigned int FeatureBase::getHits() const
{
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
+inline FactorBaseConstPtrList FeatureBase::getFactorList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : factor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList FeatureBase::getFactorList()
+{
+ return factor_list_;
+}
+
+inline FactorBaseConstPtrList FeatureBase::getConstrainedByList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList FeatureBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -190,7 +213,12 @@ inline unsigned int FeatureBase::id() const
return feature_id_;
}
-inline CaptureBasePtr FeatureBase::getCapture() const
+inline CaptureBaseConstPtr FeatureBase::getCapture() const
+{
+ return capture_ptr_.lock();
+}
+
+inline CaptureBasePtr FeatureBase::getCapture()
{
return capture_ptr_.lock();
}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index c7ab25754729c29711e65d532a2e23b36d7d309a..8d64663585af0982d7ac3209a3fcb808c753080a 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -104,7 +104,8 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// State blocks ----------------------------------------------------
public:
- StateBlockPtr getV() const;
+ StateBlockConstPtr getV() const;
+ StateBlockPtr getV();
protected:
void setProblem(ProblemPtr _problem) override final;
@@ -116,31 +117,59 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// Wolf tree access ---------------------------------------------------
public:
- TrajectoryBasePtr getTrajectory() const;
+ TrajectoryBaseConstPtr getTrajectory() const;
+ TrajectoryBasePtr getTrajectory();
- FrameBasePtr getPreviousFrame() const;
- FrameBasePtr getNextFrame() const;
+ FrameBaseConstPtr getPreviousFrame(const unsigned int& i = 1) const;
+ FrameBaseConstPtr getNextFrame(const unsigned int& i = 1) const;
+ FrameBasePtr getPreviousFrame(const unsigned int& i = 1);
+ FrameBasePtr getNextFrame(const unsigned int& i = 1);
+
+ CaptureBaseConstPtrList getCaptureList() const;
+ CaptureBasePtrList getCaptureList();
+ bool hasCapture(const CaptureBaseConstPtr& _capture) const;
+
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBasePtrList& _fac_list);
+
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr& _factor) const;
- const CaptureBasePtrList& getCaptureList() const;
template <class C>
- CaptureBasePtr getCaptureOfType() const;
- CaptureBasePtr getCaptureOfType(const std::string& type) const;
+ CaptureBaseConstPtr getCaptureOfType() const;
template <class C>
- CaptureBasePtrList getCapturesOfType() const;
- CaptureBasePtrList getCapturesOfType(const std::string& type) const;
- CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
- CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
- CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
+ CaptureBasePtr getCaptureOfType();
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
- FactorBasePtrList getFactorList() const;
- void getFactorList(FactorBasePtrList& _fac_list) const;
+ CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
+ CaptureBasePtr getCaptureOfType(const std::string& type);
- unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr& _factor) const;
+ template <class C>
+ CaptureBaseConstPtrList getCapturesOfType() const;
+ template <class C>
+ CaptureBasePtrList getCapturesOfType();
+ CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
+ CaptureBasePtrList getCapturesOfType(const std::string& type);
+
+ CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr) const;
+ CaptureBasePtr getCaptureOf(SensorBaseConstPtr _sensor_ptr);
+
+ CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const;
+ CaptureBasePtr getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type);
+
+ CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const;
+ CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr);
+
+ FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr) const;
+ FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr);
+
+ FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const;
+ FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type);
+
+ unsigned int getHits() const;
// Debug and info -------------------------------------------------------
virtual void printHeader(int depth, //
@@ -157,8 +186,8 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
@@ -204,17 +233,35 @@ inline TimeStamp FrameBase::getTimeStamp() const
return time_stamp_;
}
-inline StateBlockPtr FrameBase::getV() const
+inline StateBlockConstPtr FrameBase::getV() const
+{
+ return getStateBlock('V');
+}
+
+inline StateBlockPtr FrameBase::getV()
{
return getStateBlock('V');
}
-inline TrajectoryBasePtr FrameBase::getTrajectory() const
+inline TrajectoryBaseConstPtr FrameBase::getTrajectory() const
+{
+ return trajectory_ptr_.lock();
+}
+
+inline TrajectoryBasePtr FrameBase::getTrajectory()
{
return trajectory_ptr_.lock();
}
-inline const CaptureBasePtrList& FrameBase::getCaptureList() const
+inline CaptureBaseConstPtrList FrameBase::getCaptureList() const
+{
+ CaptureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : capture_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline CaptureBasePtrList FrameBase::getCaptureList()
{
return capture_list_;
}
@@ -224,7 +271,15 @@ inline unsigned int FrameBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
+inline FactorBaseConstPtrList FrameBase::getConstrainedByList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList FrameBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -235,19 +290,38 @@ inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
}
template <class C>
-inline CaptureBasePtr FrameBase::getCaptureOfType() const
+inline CaptureBaseConstPtr FrameBase::getCaptureOfType() const
+{
+ for (auto capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ return capture_ptr;
+ return nullptr;
+}
+
+template <class C>
+inline CaptureBasePtr FrameBase::getCaptureOfType()
{
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
return capture_ptr;
return nullptr;
}
template <class C>
-inline CaptureBasePtrList FrameBase::getCapturesOfType() const
+inline CaptureBaseConstPtrList FrameBase::getCapturesOfType() const
+{
+ CaptureBaseConstPtrList captures;
+ for (auto capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
+template <class C>
+inline CaptureBasePtrList FrameBase::getCapturesOfType()
{
CaptureBasePtrList captures;
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
captures.push_back(capture_ptr);
return captures;
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 59f423f4295472897cc2d8065811aba72538fbfb..4e86c8892af30795657610cf5576f724a7c792d3 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -46,7 +46,14 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
HardwareBase();
~HardwareBase() override;
- const SensorBasePtrList& getSensorList() const;
+ SensorBaseConstPtrList getSensorList() const;
+ SensorBasePtrList getSensorList();
+
+ /** \brief get a sensor pointer by its name
+ * \param _sensor_name The sensor name, as it was installed with installSensor()
+ */
+ SensorBaseConstPtr getSensor(const std::string& _sensor_name) const;
+ SensorBasePtr getSensor(const std::string& _sensor_name);
virtual void printHeader(int depth, //
bool constr_by, //
@@ -61,8 +68,8 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, HardwareBaseConstPtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
@@ -74,7 +81,15 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
namespace wolf {
-inline const SensorBasePtrList& HardwareBase::getSensorList() const
+inline SensorBaseConstPtrList HardwareBase::getSensorList() const
+{
+ SensorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : sensor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline SensorBasePtrList HardwareBase::getSensorList()
{
return sensor_list_;
}
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 7984e6773a80615aa33a84f67156cf83f1c42ab6..1d08d5748404c4bbe6151c8e28cf6827dc79f183 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -49,7 +49,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
+ //std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
+ //std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
static unsigned int landmark_id_count_;
@@ -80,7 +81,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
void setId(unsigned int _id);
// State blocks
- std::vector<StateBlockPtr> getUsedStateBlockVec() const;
+ //std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
+ //std::vector<StateBlockPtr> getUsedStateBlockVec();
bool getCovariance(Eigen::MatrixXd& _cov) const;
// Descriptor
@@ -89,13 +91,14 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
double getDescriptor(unsigned int _ii) const;
void setDescriptor(const Eigen::VectorXd& _descriptor);
-
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
+ MapBaseConstPtr getMap() const;
+ MapBasePtr getMap();
- MapBasePtr getMap() const;
void link(MapBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
@@ -120,8 +123,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
@@ -146,7 +149,12 @@ std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all
return lmk;
}
-inline MapBasePtr LandmarkBase::getMap() const
+inline MapBaseConstPtr LandmarkBase::getMap() const
+{
+ return map_ptr_.lock();
+}
+
+inline MapBasePtr LandmarkBase::getMap()
{
return map_ptr_.lock();
}
@@ -173,7 +181,15 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
+inline FactorBaseConstPtrList LandmarkBase::getConstrainedByList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList LandmarkBase::getConstrainedByList()
{
return constrained_by_list_;
}
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 797d2bd3892e339c96a8c40e591eb56d39818ee5..af09f86002c697ad026ad9942d218026808555c4 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -111,7 +111,8 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
public:
- const LandmarkBasePtrList& getLandmarkList() const;
+ LandmarkBaseConstPtrList getLandmarkList() const;
+ LandmarkBasePtrList getLandmarkList();
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
@@ -129,13 +130,22 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, MapBaseConstPtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
std::string dateTimeNow();
};
-inline const LandmarkBasePtrList& MapBase::getLandmarkList() const
+inline LandmarkBaseConstPtrList MapBase::getLandmarkList() const
+{
+ LandmarkBaseConstPtrList list_const;
+ for (auto&& obj_ptr : landmark_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline LandmarkBasePtrList MapBase::getLandmarkList()
{
return landmark_list_;
}
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 08a7ab52fd0598315adf2b9150ad80efd77043d0..9dfb7cae3e62adac06662e9414c75764045148e2 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -37,13 +37,11 @@ struct ParamsProcessorBase;
//wolf includes
#include "core/common/wolf.h"
+#include "core/utils/params_server.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/utils/params_server.h"
-#include "core/sensor/factory_sensor.h"
-#include "core/processor/factory_processor.h"
-#include <core/processor/motion_provider.h>
#include "core/state_block/state_composite.h"
+#include "core/processor/motion_provider.h"
// std includes
#include <mutex>
@@ -74,12 +72,12 @@ class Problem : public std::enable_shared_from_this<Problem>
friend MotionProvider;
protected:
- TreeManagerBasePtr tree_manager_;
+ TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
std::map<int, MotionProviderPtr> motion_provider_map_;
- std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
+ std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
@@ -113,13 +111,15 @@ class Problem : public std::enable_shared_from_this<Problem>
// Tree manager --------------------------------------
public:
void setTreeManager(TreeManagerBasePtr _gm);
- TreeManagerBasePtr getTreeManager() const;
+ TreeManagerBaseConstPtr getTreeManager() const;
+ TreeManagerBasePtr getTreeManager();
// Hardware branch ------------------------------------
- HardwareBasePtr getHardware() const;
+ HardwareBaseConstPtr getHardware() const;
+ HardwareBasePtr getHardware();
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
@@ -151,11 +151,14 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
- SensorBasePtr findSensor(const std::string& _sensor_name) const;
+ SensorBaseConstPtr findSensor(const std::string& _sensor_name) const;
+ SensorBasePtr findSensor(const std::string& _sensor_name);
+
/** \brief get a processor pointer by its name
* \param _processor_name The processor name, as it was installed with installProcessor()
*/
- ProcessorBasePtr findProcessor(const std::string& _processor_name) const;
+ ProcessorBaseConstPtr findProcessor(const std::string& _processor_name) const;
+ ProcessorBasePtr findProcessor(const std::string& _processor_name);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -201,11 +204,12 @@ class Problem : public std::enable_shared_from_this<Problem>
void removeMotionProvider(MotionProviderPtr proc);
public:
- std::map<int,MotionProviderPtr>& getMotionProviderMap();
- const std::map<int,MotionProviderPtr>& getMotionProviderMap() const;
+ std::map<int,MotionProviderConstPtr> getMotionProviderMap() const;
+ std::map<int,MotionProviderPtr> getMotionProviderMap();
// Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectory() const;
+ TrajectoryBaseConstPtr getTrajectory() const;
+ TrajectoryBasePtr getTrajectory();
// Prior
bool isPriorSet() const;
@@ -233,9 +237,9 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state);
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame from string frame_structure and state
* \param _time_stamp Time stamp of the frame
@@ -250,8 +254,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const VectorComposite& _frame_state);
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state);
/** \brief Emplace frame from state
* \param _time_stamp Time stamp of the frame
@@ -282,8 +286,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim);
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim);
/** \brief Emplace frame from state vector
* \param _time_stamp Time stamp of the frame
@@ -298,7 +302,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const Eigen::VectorXd& _frame_state);
+ const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame, guess all values
* \param _time_stamp Time stamp of the frame
@@ -315,8 +319,10 @@ class Problem : public std::enable_shared_from_this<Problem>
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
// Frame getters
- FrameBasePtr getLastFrame( ) const;
- FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBaseConstPtr getLastFrame( ) const;
+ FrameBasePtr getLastFrame( );
+ FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
/** \brief Give the permission to a processor to create a new key Frame
*
@@ -346,7 +352,8 @@ class Problem : public std::enable_shared_from_this<Problem>
// Map branch -----------------------------------------
- MapBasePtr getMap() const;
+ MapBaseConstPtr getMap() const;
+ MapBasePtr getMap();
void setMap(MapBasePtr);
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
@@ -362,9 +369,9 @@ class Problem : public std::enable_shared_from_this<Problem>
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
- bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
- 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 getCovarianceBlock(StateBlockConstPtr _state1, StateBlockConstPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
+ bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
+ bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
@@ -437,7 +444,12 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
-inline TreeManagerBasePtr Problem::getTreeManager() const
+inline TreeManagerBaseConstPtr Problem::getTreeManager() const
+{
+ return tree_manager_;
+}
+
+inline TreeManagerBasePtr Problem::getTreeManager()
{
return tree_manager_;
}
@@ -447,12 +459,15 @@ inline bool Problem::isPriorSet() const
return prior_options_ == nullptr;
}
-inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap()
+inline std::map<int,MotionProviderConstPtr> Problem::getMotionProviderMap() const
{
- return motion_provider_map_;
+ std::map<int,MotionProviderConstPtr> map_const;
+ for (auto&& pair : motion_provider_map_)
+ map_const[pair.first] = pair.second;
+ return map_const;
}
-inline const std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() const
+inline std::map<int,MotionProviderPtr> Problem::getMotionProviderMap()
{
return motion_provider_map_;
}
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index 9a6e84d9c8949b4c9d5c5e69cdf3ec071d66fa57..d7bd26d00df45c7f14d0ff5ae01966e271af03ef 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -80,7 +80,7 @@ class MotionProvider
void setStatePriority(int);
public:
- const StateStructure& getStateStructure ( ) { return state_structure_; };
+ const StateStructure& getStateStructure ( ) const { return state_structure_; };
void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index b2e0f6fdb42e6715b0ed41f3c720b18f8b12792e..52eab3b6b239922b5afbfe08e84361acd2c0e98e 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -92,6 +92,7 @@ class Buffer
public:
typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator
+ typedef typename std::map<TimeStamp,T>::const_iterator ConstIterator; // buffer iterator
Buffer(){};
~Buffer(void){};
@@ -101,27 +102,28 @@ public:
* Select from the buffer the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
- T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
/**\brief Select an element iterator from the buffer
*
* Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
+ ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
- T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
- T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
- T selectFirst();
+ T selectFirst() const;
- T selectLast();
+ T selectLast() const;
/**\brief Buffer size
*
*/
- SizeStd size(void);
+ SizeStd size(void) const;
/**\brief Add a element to the buffer
*
@@ -132,6 +134,7 @@ public:
*
* elements are ordered from most recent to oldest
*/
+ const std::map<TimeStamp,T>& getContainer() const;
std::map<TimeStamp,T>& getContainer();
/**\brief Remove all elements in the buffer with a time stamp older than the specified
@@ -152,7 +155,7 @@ public:
/**\brief is the buffer empty ?
*
*/
- bool empty();
+ bool empty() const;
protected:
/**\brief Check time tolerance
@@ -160,14 +163,19 @@ protected:
* Check if the time distance between two time stamps is smaller than
* the time tolerance.
*/
- static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
+ static bool checkTimeTolerance(const TimeStamp& _time_stamp1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance);
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the minimum time tolerance of the two frames.
*/
- static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2);
+ static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
+ const double& _time_tolerance1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance2);
protected:
@@ -327,14 +335,15 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
void captureCallback(CaptureBasePtr _capture);
- SensorBasePtr getSensor() const;
+ SensorBaseConstPtr getSensor() const;
+ SensorBasePtr getSensor();
+
private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
public:
bool isMotionProvider() const;
-
bool isVotingActive() const;
void setVotingActive(bool _voting_active = true);
@@ -367,8 +376,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, ProcessorBaseConstPtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
inline void ProcessorBase::startCaptureProfiling()
@@ -418,7 +427,12 @@ inline unsigned int ProcessorBase::id() const
return processor_id_;
}
-inline SensorBasePtr ProcessorBase::getSensor() const
+inline SensorBaseConstPtr ProcessorBase::getSensor() const
+{
+ return sensor_ptr_.lock();
+}
+
+inline SensorBasePtr ProcessorBase::getSensor()
{
return sensor_ptr_.lock();
}
@@ -448,6 +462,42 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&...
/////////////////////////////////////////////////////////////////////////////////////////
+template <typename T>
+typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+{
+ Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+ bool post_exists = (post != container_.end());
+
+ bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+ if (prev_exists)
+ {
+ Buffer<T>::ConstIterator prev = std::prev(post);
+
+ bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+ if (prev_ok && !post_ok)
+ return prev;
+
+ else if (!prev_ok && post_ok)
+ return post;
+
+ else if (prev_ok && post_ok)
+ {
+ if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+ return post;
+ else
+ return prev;
+ }
+ }
+ else if (post_ok)
+ return post;
+
+ return container_.end();
+}
+
template <typename T>
typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
@@ -485,12 +535,12 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st
}
template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
if (container_.empty())
return nullptr;
- Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance);
+ auto 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
@@ -502,7 +552,7 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
}
template <typename T>
-T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty())
@@ -523,7 +573,7 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
template <typename T>
-T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty())
@@ -543,7 +593,7 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
}
template <typename T>
-T Buffer<T>::selectFirst()
+T Buffer<T>::selectFirst() const
{
// There is no element
if (container_.empty())
@@ -554,7 +604,7 @@ T Buffer<T>::selectFirst()
}
template <typename T>
-T Buffer<T>::selectLast()
+T Buffer<T>::selectLast() const
{
// There is no element
if (container_.empty())
@@ -570,6 +620,12 @@ void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
container_.emplace(_time_stamp, _element);
}
+template <typename T>
+const std::map<TimeStamp,T>& Buffer<T>::getContainer() const
+{
+ return container_;
+}
+
template <typename T>
std::map<TimeStamp,T>& Buffer<T>::getContainer()
{
@@ -583,13 +639,13 @@ inline void Buffer<T>::clear()
}
template <typename T>
-inline bool Buffer<T>::empty()
+inline bool Buffer<T>::empty() const
{
return container_.empty();
}
template <typename T>
-inline SizeStd Buffer<T>::size(void)
+inline SizeStd Buffer<T>::size(void) const
{
return container_.size();
}
@@ -622,8 +678,8 @@ inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
template <typename T>
inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
- const TimeStamp& _time_stamp2,
- const double& _time_tolerance)
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
bool pass = time_diff <= _time_tolerance;
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index e9cf00b45a14bbf5ddbab51c8cbfd093d02cd985..20ed2330334aea821c7ca8bc6510f7438c555063 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -81,7 +81,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 = nullptr) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
@@ -90,7 +90,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
};
-inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 66f40e6e62e32e6312270fe994e7e38ceb426822..7c0cf9b2c776be5808c853c54c5d5c3f0cca5ff7 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -210,7 +210,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
/** \brief Finds the capture that contains the closest previous motion of _ts
* \return a pointer to the capture (if it exists) or a nullptr (otherwise)
*/
- CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts);
/** Set the origin of all motion for this processor
* \param _origin_frame the keyframe to be the origin
@@ -447,7 +448,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
* @param cap_prev : the first capture motion to be merged (input)
* @param cap_target : the second capture motion (modified)
*/
- void mergeCaptures(CaptureMotionConstPtr cap_prev,
+ void mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target);
protected:
@@ -487,13 +488,16 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
public:
- virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0;
+ virtual VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const = 0;
bool hasCalibration() const {return calib_size_ > 0;}
//getters
- CaptureMotionPtr getOrigin() const;
- CaptureMotionPtr getLast() const;
- CaptureMotionPtr getIncoming() const;
+ CaptureMotionConstPtr getOrigin() const;
+ CaptureMotionConstPtr getLast() const;
+ CaptureMotionConstPtr getIncoming() const;
+ CaptureMotionPtr getOrigin();
+ CaptureMotionPtr getLast();
+ CaptureMotionPtr getIncoming();
double getMaxTimeSpan() const;
double getMaxBuffLength() const;
@@ -609,17 +613,32 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
);
}
-inline CaptureMotionPtr ProcessorMotion::getOrigin() const
+inline CaptureMotionConstPtr ProcessorMotion::getOrigin() const
{
return origin_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getLast() const
+inline CaptureMotionConstPtr ProcessorMotion::getLast() const
{
return last_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getIncoming() const
+inline CaptureMotionConstPtr ProcessorMotion::getIncoming() const
+{
+ return incoming_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getOrigin()
+{
+ return origin_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getLast()
+{
+ return last_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index f3a50080d135b9c7d19444c2b7bed4f0237f6e2a..fea39c4d6808a7a798a83d7b8b99dfbb47abec9d 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -107,7 +107,7 @@ class ProcessorOdom2d : public ProcessorMotion
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
@@ -128,7 +128,7 @@ inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& del
return delta_corrected;
}
-inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBasePtr _capture) const
+inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 83baf370b1b536bc8e472cab19200abdc34d0314..02cdbd5ef4480ccd1a0ba28ab0db5c44886fa58c 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -129,7 +129,7 @@ class ProcessorOdom3d : public ProcessorMotion
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index f03ed4ea4d5e2ae46864479e65ad19c7d16aa9a9..b0683aba04b3e5d8fe041775199823436c728655 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -136,11 +136,14 @@ class ProcessorTracker : public ProcessorBase
ParamsProcessorTrackerPtr _params_tracker);
~ProcessorTracker() override;
- StateStructure getStateStructure() const;
+ const StateStructure& getStateStructure() const;
- virtual CaptureBasePtr getOrigin() const;
- virtual CaptureBasePtr getLast() const;
- virtual CaptureBasePtr getIncoming() const;
+ virtual CaptureBaseConstPtr getOrigin() const;
+ virtual CaptureBasePtr getOrigin();
+ virtual CaptureBaseConstPtr getLast() const;
+ virtual CaptureBasePtr getLast();
+ virtual CaptureBaseConstPtr getIncoming() const;
+ virtual CaptureBasePtr getIncoming();
protected:
/** \brief process an incoming capture
@@ -261,7 +264,8 @@ class ProcessorTracker : public ProcessorBase
public:
- FeatureBasePtrList& getNewFeaturesListLast();
+ FeatureBaseConstPtrList getNewFeaturesListLast() const;
+ FeatureBasePtrList getNewFeaturesListLast();
std::string print() const {
return this->params_tracker_->print();
@@ -286,7 +290,15 @@ class ProcessorTracker : public ProcessorBase
};
-inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListLast()
+inline FeatureBaseConstPtrList ProcessorTracker::getNewFeaturesListLast() const
+{
+ FeatureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : new_features_last_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FeatureBasePtrList ProcessorTracker::getNewFeaturesListLast()
{
return new_features_last_;
}
@@ -301,7 +313,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
-inline StateStructure ProcessorTracker::getStateStructure ( ) const
+inline const StateStructure& ProcessorTracker::getStateStructure ( ) const
{
return state_structure_;
}
@@ -311,17 +323,32 @@ inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
new_features_incoming_.push_back(_feature_ptr);
}
-inline CaptureBasePtr ProcessorTracker::getOrigin() const
+inline CaptureBaseConstPtr ProcessorTracker::getOrigin() const
+{
+ return origin_ptr_;
+}
+
+inline CaptureBasePtr ProcessorTracker::getOrigin()
{
return origin_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getLast() const
+inline CaptureBaseConstPtr ProcessorTracker::getLast() const
{
return last_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getIncoming() const
+inline CaptureBasePtr ProcessorTracker::getLast()
+{
+ return last_ptr_;
+}
+
+inline CaptureBaseConstPtr ProcessorTracker::getIncoming() const
+{
+ return incoming_ptr_;
+}
+
+inline CaptureBasePtr ProcessorTracker::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index e60a59f8233abb7fade714f20cbabc417a76e091..3b06c46381d24a2191ec271b7a741b121b26fbbb 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -45,9 +45,12 @@ using std::list;
using std::pair;
using std::shared_ptr;
-typedef map<TimeStamp, FeatureBasePtr> Track;
-typedef map<size_t, FeatureBasePtr > Snapshot;
-typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
+typedef map<TimeStamp, FeatureBasePtr> Track;
+typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
+typedef map<SizeStd, FeatureBasePtr > Snapshot;
+typedef map<SizeStd, FeatureBaseConstPtr > SnapshotConst;
+typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
+typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatchesConst; // matched feature pairs indexed by track_id
/** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
* This class implements the following data structure:
@@ -73,7 +76,7 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat
*
* Snapshot: capture-wise: a set of features tracked in a single Capture, indexed by track id:
*
- * map<size_t track_id, FeatureBasePtr f>
+ * map<SizeStd track_id, FeatureBasePtr f>
*
* The class makes sure that both accesses are consistent each time some addition or removal of features is performed.
*
@@ -99,33 +102,40 @@ class TrackMatrix
TrackMatrix();
virtual ~TrackMatrix();
- void newTrack (FeatureBasePtr _ftr);
- void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
- void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
- void remove (FeatureBasePtr _ftr);
- void remove (const SizeStd& _track_id);
- void remove (CaptureBasePtr _cap);
- SizeStd numTracks () const;
- SizeStd trackSize (const SizeStd& _track_id) const;
- Track track (const SizeStd& _track_id) const;
- Snapshot snapshot (CaptureBasePtr _capture) const;
- vector<FeatureBasePtr>
- trackAsVector(const SizeStd& _track_id) const;
- list<FeatureBasePtr>
- snapshotAsList(CaptureBasePtr _cap) const;
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const;
- FeatureBasePtr firstFeature(const SizeStd& _track_id) const;
- FeatureBasePtr lastFeature (const SizeStd& _track_id) const;
- FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap) const;
- CaptureBasePtr firstCapture(const SizeStd& _track_id) const;
-
- list<size_t> trackIds() const;
+ void newTrack (FeatureBasePtr _ftr);
+ void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
+ void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
+ void remove (FeatureBasePtr _ftr);
+ void remove (const SizeStd& _track_id);
+ void remove (CaptureBasePtr _cap);
+
+ SizeStd numTracks () const;
+ SizeStd trackSize (const SizeStd& _track_id) const;
+ TrackConst track (const SizeStd& _track_id) const;
+ Track track (const SizeStd& _track_id);
+ SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
+ Snapshot snapshot (CaptureBasePtr _capture);
+ vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
+ vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
+ FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
+ FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
+ TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
+ TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
+
+ FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr firstFeature(const SizeStd& _track_id);
+ FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
+ FeatureBasePtr lastFeature (const SizeStd& _track_id);
+ FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
+ FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
+ CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
+ CaptureBasePtr firstCapture(const SizeStd& _track_id);
+
+ list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const;
// tracks across captures that belong to keyframe
- Track trackAtKeyframes(size_t _track_id) const;
-
- const map<SizeStd, Track>& getTracks() const {return tracks_;}
- const map<CaptureBasePtr, Snapshot >& getSnapshots() const {return snapshots_;}
+ TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
+ Track trackAtKeyframes(const SizeStd& _track_id);
private:
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 0fdcac98b7c6c317b29f2a6dd718ae155db6e018..8e6c6db7c6657037e7df0eddeea9b683635e5b71 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -40,7 +40,6 @@ class StateBlock;
namespace wolf {
-
/*
* Macro for defining Autoconf sensor creator.
*
@@ -86,8 +85,6 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
} \
-
-
/** \brief base struct for intrinsic sensor parameters
*
* Derive from this struct to create structs of sensor intrinsic parameters.
@@ -120,12 +117,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+ CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
+
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:
@@ -177,8 +174,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
unsigned int id() const;
-
- HardwareBasePtr getHardware() const;
+ HardwareBaseConstPtr getHardware() const;
+ HardwareBasePtr getHardware();
private:
void setHardware(const HardwareBasePtr _hw_ptr);
@@ -186,40 +183,52 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void removeProcessor(ProcessorBasePtr _proc_ptr);
public:
- const ProcessorBasePtrList& getProcessorList() const;
+ ProcessorBaseConstPtrList getProcessorList() const;
+ ProcessorBasePtrList getProcessorList();
- CaptureBasePtr getLastCapture() const;
+ CaptureBaseConstPtr getLastCapture() const;
+ CaptureBasePtr getLastCapture();
void setLastCapture(CaptureBasePtr);
void updateLastCapture();
- CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts) const;
+ CaptureBaseConstPtr findLastCaptureBefore(const TimeStamp& _ts) const;
+ CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts);
bool process(const CaptureBasePtr capture_ptr);
// State blocks
using HasStateBlocks::addStateBlock;
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;
+ StateBlockConstPtr getStateBlockDynamic(const char& _key) const;
+ StateBlockPtr getStateBlockDynamic(const char& _key);
+ StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
+ StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts);
// Declare a state block as dynamic or static (with _dymanic = false)
void setStateBlockDynamic(const char& _key, bool _dynamic = true);
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, CaptureBaseConstPtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap);
+ bool isStateBlockInCapture(const char& _key, CaptureBaseConstPtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap);
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;
- StateBlockPtr getIntrinsic(const TimeStamp _ts) const;
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
- StateBlockPtr getIntrinsic() const;
+ StateBlockConstPtr getP(const TimeStamp _ts) const;
+ StateBlockPtr getP(const TimeStamp _ts);
+ StateBlockConstPtr getO(const TimeStamp _ts) const;
+ StateBlockPtr getO(const TimeStamp _ts);
+ StateBlockConstPtr getIntrinsic(const TimeStamp _ts) const;
+ StateBlockPtr getIntrinsic(const TimeStamp _ts);
+ StateBlockConstPtr getP() const;
+ StateBlockPtr getP();
+ StateBlockConstPtr getO() const;
+ StateBlockPtr getO();
+ StateBlockConstPtr getIntrinsic() const;
+ StateBlockPtr getIntrinsic();
protected:
- void removeStateBlocks();
- virtual void registerNewStateBlocks() const;
+ virtual void registerNewStateBlocks(ProblemPtr _problem) override;
public:
@@ -274,8 +283,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
void link(HardwareBasePtr);
template<typename classType, typename... T>
@@ -304,12 +313,25 @@ inline unsigned int SensorBase::id() const
return sensor_id_;
}
-inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
+inline ProcessorBaseConstPtrList SensorBase::getProcessorList() const
+{
+ ProcessorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : processor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline ProcessorBasePtrList SensorBase::getProcessorList()
{
return processor_list_;
}
-inline CaptureBasePtr SensorBase::getLastCapture() const
+inline CaptureBaseConstPtr SensorBase::getLastCapture() const
+{
+ return last_capture_;
+}
+
+inline CaptureBasePtr SensorBase::getLastCapture()
{
return last_capture_;
}
@@ -344,7 +366,12 @@ inline Eigen::MatrixXd SensorBase::getNoiseCov() const
return noise_cov_;
}
-inline HardwareBasePtr SensorBase::getHardware() const
+inline HardwareBaseConstPtr SensorBase::getHardware() const
+{
+ return hardware_ptr_.lock();
+}
+
+inline HardwareBasePtr SensorBase::getHardware()
{
return hardware_ptr_.lock();
}
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index f1bcd4519ae9fdb88fe182cf45270740cacc452f..ac82297f5e36b5130151bd1b0464a110264b5fe7 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -47,14 +47,18 @@ class HasStateBlocks
const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
- bool isInStructure(const char& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
+ bool isInStructure(const char& _sb_type) const { return structure_.find(_sb_type) != std::string::npos; }
- const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() const;
- std::vector<StateBlockPtr> getStateBlockVec() const;
+ const std::unordered_map<char, StateBlockConstPtr>& getStateBlockMap() const;
+ const std::unordered_map<char, StateBlockPtr>& getStateBlockMap();
+ std::vector<StateBlockConstPtr> getStateBlockVec() const;
+ std::vector<StateBlockPtr> getStateBlockVec();
// Some typical shortcuts -- not all should be coded here, see notes below.
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
+ StateBlockConstPtr getP() const;
+ StateBlockConstPtr getO() const;
+ StateBlockPtr getP();
+ StateBlockPtr getO();
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
virtual void fix();
@@ -64,11 +68,13 @@ class HasStateBlocks
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 char& _sb_type) const;
+ bool hasStateBlock(const StateBlockConstPtr& _sb) 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;
+ bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
+ StateBlockConstPtr getStateBlock(const char& _sb_type) const;
+ StateBlockPtr getStateBlock(const char& _sb_type);
+ std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
+ std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
@@ -79,8 +85,8 @@ class HasStateBlocks
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;
- void removeStateBlocks(ProblemPtr _problem);
+ virtual void registerNewStateBlocks(ProblemPtr _problem);
+ virtual void removeStateBlocks(ProblemPtr _problem);
// States
VectorComposite getState(const StateStructure& structure="") const;
@@ -103,6 +109,7 @@ class HasStateBlocks
private:
StateStructure structure_;
std::unordered_map<char, StateBlockPtr> state_block_map_;
+ std::unordered_map<char, StateBlockConstPtr> state_block_const_map_;
};
@@ -117,7 +124,8 @@ namespace wolf{
inline HasStateBlocks::HasStateBlocks() :
structure_(std::string("")),
- state_block_map_()
+ state_block_map_(),
+ state_block_const_map_()
{
//
}
@@ -127,14 +135,19 @@ inline HasStateBlocks::~HasStateBlocks()
//
}
-inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+inline const std::unordered_map<char, StateBlockConstPtr>& HasStateBlocks::getStateBlockMap() const
+{
+ return state_block_const_map_;
+}
+
+inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap()
{
return state_block_map_;
}
-inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
+inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
{
- std::vector<StateBlockPtr> sbv;
+ std::vector<StateBlockConstPtr> sbv;
for (auto& key : structure_)
{
sbv.push_back(getStateBlock(key));
@@ -142,20 +155,26 @@ 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 std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
+{
+ std::vector<StateBlockPtr> sbv;
+ for (auto& key : structure_)
+ {
+ sbv.push_back(getStateBlock(key));
+ }
+ return sbv;
+}
inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
{
return state_block_map_.erase(_sb_type);
+ return state_block_const_map_.erase(_sb_type);
}
template<typename SB, typename ... Args>
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.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_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)...);
addStateBlock(_sb_type, sb, _problem);
@@ -166,7 +185,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_typ
template<typename ... Args>
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.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_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)...);
addStateBlock(_sb_type, sb, _problem);
@@ -177,12 +196,22 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, Pro
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.");
+ assert ( state_block_map_.count(_sb_type) > 0 && state_block_const_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
state_block_map_.at(_sb_type) = _sb;
+ state_block_const_map_.at(_sb_type) = _sb;
return true; // success
}
-inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+{
+ auto it = state_block_const_map_.find(_sb_type);
+ if (it != state_block_const_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type)
{
auto it = state_block_map_.find(_sb_type);
if (it != state_block_map_.end())
@@ -191,12 +220,22 @@ inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) c
return nullptr;
}
-inline wolf::StateBlockPtr HasStateBlocks::getP() const
+inline wolf::StateBlockConstPtr HasStateBlocks::getP() const
{
return getStateBlock('P');
}
-inline wolf::StateBlockPtr HasStateBlocks::getO() const
+inline wolf::StateBlockPtr HasStateBlocks::getP()
+{
+ return getStateBlock('P');
+}
+
+inline wolf::StateBlockConstPtr HasStateBlocks::getO() const
+{
+ return getStateBlock('O');
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getO()
{
return getStateBlock('O');
}
@@ -358,7 +397,20 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co
return size;
}
-inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockConstPtr& _sb) const
+{
+ const auto& it = std::find_if(state_block_const_map_.begin(),
+ state_block_const_map_.end(),
+ [_sb](const std::pair<char, StateBlockConstPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb)
{
const auto& it = std::find_if(state_block_map_.begin(),
state_block_map_.end(),
@@ -371,18 +423,16 @@ inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::f
return it;
}
-inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
+inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
{
- const auto& it = this->find(_sb);
-
- return it != state_block_map_.end();
+ return this->find(_sb) != state_block_const_map_.end();
}
-inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, char& _key) const
+inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const
{
const auto& it = this->find(_sb);
- bool found = (it != state_block_map_.end());
+ bool found = (it != state_block_const_map_.end());
if (found)
{
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 46cce552ccbfe28ec30981b905b191ec2b9456cc..21b1ab5be7080da9334a979274d5f4513f9a23f0 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -35,47 +35,14 @@ class FrameBase;
#include "core/common/time_stamp.h"
#include "core/state_block/state_composite.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:
- FrameMap frame_map_;
+ FramePtrMap frame_map_;
protected:
StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
@@ -85,14 +52,22 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
~TrajectoryBase() override;
// Frames
- const FrameMap& getFrameMap() const;
- FrameBasePtr getLastFrame() const;
- FrameBasePtr getFirstFrame() const;
- FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
- TrajectoryIter begin() const;
- TrajectoryIter end() const;
- TrajectoryRevIter rbegin() const;
- TrajectoryRevIter rend() const;
+ SizeEigen size() const;
+ FrameConstPtrMap getFrameMap() const;
+ FramePtrMap getFrameMap();
+ FrameBaseConstPtr getLastFrame() const;
+ FrameBasePtr getLastFrame();
+ FrameBaseConstPtr getFirstFrame() const;
+ FrameBasePtr getFirstFrame();
+ FrameBaseConstPtr getNextFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
+ FrameBasePtr getNextFrame(FrameBasePtr, const unsigned int& i = 1);
+ FrameBaseConstPtr getPreviousFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
+ FrameBasePtr getPreviousFrame(FrameBasePtr, const unsigned int& i = 1);
+ TimeStamp closestTimeStampToTimeStamp(const TimeStamp& _ts) const;
+ FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
+ bool hasTimeStamp(const TimeStamp& _ts) const;
+ bool hasFrame(FrameBaseConstPtr _frame) const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -107,52 +82,80 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, TrajectoryBaseConstPtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
public:
// factors
- void getFactorList(FactorBasePtrList & _fac_list) const;
+ void getFactorList(FactorBaseConstPtrList & _fac_list) const;
+ void getFactorList(FactorBasePtrList & _fac_list);
};
-inline const FrameMap& TrajectoryBase::getFrameMap() const
+inline FrameConstPtrMap TrajectoryBase::getFrameMap() const
+{
+ FrameConstPtrMap map_const;
+ for (auto&& pair : frame_map_)
+ map_const[pair.first] = pair.second;
+ return map_const;
+}
+
+inline FramePtrMap TrajectoryBase::getFrameMap()
{
return frame_map_;
}
-inline FrameBasePtr TrajectoryBase::getFirstFrame() const
+inline FrameBaseConstPtr TrajectoryBase::getFirstFrame() const
+{
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.begin()->second;
+}
+
+inline FrameBasePtr TrajectoryBase::getFirstFrame()
{
- auto it = static_cast<TrajectoryIter>(frame_map_.begin());
- return *it;
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.begin()->second;
}
-inline TrajectoryIter TrajectoryBase::begin() const
+
+inline FrameBaseConstPtr TrajectoryBase::getLastFrame() const
{
- return static_cast<TrajectoryIter>(frame_map_.begin());
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.rbegin()->second;
}
-inline TrajectoryIter TrajectoryBase::end() const
+
+inline FrameBasePtr TrajectoryBase::getLastFrame()
{
- return static_cast<TrajectoryIter>(frame_map_.end());
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.rbegin()->second;
}
-inline TrajectoryRevIter TrajectoryBase::rbegin() const
+
+inline SizeEigen TrajectoryBase::size() const
{
- return static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+ return frame_map_.size();
}
-inline TrajectoryRevIter TrajectoryBase::rend() const
+
+inline bool TrajectoryBase::hasTimeStamp(const TimeStamp& _ts) const
{
- return static_cast<TrajectoryRevIter>(frame_map_.rend());
+ return frame_map_.count(_ts) != 0;
}
-inline FrameBasePtr TrajectoryBase::getLastFrame() const
+inline bool TrajectoryBase::hasFrame(FrameBaseConstPtr _frame) const
{
- // return last_key_frame_ptr_;
- auto last = this->rbegin();
- if(last != this->rend()) return *(this->rbegin());
- else return nullptr;
+ return std::find_if(frame_map_.begin(),
+ frame_map_.end(),
+ [&_frame](std::pair<TimeStamp,FrameBaseConstPtr> frm_it)
+ {
+ return frm_it.second == _frame;
+ }) != frame_map_.end();
}
+
} // namespace wolf
#endif
diff --git a/internal/config.h.in b/internal/config.h.in
index 7c0fe4756e443695c5450cfb46054e8b7cd107a7..8f0d62ec930e2db6e5f4bc874126b8e0787eb856 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -32,5 +32,6 @@
#cmakedefine _WOLF_TRACE
#define _WOLF_ROOT_DIR "${_WOLF_ROOT_DIR}"
+#define _WOLF_LIB_DIR "${_WOLF_LIB_DIR}"
#endif /* WOLF_INTERNAL_CONFIG_H_ */
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 8e91f7e75caedf348de594a725f0d0ee2c707d3d..e6d4a1e660bd0664f2fa1196c2ab558840664263 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -46,6 +46,7 @@ CaptureBase::CaptureBase(const std::string& _type,
{
if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
{
+ WOLF_ERROR_COND(not _p_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state P dynamic but provided _p_ptr is nullptr")
assert(_p_ptr && "Pointer to dynamic position params is null!");
addStateBlock('P', _p_ptr, nullptr);
}
@@ -54,6 +55,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
{
+ WOLF_ERROR_COND(not _o_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state O dynamic but provided _o_ptr is nullptr")
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
addStateBlock('O', _o_ptr, nullptr);
}
@@ -62,6 +64,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
{
+ WOLF_ERROR_COND(not _intr_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state I dynamic but provided _i_ptr is nullptr")
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
addStateBlock('I', _intr_ptr, nullptr);
}
@@ -126,8 +129,6 @@ bool CaptureBase::process()
return getSensor()->process(shared_from_this());
}
-
-
FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
@@ -140,14 +141,27 @@ void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
feature_list_.remove(_ft_ptr);
}
-FactorBasePtrList CaptureBase::getFactorList() const
+FactorBaseConstPtrList CaptureBase::getFactorList() const
+{
+ FactorBaseConstPtrList fac_list;
+ getFactorList(fac_list);
+ return fac_list;
+}
+
+FactorBasePtrList CaptureBase::getFactorList()
{
FactorBasePtrList fac_list;
getFactorList(fac_list);
return fac_list;
}
-void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const
+void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
+{
+ for (auto f_ptr : getFeatureList())
+ f_ptr->getFactorList(_fac_list);
+}
+
+void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto f_ptr : getFeatureList())
f_ptr->getFactorList(_fac_list);
@@ -164,23 +178,34 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ return std::find(feature_list_.begin(),
+ feature_list_.end(),
+ _feature) != feature_list_.end();
}
+bool CaptureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
+{
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
+}
+
+StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
+{
+ if (getSensor() and getSensor()->hasStateBlock(_key))
+ {
+ if (getSensor()->isStateBlockDynamic(_key))
+ return HasStateBlocks::getStateBlock(_key);
+ else
+ return getSensor()->getStateBlock(_key);
+ }
+ else // No sensor associated, or sensor without this state block: assume sensor params are here
+ return HasStateBlocks::getStateBlock(_key);
+}
-StateBlockPtr CaptureBase::getStateBlock(const char& _key) const
+StateBlockPtr CaptureBase::getStateBlock(const char& _key)
{
if (getSensor() and getSensor()->hasStateBlock(_key))
{
@@ -303,7 +328,7 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
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
+CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -346,12 +371,12 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
// check contrained_by
- for (const auto& cby : getConstrainedByList())
+ for (auto cby : getConstrainedByList())
{
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (const auto& Cow : cby->getCaptureOtherList())
+ for (auto Cow : cby->getCaptureOtherList())
_stream << " Cap" << Cow.lock()->id();
_stream << std::endl;
}
@@ -377,48 +402,52 @@ 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() << "\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);
+ // check frame
+ auto frm_cap = _cap_ptr->getFrame();
+ inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
+ << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
+ << " -X-> Frm" << id() << "\n";
+ auto frm_cap_list = frm_cap->getCaptureList();
+ auto frame_has_cap = std::find(frm_cap_list.begin(), frm_cap_list.end(), _cap_ptr);
+ log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
- for(auto f : getFeatureList())
- {
- inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
- << " ---> Ftr" << f->id() << " @ " << f
- << " -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 )
+
+ // check features
+ for(auto f : getFeatureList())
+ {
+ inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+ << " ---> Ftr" << f->id() << " @ " << f
+ << " -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 prc : getSensor()->getProcessorList())
{
- 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);
+ 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
+bool CaptureBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto sen_ptr = std::static_pointer_cast<CaptureBase>(_node_ptr);
+ auto sen_ptr = std::static_pointer_cast<const CaptureBase>(_node_ptr);
auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 3f925fd62a6acb93d7bcff2b3a45fbb28edc1bd7..e8f48dcd7b52cb8b5d2dc0ba95cc0c0f689de9ea 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -68,7 +68,7 @@ CaptureMotion::~CaptureMotion()
//
}
-bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance)
+bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) const
{
assert(_ts.ok() and this->time_stamp_.ok());
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index ab8753351aee84c5b4f290af171b370663060499..c83686ef4100dfa4cee61e576f99fa0759a1e94a 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -118,19 +118,21 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// 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())
- for (const auto& key : fr_ptr->getStructure())
+ for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
+ for (auto key : fr_pair.second->getStructure())
{
- const auto& sb = fr_ptr->getStateBlock(key);
+ const auto& sb = fr_pair.second->getStateBlock(key);
all_state_blocks.push_back(sb);
}
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- {
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
- all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
- }
+ for (auto key : l_ptr->getStructure())
+ {
+ const auto& sb = l_ptr->getStateBlock(key);
+ all_state_blocks.push_back(sb);
+ }
+
// double loop all against all (without repetitions)
for (unsigned int i = 0; i < all_state_blocks.size(); i++)
{
@@ -149,15 +151,15 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
{
// first create a vector containing all state blocks
- for(auto fr_ptr : *wolf_problem_->getTrajectory())
- for (const auto& key1 : wolf_problem_->getFrameStructure())
+ for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
+ for (auto key1 : fr_pair.second->getStructure())
{
- const auto& sb1 = fr_ptr->getStateBlock(key1);
+ const auto& sb1 = fr_pair.second->getStateBlock(key1);
assert(isStateBlockRegisteredDerived(sb1));
- for (const auto& key2 : wolf_problem_->getFrameStructure())
+ for (auto key2 : fr_pair.second->getStructure())
{
- const auto& sb2 = fr_ptr->getStateBlock(key2);
+ const auto& sb2 = fr_pair.second->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb1, sb2);
@@ -169,18 +171,23 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- for (auto sb : l_ptr->getUsedStateBlockVec())
+ for (auto key1 : l_ptr->getStructure())
{
- assert(isStateBlockRegisteredDerived(sb));
- for(auto sb2 : l_ptr->getUsedStateBlockVec())
+ const auto& sb1 = l_ptr->getStateBlock(key1);
+ assert(isStateBlockRegisteredDerived(sb1));
+
+ for (auto key2 : l_ptr->getStructure())
{
+ const auto& sb2 = l_ptr->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
- if (sb == sb2)
+
+ state_block_pairs.emplace_back(sb1, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+ if (sb1 == sb2)
break;
}
}
+
break;
}
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
@@ -203,35 +210,33 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
getAssociatedMemBlockPtr(last_key_frame->getO()));
// landmarks
- std::vector<StateBlockPtr> landmark_state_blocks;
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- {
- // load state blocks vector
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
-
- for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
+ for (auto key : l_ptr->getStructure())
{
- assert(isStateBlockRegisteredDerived(*state_it));
+ const auto& sb = l_ptr->getStateBlock(key);
+ assert(isStateBlockRegisteredDerived(sb));
// robot - landmark
- state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
- state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
+ state_block_pairs.emplace_back(last_key_frame->getP(), sb);
+ state_block_pairs.emplace_back(last_key_frame->getO(), sb);
double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
- getAssociatedMemBlockPtr((*state_it)));
+ getAssociatedMemBlockPtr(sb));
double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
- getAssociatedMemBlockPtr((*state_it)));
+ getAssociatedMemBlockPtr(sb));
// landmark marginal
- for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
+ for (auto key2 : l_ptr->getStructure())
{
- assert(isStateBlockRegisteredDerived(*next_state_it));
+ const auto& sb2 = l_ptr->getStateBlock(key2);
+ assert(isStateBlockRegisteredDerived(sb2));
- state_block_pairs.emplace_back(*state_it, *next_state_it);
- double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
- getAssociatedMemBlockPtr((*next_state_it)));
+ state_block_pairs.emplace_back(sb, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+ if (sb == sb2)
+ break;
}
}
- }
+
break;
}
case CovarianceBlocksToBeComputed::GAUSS:
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index bf7fb3d0256dde347713302ab1daee5353655bbc..b41394215b429155edd7d23afcc7d41916456c40 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -32,12 +32,49 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status,
+ bool _apply_loss_function,
+ FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAnalytic(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr){}
+
+FactorAnalytic::FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
+ FactorBase(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ _processor_ptr,
+ _apply_loss_function,
+ _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+ state_ptr_const_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
_state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
_state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
@@ -52,7 +89,12 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
resizeVectors();
}
-std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector() const
+std::vector<StateBlockConstPtr> FactorAnalytic::getStateBlockPtrVector() const
+{
+ return state_ptr_const_vector_;
+}
+
+std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector()
{
return state_ptr_vector_;
}
@@ -75,6 +117,7 @@ void FactorAnalytic::resizeVectors()
if (state_ptr_vector_.at(ii) == nullptr)
{
state_ptr_vector_.resize(ii);
+ state_ptr_const_vector_.resize(ii);
state_block_sizes_vector_.resize(ii);
break;
}
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 46151eba686f2fa4f12a1c734565575da362eeec..9ac1675b8b9ddc388f85b7fd6d8d7e34a274e65b 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -27,42 +27,6 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
-FactorBase::FactorBase(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- NodeBase("FACTOR", _tp),
- feature_ptr_(), // will be filled in link()
- factor_id_(++factor_id_count_),
- topology_(_top),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
- frame_other_list_(),
- capture_other_list_(),
- feature_other_list_(),
- landmark_other_list_(),
- processor_ptr_(_processor_ptr)
-{
- if (_frame_other_ptr)
- frame_other_list_.push_back(_frame_other_ptr);
- if (_capture_other_ptr)
- capture_other_list_.push_back(_capture_other_ptr);
- if (_feature_other_ptr)
- feature_other_list_.push_back(_feature_other_ptr);
- if (_landmark_other_ptr)
- landmark_other_list_.push_back(_landmark_other_ptr);
-
- assert(_feature_ptr && "null feature pointer when creating a factor");
- measurement_ = _feature_ptr->getMeasurement();
- measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
-}
-
FactorBase::FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
@@ -75,23 +39,23 @@ FactorBase::FactorBase(const std::string& _tp,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
- factor_id_(++factor_id_count_),
- topology_(_top),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
+ processor_ptr_(_processor_ptr),
frame_other_list_(),
capture_other_list_(),
feature_other_list_(),
landmark_other_list_(),
- processor_ptr_(_processor_ptr)
+ factor_id_(++factor_id_count_),
+ topology_(_top),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function)
{
- for (auto& Fo : _frame_other_list)
+ for (auto Fo : _frame_other_list)
frame_other_list_.push_back(Fo);
- for (auto& Co : _capture_other_list)
+ for (auto Co : _capture_other_list)
capture_other_list_.push_back(Co);
- for (auto& fo : _feature_other_list)
+ for (auto fo : _feature_other_list)
feature_other_list_.push_back(fo);
- for (auto& Lo : landmark_other_list_)
+ for (auto Lo : _landmark_other_list)
landmark_other_list_.push_back(Lo);
assert(_feature_ptr && "null feature pointer when creating a factor");
@@ -174,21 +138,42 @@ void FactorBase::remove(bool viral_remove_empty_parent)
}
}
-CaptureBasePtr FactorBase::getCapture() const
+CaptureBaseConstPtr FactorBase::getCapture() const
{
auto ftr = getFeature();
if (ftr != nullptr) return ftr->getCapture();
else return nullptr;
}
-FrameBasePtr FactorBase::getFrame() const
+CaptureBasePtr FactorBase::getCapture()
+{
+ auto ftr = getFeature();
+ if (ftr != nullptr) return ftr->getCapture();
+ else return nullptr;
+}
+
+FrameBaseConstPtr FactorBase::getFrame() const
{
auto cpt = getCapture();
if(cpt != nullptr) return cpt->getFrame();
else return nullptr;
}
-SensorBasePtr FactorBase::getSensor() const
+FrameBasePtr FactorBase::getFrame()
+{
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getFrame();
+ else return nullptr;
+}
+
+SensorBaseConstPtr FactorBase::getSensor() const
+{
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getSensor();
+ else return nullptr;
+}
+
+SensorBasePtr FactorBase::getSensor()
{
auto cpt = getCapture();
if(cpt != nullptr) return cpt->getSensor();
@@ -209,60 +194,56 @@ void FactorBase::setStatus(FactorStatus _status)
status_ = _status;
}
-bool FactorBase::hasFrameOther(const FrameBasePtr &_frm_other) const
+bool FactorBase::hasFrameOther(const FrameBaseConstPtr &_frm_other) const
{
- FrameBaseWConstIter frm_it = find_if(frame_other_list_.begin(),
- frame_other_list_.end(),
- [_frm_other](const FrameBaseWPtr &frm_ow)
- {
- return frm_ow.lock() == _frm_other;
- }
- );
+ auto frm_it = find_if(frame_other_list_.begin(),
+ frame_other_list_.end(),
+ [_frm_other](const FrameBaseConstWPtr &frm_ow)
+ {
+ return frm_ow.lock() == _frm_other;
+ });
if (frm_it != frame_other_list_.end())
return true;
return false;
}
-bool FactorBase::hasCaptureOther(const CaptureBasePtr &_cap_other) const
+bool FactorBase::hasCaptureOther(const CaptureBaseConstPtr &_cap_other) const
{
- CaptureBaseWConstIter cap_it = find_if(capture_other_list_.begin(),
- capture_other_list_.end(),
- [_cap_other](const CaptureBaseWPtr &cap_ow)
- {
- return cap_ow.lock() == _cap_other;
- }
- );
+ auto cap_it = find_if(capture_other_list_.begin(),
+ capture_other_list_.end(),
+ [_cap_other](const CaptureBaseConstWPtr &cap_ow)
+ {
+ return cap_ow.lock() == _cap_other;
+ });
if (cap_it != capture_other_list_.end())
return true;
return false;
}
-bool FactorBase::hasFeatureOther(const FeatureBasePtr &_ftr_other) const
+bool FactorBase::hasFeatureOther(const FeatureBaseConstPtr &_ftr_other) const
{
- FeatureBaseWConstIter ftr_it = find_if(feature_other_list_.begin(),
- feature_other_list_.end(),
- [_ftr_other](const FeatureBaseWPtr &ftr_ow)
- {
- return ftr_ow.lock() == _ftr_other;
- }
- );
+ auto ftr_it = find_if(feature_other_list_.begin(),
+ feature_other_list_.end(),
+ [_ftr_other](const FeatureBaseConstWPtr &ftr_ow)
+ {
+ return ftr_ow.lock() == _ftr_other;
+ });
if (ftr_it != feature_other_list_.end())
return true;
return false;
}
-bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const
+bool FactorBase::hasLandmarkOther(const LandmarkBaseConstPtr &_lmk_other) const
{
- LandmarkBaseWConstIter lmk_it = find_if(landmark_other_list_.begin(),
- landmark_other_list_.end(),
- [_lmk_other](const LandmarkBaseWPtr &lmk_ow)
- {
- return lmk_ow.lock() == _lmk_other;
- }
- );
+ auto lmk_it = find_if(landmark_other_list_.begin(),
+ landmark_other_list_.end(),
+ [_lmk_other](const LandmarkBaseConstWPtr &lmk_ow)
+ {
+ return lmk_ow.lock() == _lmk_other;
+ });
if (lmk_it != landmark_other_list_.end())
return true;
@@ -335,16 +316,16 @@ void FactorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
&& getLandmarkOtherList().empty())
_stream << " Abs";
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
if (!Fow.expired())
_stream << " Frm" << Fow.lock()->id();
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
if (!Cow.expired())
_stream << " Cap" << Cow.lock()->id();
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
if (!fow.expired())
_stream << " Ftr" << fow.lock()->id();
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
if (!Low.expired())
_stream << " Lmk" << Low.lock()->id();
_stream << std::endl;
@@ -355,7 +336,7 @@ void FactorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
}
-CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -373,7 +354,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find constrained_by pointer in constrained frame
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
{
@@ -396,7 +377,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
if (_verbose && !getFrameOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained capture
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
@@ -420,7 +401,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
if (_verbose && !getCaptureOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained feature
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
{
if (!fow.expired())
{
@@ -443,7 +424,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
_stream << ")";
// find constrained_by pointer in constrained landmark
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
{
if (Low.expired())
{
@@ -468,6 +449,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
_stream << ")";
if (_verbose)
_stream << std::endl;
+
//Check Problem and feature ptrs
if (_verbose)
{
@@ -475,6 +457,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
_stream << _tabs << " " << "-> Ftr" << getFeature()->id() << " @ " << getFeature().get() << std::endl;
}
auto ftr_ptr = getFeature();
+
// check problem and feature pointers
inconsistency_explanation << "The factor " << id() << " @ " << _fac_ptr.get() << " problem ptr " << getProblem().get()
<< " is different from Feature's problem ptr " << ftr_ptr->getProblem().get() << "\n";
@@ -485,13 +468,13 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
<< " ---> Ftr" << ftr_ptr->id() << " @ " << ftr_ptr
<< " -X-> Fac" << id();
auto ftr_fac_list = ftr_ptr->getFactorList();
- auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBasePtr fac){ return fac == _fac_ptr;});
+ auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBaseConstPtr fac){ return fac == _fac_ptr;});
log.assertTrue(ftr_has_fac!= ftr_fac_list.end(), inconsistency_explanation);
// find state block pointers in all constrained nodes
- SensorBasePtr S = getSensor(); // get own sensor to check sb
- FrameBasePtr F = getFrame();
- CaptureBasePtr C = getCapture();
+ auto S = getSensor(); // get own sensor to check sb
+ auto F = getFrame();
+ auto C = getCapture();
for (auto sb : getStateBlockPtrVector())
{
@@ -537,7 +520,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
// find in constrained Frame
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
{
@@ -558,7 +541,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find in constrained Capture
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
@@ -570,7 +553,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find in constrained Feature
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
{
if (!fow.expired())
{
@@ -596,7 +579,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find in constrained landmark
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
{
if (!Low.expired())
{
@@ -626,9 +609,9 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
return log;
}
-bool FactorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool FactorBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto fac_ptr = std::static_pointer_cast<FactorBase>(_node_ptr);
+ auto fac_ptr = std::static_pointer_cast<const FactorBase>(_node_ptr);
auto local_log = localCheck(_verbose, fac_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 5e3516cccc2f6095e2c4c8905ceb90649cfeda9e..fcef79178934508f840a2a2d51b918b85827c1af 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -102,7 +102,12 @@ void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
factor_list_.remove(_co_ptr);
}
-FrameBasePtr FeatureBase::getFrame() const
+FrameBaseConstPtr FeatureBase::getFrame() const
+{
+ return capture_ptr_.lock()->getFrame();
+}
+
+FrameBasePtr FeatureBase::getFrame()
{
return capture_ptr_.lock()->getFrame();
}
@@ -118,27 +123,27 @@ void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool FeatureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool FeatureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
-const FactorBasePtrList& FeatureBase::getFactorList() const
+bool FeatureBase::hasFactor(FactorBaseConstPtr _factor) const
{
- return factor_list_;
+ return std::find(factor_list_.begin(),
+ factor_list_.end(),
+ _factor) != factor_list_.end();
}
-void FeatureBase::getFactorList(FactorBasePtrList & _fac_list) const
+void FeatureBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
+{
+ // FIXME
+ _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
+}
+
+void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
{
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
@@ -222,7 +227,7 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
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
+CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -248,7 +253,7 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (const auto& fow : cby->getFeatureOtherList())
+ for (auto fow : cby->getFeatureOtherList())
_stream << " Ftr" << fow.lock()->id();
_stream << std::endl;
}
@@ -257,17 +262,18 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
<< " not found among constrained-by factors\n";
log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
-
}
+ // Check capture
auto cap_ftr = _ftr_ptr->getCapture();
inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
<< " ---> Cap" << cap_ftr->id() << " @ " << cap_ftr
<< " -X-> Ftr" << id();
auto cap_ftr_list = cap_ftr->getFeatureList();
- auto frame_has_cap = std::find_if(cap_ftr_list.begin(), cap_ftr_list.end(), [&_ftr_ptr](FeatureBasePtr ftr){ return ftr == _ftr_ptr;});
+ auto frame_has_cap = std::find(cap_ftr_list.begin(), cap_ftr_list.end(), _ftr_ptr);
log.assertTrue(frame_has_cap != cap_ftr_list.end(), inconsistency_explanation);
+ // Check factors
for(auto fac : getFactorList())
{
inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
@@ -275,11 +281,12 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
<< " -X-> Ftr" << id();
log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
}
+
return log;
}
-bool FeatureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool FeatureBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto ftr_ptr = std::static_pointer_cast<FeatureBase>(_node_ptr);
+ auto ftr_ptr = std::static_pointer_cast<const FeatureBase>(_node_ptr);
auto local_log = localCheck(_verbose, ftr_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 951d544ea9afb33dbc412128f8cda51ac7d1cdb9..aae21b6eb2fa9d5552e8212259299b71e0e0c440 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -156,38 +156,38 @@ bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
-FrameBasePtr FrameBase::getPreviousFrame() const
+FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
+ return getTrajectory()->getPreviousFrame(shared_from_this(), i);
+}
- if (current_frame_it == getTrajectory()->getFrameMap().begin())
- return nullptr;
+FrameBasePtr FrameBase::getPreviousFrame(const unsigned int& i)
+{
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return std::prev(current_frame_it)->second;
+ return getTrajectory()->getPreviousFrame(shared_from_this(), i);
}
-FrameBasePtr FrameBase::getNextFrame() const
+FrameBaseConstPtr FrameBase::getNextFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
+ return getTrajectory()->getNextFrame(shared_from_this(), i);
+}
- if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
- return nullptr;
+FrameBasePtr FrameBase::getNextFrame(const unsigned int& i)
+{
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return std::next(current_frame_it)->second;
+ return getTrajectory()->getNextFrame(shared_from_this(), i);
}
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
{
WOLF_WARN_COND(getCaptureOf(_capt_ptr->getSensor()) != nullptr, "FrameBase::addCapture adding new capture ", _capt_ptr->id(), " in a frame with another capture of the same sensor: ", getCaptureOf(_capt_ptr->getSensor())->id());
- capture_list_.push_back(_capt_ptr);
+ capture_list_.push_back(_capt_ptr);
return _capt_ptr;
}
@@ -196,88 +196,177 @@ void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
capture_list_.remove(_capt_ptr);
}
-CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type) const
+CaptureBaseConstPtr FrameBase::getCaptureOfType(const std::string& type) const
{
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
return capture_ptr;
return nullptr;
}
-CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type) const
+CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type)
+{
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ return capture_ptr;
+ return nullptr;
+}
+
+CaptureBaseConstPtrList FrameBase::getCapturesOfType(const std::string& type) const
+{
+ CaptureBaseConstPtrList captures;
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
+CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type)
{
CaptureBasePtrList captures;
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
captures.push_back(capture_ptr);
return captures;
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const
+{
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
+ return capture_ptr;
+ return nullptr;
+}
+
+CaptureBasePtr FrameBase::getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type)
{
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
return capture_ptr;
return nullptr;
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr) const
{
if (_sensor_ptr)
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
return capture_ptr;
return nullptr;
}
-CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
+CaptureBasePtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr)
+{
+ if (_sensor_ptr)
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getSensor() == _sensor_ptr)
+ return capture_ptr;
+ return nullptr;
+}
+
+CaptureBaseConstPtrList FrameBase::getCapturesOf(SensorBaseConstPtr _sensor_ptr) const
+{
+ CaptureBaseConstPtrList captures;
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getSensor() == _sensor_ptr)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
+CaptureBasePtrList FrameBase::getCapturesOf(SensorBasePtr _sensor_ptr)
{
CaptureBasePtrList captures;
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
captures.push_back(capture_ptr);
return captures;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
+FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const
{
- for (const FactorBasePtr& factor_ptr : getConstrainedByList())
+ for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
- for (const FactorBasePtr& factor_ptr : getFactorList())
+ for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
return nullptr;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
+FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type)
{
- for (const FactorBasePtr& factor_ptr : getConstrainedByList())
+ for (auto factor_ptr : getConstrainedByList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr) const
+{
+ for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
- for (const FactorBasePtr& factor_ptr : getFactorList())
+ for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
return nullptr;
}
-FactorBasePtrList FrameBase::getFactorList() const
+FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr)
+{
+ for (auto factor_ptr : getConstrainedByList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+FactorBaseConstPtrList FrameBase::getFactorList() const
+{
+ FactorBaseConstPtrList fac_list;
+ getFactorList(fac_list);
+ return fac_list;
+}
+
+FactorBasePtrList FrameBase::getFactorList()
{
FactorBasePtrList fac_list;
getFactorList(fac_list);
return fac_list;
}
-void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const
+void FrameBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
+{
+ for (auto c_ptr : getCaptureList())
+ c_ptr->getFactorList(_fac_list);
+}
+
+void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto c_ptr : getCaptureList())
c_ptr->getFactorList(_fac_list);
}
+bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
+{
+ return std::find(capture_list_.begin(),
+ capture_list_.end(),
+ _capture) != capture_list_.end();
+}
+
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
@@ -289,19 +378,11 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool FrameBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool FrameBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
@@ -360,7 +441,7 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo
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
+CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -371,7 +452,8 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
_stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
}
- for (const auto &pair: getStateBlockMap()) {
+ for (const auto &pair: getStateBlockMap())
+ {
auto sb = pair.second;
@@ -405,15 +487,14 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (const auto& Fow : cby->getFrameOtherList())
+ for (auto Fow : cby->getFrameOtherList())
_stream << " Frm" << Fow.lock()->id() << std::endl;
// check constrained_by pointer to this frame
inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
<< " not found among constrained-by factors\n";
- auto F = std::static_pointer_cast<FrameBase>(_frm_ptr);
- log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
+ log.assertTrue((cby->hasFrameOther(_frm_ptr)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
@@ -430,13 +511,14 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
}
}
+ // Trajectory
auto trj_ptr = getTrajectory();
inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
<< " ---> Trj" << " @ " << trj_ptr
<< " -X-> Frm" << id();
- 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);
+ log.assertTrue(trj_ptr->hasFrame(_frm_ptr), inconsistency_explanation);
+ // Captures
for(auto C : getCaptureList())
{
inconsistency_explanation << "Frm " << id() << " @ " << _frm_ptr
@@ -444,15 +526,17 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
<< " -X-> Frm" << id();
log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
}
+
return log;
}
-bool FrameBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool FrameBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto frm_ptr = std::static_pointer_cast<FrameBase>(_node_ptr);
+ auto frm_ptr = std::static_pointer_cast<const FrameBase>(_node_ptr);
auto local_log = localCheck(_verbose, frm_ptr, _stream, _tabs);
_log.compose(local_log);
- for(auto C : getCaptureList()) C->check(_log, C, _verbose, _stream, _tabs + " ");
+ for(auto C : getCaptureList())
+ C->check(_log, C, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 9db6b0c373e8f49838ea0d354a0694bffba0da04..470057fabef0c9f868d26cc28f88e12a20cc595b 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -41,6 +41,36 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
return _sensor_ptr;
}
+SensorBaseConstPtr HardwareBase::getSensor(const std::string& _sensor_name) const
+{
+ auto sen_it = std::find_if(sensor_list_.begin(),
+ sensor_list_.end(),
+ [&](SensorBaseConstPtr sb)
+ {
+ return sb->getName() == _sensor_name;
+ }); // lambda function for the find_if
+
+ if (sen_it == sensor_list_.end())
+ return nullptr;
+
+ return (*sen_it);
+}
+
+SensorBasePtr HardwareBase::getSensor(const std::string& _sensor_name)
+{
+ auto sen_it = std::find_if(sensor_list_.begin(),
+ sensor_list_.end(),
+ [&](SensorBasePtr sb)
+ {
+ return sb->getName() == _sensor_name;
+ }); // lambda function for the find_if
+
+ if (sen_it == sensor_list_.end())
+ return nullptr;
+
+ return (*sen_it);
+}
+
void HardwareBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "") << std::endl;
@@ -55,7 +85,7 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
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
+CheckLog HardwareBase::localCheck(bool _verbose, HardwareBaseConstPtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -68,11 +98,12 @@ CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::
inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
<< "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
log.assertTrue((_hwd_ptr->getProblem()->getHardware().get() == _hwd_ptr.get()), inconsistency_explanation);
+
return log;
}
-bool HardwareBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool HardwareBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto hwd_ptr = std::static_pointer_cast<HardwareBase>(_node_ptr);
+ auto hwd_ptr = std::static_pointer_cast<const HardwareBase>(_node_ptr);
auto local_log = localCheck(_verbose, hwd_ptr, _stream, _tabs);
_log.compose(local_log);
for (auto S : getSensorList())
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 09d317544ff4c0ae6ce7b6a24e2d3b2acf2cfe93..e7acd4f0d90ef0883eea1af949928b0458e25a44 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -37,7 +37,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
NodeBase("LANDMARK", _type),
HasStateBlocks(""),
map_ptr_(),
- state_block_vec_(0), // Resize in derived constructors if needed.
+ //state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
{
if (_p_ptr)
@@ -84,25 +84,45 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
-{
- std::vector<StateBlockPtr> used_state_block_vec(0);
-
- // normal state blocks in {P,O,V,W}
- for (const auto& key : getStructure())
- {
- const auto& sbp = getStateBlock(key);
- if (sbp)
- used_state_block_vec.push_back(sbp);
- }
-
- // other state blocks in a vector
- for (auto sbp : state_block_vec_)
- if (sbp)
- used_state_block_vec.push_back(sbp);
-
- return used_state_block_vec;
-}
+// std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
+// {
+// std::vector<StateBlockConstPtr> used_state_block_vec(0);
+
+// // normal state blocks in {P,O,V,W}
+// for (auto key : getStructure())
+// {
+// const auto sbp = getStateBlock(key);
+// if (sbp)
+// used_state_block_vec.push_back(sbp);
+// }
+
+// // // other state blocks in a vector
+// // for (auto sbp : state_block_vec_)
+// // if (sbp)
+// // used_state_block_vec.push_back(sbp);
+
+// return used_state_block_vec;
+// }
+
+// std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec()
+// {
+// std::vector<StateBlockPtr> used_state_block_vec(0);
+
+// // normal state blocks in {P,O,V,W}
+// for (auto key : getStructure())
+// {
+// auto sbp = getStateBlock(key);
+// if (sbp)
+// used_state_block_vec.push_back(sbp);
+// }
+
+// // // other state blocks in a vector
+// // for (auto sbp : state_block_vec_)
+// // if (sbp)
+// // used_state_block_vec.push_back(sbp);
+
+// return used_state_block_vec;
+// }
bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
{
@@ -164,19 +184,11 @@ void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool LandmarkBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -224,7 +236,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
return lmk;
}
-CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -234,7 +246,7 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
_stream << _tabs << "Lmk" << id() << " @ " << _lmk_ptr.get() << std::endl;
_stream << _tabs << " -> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " -> Map @ " << getMap().get() << std::endl;
- for (const auto& pair : getStateBlockMap())
+ for (auto pair : getStateBlockMap())
{
auto sb = pair.second;
_stream << _tabs << " " << pair.first << " sb @ " << sb.get();
@@ -256,24 +268,24 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
+ // check constrained-by factors
for (auto cby : getConstrainedByList())
{
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
- for (const auto& Low : cby->getLandmarkOtherList())
+ for (auto Low : cby->getLandmarkOtherList())
{
if (!Low.expired())
{
- const auto& Lo = Low.lock();
+ auto Lo = Low.lock();
_stream << " Lmk" << Lo->id();
}
}
_stream << std::endl;
}
- // check constrained-by factors
inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
<< " not found among constrained-by factors\n";
log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
@@ -291,18 +303,21 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
}
}
+ // check map
inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
<< " ---> Map" << map_ptr
<< " -X-> Lmk" << id();
auto map_lmk_list = map_ptr->getLandmarkList();
- auto map_has_lmk = std::find_if(map_lmk_list.begin(), map_lmk_list.end(), [&_lmk_ptr](LandmarkBasePtr lmk){ return lmk == _lmk_ptr;});
+ auto map_has_lmk = std::find(map_lmk_list.begin(),
+ map_lmk_list.end(),
+ _lmk_ptr);
log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
return log;
}
-bool LandmarkBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto lmk_ptr = std::static_pointer_cast<LandmarkBase>(_node_ptr);
+ auto lmk_ptr = std::static_pointer_cast<const LandmarkBase>(_node_ptr);
auto local_log = localCheck(_verbose, lmk_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 8194c87d95dc05bb20b5f736afdb704a11a75560..66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -23,7 +23,7 @@
// wolf
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-//#include "core/common/factory.h"
+#include "core/common/factory.h"
// YAML
#include <yaml-cpp/yaml.h>
@@ -121,6 +121,7 @@ void MapBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state
{
_stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "") << std::endl;
}
+
void MapBase::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);
@@ -130,7 +131,7 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block
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
+CheckLog MapBase::localCheck(bool _verbose, MapBaseConstPtr _map_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -144,15 +145,17 @@ CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _
log.assertTrue((_map_ptr->getProblem()->getMap().get() == _map_ptr.get()), inconsistency_explanation);
return log;
}
-bool MapBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+
+bool MapBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto map_ptr = std::static_pointer_cast<MapBase>(_node_ptr);
+ auto map_ptr = std::static_pointer_cast<const MapBase>(_node_ptr);
auto local_log = localCheck(_verbose, map_ptr, _stream, _tabs);
_log.compose(local_log);
for (auto L : getLandmarkList())
L->check(_log, L, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
+
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d80dafb51c2d4e2dfd772c19fa0786e4f5c173b7..6fe2afb34530e550d8a361e56bcaae2c07938349 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -21,42 +21,17 @@
//--------LICENSE_END--------
// wolf includes
#include "core/problem/problem.h"
-#include "core/hardware/hardware_base.h"
-#include "core/trajectory/trajectory_base.h"
-#include "core/map/map_base.h"
#include "core/map/factory_map.h"
-#include "core/sensor/sensor_base.h"
#include "core/sensor/factory_sensor.h"
#include "core/processor/factory_processor.h"
-#include "core/processor/processor_motion.h"
-#include "core/processor/processor_tracker.h"
-#include "core/capture/capture_pose.h"
#include "core/capture/capture_void.h"
-#include "core/factor/factor_base.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
-#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_base.h"
-
-#include "core/utils/params_server.h"
#include "core/utils/loader.h"
-#include "core/utils/check_log.h"
-#include "core/math/covariance.h"
-#include "core/state_block/factory_state_block.h"
-
-// IRI libs includes
-
-// C++ includes
-#include <algorithm>
-#include <map>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-#include <vector>
-#include <unordered_set>
namespace wolf
@@ -125,8 +100,11 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Load plugins
auto loaders = std::vector<std::shared_ptr<Loader>>();
- std::string plugins_path;
- try
+ std::string plugins_path = _WOLF_LIB_DIR;
+ if (plugins_path.back() != '/'){
+ plugins_path += '/'; // only works for UNIX systems
+ }
+ /*try
{
plugins_path = _server.getParam<std::string>("plugins_path");
}
@@ -135,7 +113,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
WOLF_WARN(e.what());
WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
plugins_path="/usr/local/lib/";
- }
+ }*/
for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins"))
{
if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
@@ -335,22 +313,17 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return prc_ptr;
}
-SensorBasePtr Problem::findSensor(const std::string& _sensor_name) const
+SensorBaseConstPtr Problem::findSensor(const std::string& _sensor_name) const
{
- auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
- getHardware()->getSensorList().end(),
- [&](SensorBasePtr sb)
- {
- return sb->getName() == _sensor_name;
- }); // lambda function for the find_if
-
- if (sen_it == getHardware()->getSensorList().end())
- return nullptr;
+ return getHardware()->getSensor(_sensor_name);
+}
- return (*sen_it);
+SensorBasePtr Problem::findSensor(const std::string& _sensor_name)
+{
+ return getHardware()->getSensor(_sensor_name);
}
-ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name) const
+ProcessorBaseConstPtr Problem::findProcessor(const std::string& _processor_name) const
{
for (const auto& sensor : getHardware()->getSensorList())
for (const auto& processor : sensor->getProcessorList())
@@ -359,16 +332,24 @@ ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name) cons
return nullptr;
}
+ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name)
+{
+ for (const auto& sensor : getHardware()->getSensorList())
+ for (const auto& processor : sensor->getProcessorList())
+ if (processor->getName() == _processor_name)
+ return processor;
+ return nullptr;
+}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state)
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state)
{
VectorComposite state;
SizeEigen index = 0;
SizeEigen size = 0;
- for (const auto& key : _frame_structure)
+ for (auto key : _frame_structure)
{
if (key == 'O')
{
@@ -422,9 +403,9 @@ 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)
+ for (auto pair_key_vec : _frame_state)
{
- const auto& key = pair_key_vec.first;
+ auto key = pair_key_vec.first;
if (frame_structure_.find(key) == std::string::npos) // not found
frame_structure_ += std::string(1,key);
}
@@ -455,7 +436,7 @@ TimeStamp Problem::getTimeStamp ( ) const
{
TimeStamp ts = TimeStamp::Invalid();
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
if (prc_pair.second->getTimeStamp().ok())
if ( (not ts.ok() ) or prc_pair.second->getTimeStamp() > ts)
ts = prc_pair.second->getTimeStamp();
@@ -463,7 +444,7 @@ TimeStamp Problem::getTimeStamp ( ) const
if (not ts.ok())
{
- const auto& last_kf = trajectory_ptr_->getLastFrame();
+ auto last_kf = trajectory_ptr_->getLastFrame();
if (last_kf)
ts = last_kf->getTimeStamp(); // Use last estimated frame's state
@@ -483,12 +464,12 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
{
- const auto& prc_state = prc_pair.second->getState(structure);
+ auto prc_state = prc_pair.second->getState(structure);
// transfer processor vector blocks to problem state
- for (const auto& pair_key_vec : prc_state)
+ for (auto pair_key_vec : prc_state)
{
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
{
@@ -507,7 +488,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state_last;
- const auto& last_kf = trajectory_ptr_->getLastFrame();
+ auto last_kf = trajectory_ptr_->getLastFrame();
if (last_kf)
state_last = last_kf->getState(structure);
@@ -515,7 +496,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
else if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
- for (const auto& key : structure)
+ for (auto key : structure)
{
if (state.count(key) == 0)
{
@@ -539,12 +520,12 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
{
- const auto& prc_state = prc_pair.second->getState(_ts, structure);
+ auto prc_state = prc_pair.second->getState(_ts, structure);
// transfer processor vector blocks to problem state
- for (const auto& pair_key_vec : prc_state)
+ for (auto pair_key_vec : prc_state)
{
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
state.insert(pair_key_vec);
@@ -561,7 +542,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state_last;
- const auto& last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);
+ auto last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);
if (last_kf)
state_last = last_kf->getState(structure);
@@ -569,7 +550,7 @@ 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& key : structure)
+ for (auto key : structure)
{
if (state.count(key) == 0)
{
@@ -593,12 +574,12 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
VectorComposite odom_state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
{
- const auto& prc_state = prc_pair.second->getOdometry();
+ auto prc_state = prc_pair.second->getOdometry();
// transfer processor vector blocks to problem state
- for (const auto& pair_key_vec : prc_state)
+ for (auto pair_key_vec : prc_state)
{
if (odom_state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
odom_state.insert(pair_key_vec);
@@ -612,7 +593,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
- for (const auto& key : structure)
+ for (auto key : structure)
{
if (odom_state.count(key) == 0)
{
@@ -641,7 +622,7 @@ const StateStructure& Problem::getFrameStructure() const
void Problem::appendToStructure(const StateStructure& _structure)
{
- for (const auto& key : _structure)
+ for (auto key : _structure)
if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
frame_structure_ += key;
}
@@ -652,7 +633,6 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
tree_manager_->setProblem(nullptr);
tree_manager_ = _gm;
tree_manager_->setProblem(shared_from_this());
-
}
void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
@@ -687,7 +667,7 @@ void Problem::removeMotionProvider(MotionProviderPtr proc)
// rebuild frame structure with remaining motion processors
frame_structure_.clear();
- for (const auto& pm : motion_provider_map_)
+ for (auto pm : motion_provider_map_)
appendToStructure(pm.second->getStateStructure());
}
@@ -696,7 +676,7 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
VectorComposite state;
- for (const auto& key : structure)
+ for (auto key : structure)
{
VectorXd vec;
if (key == 'O')
@@ -862,7 +842,10 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row,
+bool Problem::getCovarianceBlock(StateBlockConstPtr _state1,
+ StateBlockConstPtr _state2,
+ Eigen::MatrixXd& _cov,
+ const int _row,
const int _col) const
{
//std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
@@ -880,12 +863,12 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
std::lock_guard<std::mutex> lock(mut_covariances_);
- if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
+ if (covariances_.find(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state1, _state2)) != covariances_.end())
+ _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
+ covariances_.at(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state1, _state2));
+ else if (covariances_.find(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state2, _state1)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2));
- else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
- _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)).transpose();
+ covariances_.at(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state2, _state1)).transpose();
else
{
WOLF_DEBUG("Could not find the requested covariance block.");
@@ -895,18 +878,18 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
return true;
}
-bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
+bool Problem::getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
{
std::lock_guard<std::mutex> lock(mut_covariances_);
- // fill covariance
+ // fill _cov
for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
{
- StateBlockPtr sb1 = it1->first;
- StateBlockPtr sb2 = it2->first;
- std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2);
- std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1);
+ auto sb1 = it1->first;
+ auto sb2 = it2->first;
+ std::pair<StateBlockConstPtr, StateBlockConstPtr> pair_12(sb1, sb2);
+ std::pair<StateBlockConstPtr, StateBlockConstPtr> pair_21(sb2, sb1);
// search st1 & st2
if (covariances_.find(pair_12) != covariances_.end())
@@ -936,7 +919,7 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
return true;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
+bool Problem::getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
{
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
@@ -946,15 +929,15 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
bool success(true);
// resizing
- SizeEigen sz = _frame_ptr->getLocalSize();
+ SizeEigen sz = _frame_ptr->getLocalSize();
_covariance.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
- for (const auto& sb_i : _frame_ptr->getStateBlockVec())
+ for (auto sb_i : _frame_ptr->getStateBlockVec())
{
j = 0;
- for (const auto& sb_j : _frame_ptr->getStateBlockVec())
+ for (auto sb_j : _frame_ptr->getStateBlockVec())
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
@@ -967,7 +950,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const
{
- FrameBasePtr frm = getLastFrame();
+ auto frm = getLastFrame();
return getFrameCovariance(frm, cov);
}
@@ -976,15 +959,15 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
bool success(true);
// resizing
- SizeEigen sz = _landmark_ptr->getLocalSize();
+ SizeEigen sz = _landmark_ptr->getLocalSize();
_covariance.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
- for (const auto& sb_i : _landmark_ptr->getStateBlockVec())
+ for (auto sb_i : _landmark_ptr->getStateBlockVec())
{
j = 0;
- for (const auto& sb_j : _landmark_ptr->getStateBlockVec())
+ for (auto sb_j : _landmark_ptr->getStateBlockVec())
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
@@ -995,7 +978,12 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
return success;
}
-MapBasePtr Problem::getMap() const
+MapBaseConstPtr Problem::getMap() const
+{
+ return map_ptr_;
+}
+
+MapBasePtr Problem::getMap()
{
return map_ptr_;
}
@@ -1007,22 +995,42 @@ void Problem::setMap(MapBasePtr _map_ptr)
map_ptr_ = _map_ptr;
}
-TrajectoryBasePtr Problem::getTrajectory() const
+TrajectoryBaseConstPtr Problem::getTrajectory() const
+{
+ return trajectory_ptr_;
+}
+
+TrajectoryBasePtr Problem::getTrajectory()
{
return trajectory_ptr_;
}
-HardwareBasePtr Problem::getHardware() const
+HardwareBaseConstPtr Problem::getHardware() const
+{
+ return hardware_ptr_;
+}
+
+HardwareBasePtr Problem::getHardware()
{
return hardware_ptr_;
}
-FrameBasePtr Problem::getLastFrame() const
+FrameBaseConstPtr Problem::getLastFrame() const
+{
+ return trajectory_ptr_->getLastFrame();
+}
+
+FrameBasePtr Problem::getLastFrame()
{
return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
+FrameBaseConstPtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ return trajectory_ptr_->closestFrameToTimeStamp(_ts);
+}
+
+FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts)
{
return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
@@ -1095,10 +1103,10 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
// Emplace a feature and a factor for each state block
- for (const auto& pair_key_sb : prior_keyframe->getStateBlockMap())
+ for (auto pair_key_sb : prior_keyframe->getStateBlockMap())
{
- const auto& key = pair_key_sb.first;
- const auto& sb = pair_key_sb.second;
+ auto key = pair_key_sb.first;
+ auto sb = pair_key_sb.second;
const auto& state_blk = prior_options_->state.at(key);
const auto& covar_blk = prior_options_->cov.at(key,key);
@@ -1236,19 +1244,19 @@ bool Problem::check(int _verbose_level) const
void Problem::perturb(double amplitude)
{
// Sensors
- for (auto& S : getHardware()->getSensorList())
+ for (auto S : getHardware()->getSensorList())
S->perturb(amplitude);
// Frames and Captures
- for (auto& F : *(getTrajectory()))
+ for (auto F : getTrajectory()->getFrameMap())
{
- F->perturb(amplitude);
- for (auto& C : F->getCaptureList())
+ F.second->perturb(amplitude);
+ for (auto& C : F.second->getCaptureList())
C->perturb(amplitude);
}
// Landmarks
- for (auto& L : getMap()->getLandmarkList())
+ for (auto L : getMap()->getLandmarkList())
L->perturb(amplitude);
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index adeac3db58b63afc2907d12892325fcd8feefc8d..90e40f241c6d15a131b469f31db5c5b9b2938ee2 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -165,7 +165,7 @@ void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _co
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
}
-CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBaseConstPtr _prc_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -187,14 +187,14 @@ CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std
<< " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr
<< " -X-> Prc" << id();
auto sen_prc_list = sen_ptr->getProcessorList();
- auto sen_has_prc = std::find_if(sen_prc_list.begin(), sen_prc_list.end(), [&_prc_ptr](ProcessorBasePtr prc){ return prc == _prc_ptr;});
+ auto sen_has_prc = std::find(sen_prc_list.begin(), sen_prc_list.end(), _prc_ptr);
log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation);
return log;
}
-bool ProcessorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool ProcessorBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto prc_ptr = std::static_pointer_cast<ProcessorBase>(_node_ptr);
+ auto prc_ptr = std::static_pointer_cast<const ProcessorBase>(_node_ptr);
auto local_log = localCheck(_verbose, prc_ptr, _stream, _tabs);
_log.compose(local_log);
return _log.is_consistent_;
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 2c8e16ce62b9cb38c47849ae7520dba095a51776..288fbcfd2567f057ba958a2503b797935b931fcc 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -159,7 +159,7 @@ void ProcessorLoopClosure::process(CaptureBasePtr _capture)
// Emplace factors for each LC if validated
auto n_loops = 0;
- for (const auto& match_pair : match_lc_map)
+ for (auto match_pair : match_lc_map)
if (validateLoopClosure(match_pair.second))
{
emplaceFactors(match_pair.second);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 805f9bb4cac6847906a3aa9ed2e5f577dbdc63c4..0ad4a754dd665d4533303f2e27569b0e7c151048 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -82,7 +82,7 @@ ProcessorMotion::~ProcessorMotion()
}
-void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
+void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target)
{
assert(cap_prev != nullptr);
@@ -418,7 +418,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
- const auto& ts = getTimeStamp();
+ auto ts = getTimeStamp();
last_ptr_->setTimeStamp( ts );
last_ptr_->getFrame()->setTimeStamp( ts );
VectorComposite state_propa = getState( ts );
@@ -552,7 +552,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
*/
// Get state of origin
- const auto& x_origin = getOrigin()->getFrame()->getState(state_structure_);
+ auto x_origin = getOrigin()->getFrame()->getState(state_structure_);
// Get most recent motion
const auto& motion = last_ptr_->getBuffer().back();
@@ -561,7 +561,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
const auto& delta_preint = motion.delta_integr_;
// Get calibration preint -- stored in last capture
- const auto& calib_preint = last_ptr_->getCalibrationPreint();
+ auto calib_preint = last_ptr_->getCalibrationPreint();
VectorComposite state;
//WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp());
@@ -569,7 +569,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
if ( hasCalibration())
{
// Get current calibration -- from origin capture
- const auto& calib = getCalibration(origin_ptr_);
+ auto calib = getCalibration(origin_ptr_);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
@@ -578,7 +578,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
const auto& delta_step = J_delta_calib * (calib - calib_preint);
// correct delta // this is (+)
- const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+ auto delta_corrected = correctDelta(delta_preint, delta_step);
// compute current state // this is [+]
statePlusDelta(x_origin, delta_corrected, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
@@ -618,7 +618,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
// We need to search for the capture containing a motion buffer with the queried time stamp
- CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
+ auto capture_motion = findCaptureContainingTimeStamp(_ts);
if (capture_motion == nullptr) // we do not have any info of where to find a valid state
{
@@ -650,8 +650,8 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
*/
// Get state of origin
- CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- const auto& x_origin = cap_orig->getFrame()->getState(state_structure_);
+ auto cap_orig = capture_motion->getOriginCapture();
+ auto x_origin = cap_orig->getFrame()->getState(state_structure_);
// Get motion at time stamp
const auto& motion = capture_motion->getBuffer().getMotion(_ts);
@@ -660,14 +660,14 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
const auto& delta_preint = motion.delta_integr_;
// Get calibration preint -- stored in last capture
- const auto& calib_preint = capture_motion->getCalibrationPreint();
+ auto calib_preint = capture_motion->getCalibrationPreint();
VectorComposite state;
if ( hasCalibration() )
{
// Get current calibration -- from origin capture
- const auto& calib = getCalibration(cap_orig);
+ auto calib = getCalibration(cap_orig);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
@@ -676,7 +676,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
const auto& delta_step = J_delta_calib * (calib - calib_preint);
// correct delta // this is (+)
- const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+ auto delta_corrected = correctDelta(delta_preint, delta_step);
// compute current state // this is [+]
statePlusDelta(x_origin, delta_corrected, _ts - cap_orig->getTimeStamp(), state);
@@ -876,7 +876,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
}
}
-CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
+CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
assert(_ts.ok());
// assert(last_ptr_ != nullptr);
@@ -898,14 +898,66 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
- FrameBasePtr frame = nullptr;
- CaptureBasePtr capture = nullptr;
- CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->rend();
- ++frame_rev_iter)
+ FrameBaseConstPtr frame = nullptr;
+ CaptureBaseConstPtr capture = nullptr;
+ CaptureMotionConstPtr capture_motion = nullptr;
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- frame = *frame_rev_iter;
+ frame = frame_rev_iter->second;
+ auto sensor = getSensor();
+ capture = frame->getCaptureOf(sensor);
+ if (capture != nullptr)
+ {
+ assert(std::dynamic_pointer_cast<const CaptureMotion>(capture) != nullptr);
+ // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
+ capture_motion = std::static_pointer_cast<const CaptureMotion>(capture);
+
+ if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
+ {
+ // Found time stamp satisfying rule 3 above !! ==> break for loop
+ break;
+ }
+ else
+ capture_motion = nullptr;
+ }
+ }
+ return capture_motion;
+}
+
+CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts)
+{
+ 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:
+ // 1. See that the KF contains a CaptureMotion
+ // 2. See that the TS is smaller than the KF's TS
+ // 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
+ FrameBasePtr frame = nullptr;
+ CaptureBasePtr capture = nullptr;
+ CaptureMotionPtr capture_motion = nullptr;
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
+ {
+ frame = frame_rev_iter->second;
auto sensor = getSensor();
capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index bf24a1d4e2d04c9111605792120df484e369f888..471dd627361266aad20356543345355a5d18bdbf 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -242,7 +242,7 @@ FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, Cap
return fac_odom;
}
-VectorXd ProcessorOdom3d::getCalibration (const CaptureBasePtr _capture) const
+VectorXd ProcessorOdom3d::getCalibration (const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 512d941b7669c13e22a87aee23c5a8f84ed44329..b0e08b4b59240b359457565c95071d2b7c1385e6 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -112,15 +112,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->link(keyframe);
// Process info
+ // TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
+ // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
- // We only process new features in Last, here last = nullptr, so we do not have anything to do.
// Issue KF callback with new KF
getProblem()->keyFrameCallback(keyframe, shared_from_this());
- resetDerived();
-
// Update pointers
+ resetDerived();
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -130,9 +130,17 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
case SECOND_TIME_WITH_KEYFRAME :
{
// No-break case only for debug. Next case will be executed too.
- FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(),
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp(),
params_tracker_->time_tolerance);
-
+ // This received KF is discarded since it is most likely the same KF we createed in FIRST_TIME, ...
+ // ... only that in FIRST_TIME we checked for incominig, and now we checked for last.
+ // Such KF however should have been removed from the buffer of keyframes with the call to buffer_frame_.removeUpTo()
+
+ // The debug line is here to check if this is really the same KF
+ // or it is rather a new KF created by some other processor,
+ // which happens to be within tolerance of timestamps.
+ // In this case we discard it anyway because we already have a KF in last
+ // and we can't link a capture to two KFs.
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
}
// Fall through
@@ -140,14 +148,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
+ // Make a NON KEY Frame to hold incoming capture
FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
incoming_ptr_->link(keyframe);
- // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
+ // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
+ // Both Trackers: We have a last_ Capture with not enough features, so populate it.
processNew(params_tracker_->max_new_features);
// Establish factors
@@ -177,13 +187,13 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->move(keyframe_from_callback);
last_old_frame->remove();
- // Create new frame for incoming
+ // Create new NON KEY frame for incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
incoming_ptr_->link(frame);
- // Detect new Features, initialize Landmarks, create Factors, ...
+ // Detect new Features, initialize Landmarks, ...
processNew(params_tracker_->max_new_features);
// Establish factors
@@ -209,31 +219,26 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// process
processNew(params_tracker_->max_new_features);
- //TODO abort KF if known_features_last_.size() < params_tracker_->min_features_for_keyframe
-
// We create a KF
// set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
last_ptr_->getFrame()->link(getProblem());
- // // make F; append incoming to new F
- // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
- // incoming_ptr_->link(frm);
-
// Establish factors
establishFactors();
// Call the new keyframe callback in order to let the other processors to establish their factors
getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
- // Update pointers
- resetDerived();
- // make F; append incoming to new F
+ // make NON KEY frame; append incoming to new frame
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
incoming_ptr_ ->link(frame);
+
+ // Update pointers
+ resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
@@ -247,10 +252,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Advance this
advanceDerived();
- // Replace last frame for a new one in incoming
+ // Replace last frame for a new NON KEY frame in incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
incoming_ptr_->link(frame);
last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
@@ -282,7 +287,7 @@ void ProcessorTracker::computeProcessingStep()
// Then combine with the existence (or not) of a keyframe callback pack
switch (step)
{
- case FIRST_TIME :
+ case FIRST_TIME : // We check for KF in incoming
if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
processing_step_ = FIRST_TIME_WITH_KEYFRAME;
@@ -290,7 +295,7 @@ void ProcessorTracker::computeProcessingStep()
processing_step_ = FIRST_TIME_WITHOUT_KEYFRAME;
break;
- case SECOND_TIME :
+ case SECOND_TIME : // We check for KF in last
if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
processing_step_ = SECOND_TIME_WITH_KEYFRAME;
@@ -298,7 +303,7 @@ void ProcessorTracker::computeProcessingStep()
processing_step_ = SECOND_TIME_WITHOUT_KEYFRAME;
break;
- case RUNNING :
+ case RUNNING : // We check for KF in last
default :
if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index d7154a788b5727b6e2f8cd201b382643110d5bf1..ed4215d12e13403c135b433c440f7f1493612bca 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -31,7 +31,7 @@
namespace wolf
{
-size_t TrackMatrix::track_id_count_ = 0;
+SizeStd TrackMatrix::track_id_count_ = 0;
TrackMatrix::TrackMatrix()
{
@@ -43,7 +43,17 @@ TrackMatrix::~TrackMatrix()
//
}
-Track TrackMatrix::track(const SizeStd& _track_id) const
+TrackConst TrackMatrix::track(const SizeStd& _track_id) const
+{
+ TrackConst track_const;
+ if (tracks_.count(_track_id) > 0)
+ for (auto&& pair : tracks_.at(_track_id))
+ track_const[pair.first]=pair.second;
+
+ return track_const;
+}
+
+Track TrackMatrix::track(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id);
@@ -51,7 +61,27 @@ Track TrackMatrix::track(const SizeStd& _track_id) const
return Track();
}
-Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture) const
+SnapshotConst TrackMatrix::snapshot(CaptureBaseConstPtr _capture) const
+{
+ const auto& it = std::find_if(snapshots_.begin(),
+ snapshots_.end(),
+ [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair)
+ {
+ return pair.first == _capture;
+ }
+ );
+
+ if (it == snapshots_.end())
+ return SnapshotConst();
+
+ SnapshotConst snapshot_const;
+ for (auto&& pair : it->second)
+ snapshot_const[pair.first]=pair.second;
+
+ return snapshot_const;
+}
+
+Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
{
if (_capture && snapshots_.count(_capture) > 0)
return snapshots_.at(_capture);
@@ -91,7 +121,6 @@ void TrackMatrix::remove(const SizeStd& _track_id)
snapshots_.at(cap).erase(_track_id);
if (snapshots_.at(cap).empty())
snapshots_.erase(cap);
-
}
// Remove track
@@ -127,13 +156,13 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
{
if(tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp()))
{
- tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
+ tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
if (tracks_.at(_ftr->trackId()).empty())
tracks_.erase(_ftr->trackId());
}
if(snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId()))
{
- snapshots_. at(cap).erase(_ftr->trackId());
+ snapshots_ .at(cap).erase(_ftr->trackId());
if (snapshots_.at(cap).empty())
snapshots_.erase(cap);
}
@@ -141,17 +170,17 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
}
}
-size_t TrackMatrix::numTracks() const
+SizeStd TrackMatrix::numTracks() const
{
return tracks_.size();
}
-size_t TrackMatrix::trackSize(const SizeStd& _track_id) const
+SizeStd TrackMatrix::trackSize(const SizeStd& _track_id) const
{
return track(_track_id).size();
}
-FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
+FeatureBaseConstPtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).begin()->second;
@@ -159,7 +188,23 @@ FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
return nullptr;
}
-FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
+FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id)
+{
+ if (tracks_.count(_track_id) > 0)
+ return tracks_.at(_track_id).begin()->second;
+ else
+ return nullptr;
+}
+
+FeatureBaseConstPtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
+{
+ if (tracks_.count(_track_id) > 0)
+ return tracks_.at(_track_id).rbegin()->second;
+ else
+ return nullptr;
+}
+
+FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).rbegin()->second;
@@ -167,7 +212,19 @@ FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
return nullptr;
}
-vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
+vector<FeatureBaseConstPtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
+{
+ vector<FeatureBaseConstPtr> vec;
+ if (tracks_.count(_track_id))
+ {
+ vec.reserve(trackSize(_track_id));
+ for (auto const& pair_time_ftr : tracks_.at(_track_id))
+ vec.push_back(pair_time_ftr.second);
+ }
+ return vec;
+}
+
+vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
{
vector<FeatureBasePtr> vec;
if (tracks_.count(_track_id))
@@ -179,23 +236,59 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) cons
return vec;
}
-std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap) const
+FeatureBaseConstPtrList TrackMatrix::snapshotAsList(CaptureBaseConstPtr _cap) const
{
- std::list<FeatureBasePtr> lst;
+ SnapshotConst snapshot_const = snapshot(_cap);
+
+ FeatureBaseConstPtrList lst;
+
+ for (auto const& pair_trkid_ftr : snapshot_const)
+ lst.push_back(pair_trkid_ftr.second);
+
+ return lst;
+}
+
+FeatureBasePtrList TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
+{
+ FeatureBasePtrList lst;
if (snapshots_.count(_cap))
for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
lst.push_back(pair_trkid_ftr.second);
return lst;
}
-TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const
+TrackMatchesConst TrackMatrix::matches(CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const
+{
+ TrackMatchesConst pairs;
+
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
+ auto s_short = s_1;
+ auto s_long = s_2;
+ if (s_1.size() > s_2.size())
+ {
+ s_long = s_1;
+ s_short = s_2;
+ }
+
+ for (auto const & pair_trkid_ftr : s_short)
+ {
+ SizeStd trk_id = pair_trkid_ftr.first;
+ if (s_long.count(trk_id))
+ pairs[trk_id] = pair<FeatureBaseConstPtr, FeatureBaseConstPtr>(s_1.at(trk_id), s_2.at(trk_id));
+ }
+
+ return pairs;
+}
+
+TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
{
TrackMatches pairs;
- Snapshot s_1 = snapshot(_cap_1);
- Snapshot s_2 = snapshot(_cap_2);
- Snapshot s_short = s_1;
- Snapshot s_long = s_2;
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
+ auto s_short = s_1;
+ auto s_long = s_2;
if (s_1.size() > s_2.size())
{
s_long = s_1;
@@ -212,7 +305,15 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
return pairs;
}
-FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap) const
+FeatureBaseConstPtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBaseConstPtr _cap) const
+{
+ if (snapshot(_cap).count(_track_id))
+ return snapshot(_cap).at(_track_id);
+ else
+ return nullptr;
+}
+
+FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap)
{
if (snapshot(_cap).count(_track_id))
return snapshot(_cap).at(_track_id);
@@ -220,12 +321,36 @@ FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _ca
return nullptr;
}
-CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
+CaptureBaseConstPtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
{
return firstFeature(_track_id)->getCapture();
}
-Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
+CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id)
+{
+ return firstFeature(_track_id)->getCapture();
+}
+
+TrackConst TrackMatrix::trackAtKeyframes(const SizeStd& _track_id) const
+{
+ // We assemble a track_kf on the fly by checking each capture's frame.
+ if (tracks_.count(_track_id))
+ {
+ TrackConst track_kf;
+ for (auto& pair_ts_ftr : tracks_.at(_track_id))
+ {
+ auto& ts = pair_ts_ftr.first;
+ auto& ftr = pair_ts_ftr.second;
+ if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem())
+ track_kf[ts] = ftr;
+ }
+ return track_kf;
+ }
+ else
+ return TrackConst();
+}
+
+Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id)
{
// We assemble a track_kf on the fly by checking each capture's frame.
if (tracks_.count(_track_id))
@@ -244,12 +369,26 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
return Track();
}
-list<size_t> TrackMatrix::trackIds() const
+list<SizeStd> TrackMatrix::trackIds(CaptureBaseConstPtr _capture) const
{
- list<size_t> track_ids;
- for (auto track : tracks_)
+ list<SizeStd> track_ids;
+
+ if (not _capture)
+ for (auto track_pair : tracks_)
+ track_ids.push_back(track_pair.first);
+ else
{
- track_ids.push_back(track.first);
+ auto it = std::find_if(snapshots_.begin(),
+ snapshots_.end(),
+ [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair)
+ {
+ return pair.first == _capture;
+ }
+ );
+
+ if (it != snapshots_.end())
+ for (auto track_pair : it->second)
+ track_ids.push_back(track_pair.first);
}
return track_ids;
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0e649ec5cce5c11ca522c21bab70c67855922839..996d48263c113555ba055422153bf10459bfb564 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -42,9 +42,11 @@ SensorBase::SensorBase(const std::string& _type,
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
+ state_block_dynamic_(),
+ params_prior_map_(),
+ last_capture_(nullptr),
noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size),
- last_capture_(nullptr)
+ noise_cov_(_noise_size, _noise_size)
{
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.");
@@ -76,9 +78,11 @@ SensorBase::SensorBase(const std::string& _type,
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
+ state_block_dynamic_(),
+ params_prior_map_(),
+ last_capture_(nullptr),
noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size()),
- last_capture_(nullptr)
+ noise_cov_(_noise_std.size(), _noise_std.size())
{
setNoiseStd(_noise_std);
@@ -95,28 +99,12 @@ SensorBase::SensorBase(const std::string& _type,
SensorBase::~SensorBase()
{
// Remove State Blocks
- removeStateBlocks();
-}
-
-void SensorBase::removeStateBlocks()
-{
- for (const auto& key : getStructure())
- {
- auto sbp = getStateBlock(key);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr && !isStateBlockInCapture(key))
- {
- getProblem()->notifyStateBlock(sbp,REMOVE);
- }
- removeStateBlock(key);
- }
- }
+ removeStateBlocks(getProblem());
}
void SensorBase::fixExtrinsics()
{
- for (const auto& key : std::string("PO"))
+ for (auto key : std::string("PO"))
{
auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
@@ -126,7 +114,7 @@ void SensorBase::fixExtrinsics()
void SensorBase::unfixExtrinsics()
{
- for (const auto& key : std::string("PO"))
+ for (auto key : std::string("PO"))
{
auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
@@ -136,7 +124,7 @@ void SensorBase::unfixExtrinsics()
void SensorBase::fixIntrinsics()
{
- for (const auto& key : getStructure())
+ for (auto key : getStructure())
{
if (key != 'P' and key != 'O') // exclude extrinsics
{
@@ -149,7 +137,7 @@ void SensorBase::fixIntrinsics()
void SensorBase::unfixIntrinsics()
{
- for (const auto& key : getStructure())
+ for (auto key : getStructure())
{
if (key != 'P' and key != 'O') // exclude extrinsics
{
@@ -203,17 +191,17 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
params_prior_map_[_key] = ftr_prior;
}
-void SensorBase::registerNewStateBlocks() const
+void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
{
- if (getProblem() != nullptr)
+ if (_problem != nullptr)
{
for (auto & pair_key_sbp : getStateBlockMap())
{
auto key = pair_key_sbp.first;
auto sbp = pair_key_sbp.second;
- if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key))
- getProblem()->notifyStateBlock(sbp, ADD);
+ if (sbp and not isStateBlockDynamic(key))
+ _problem->notifyStateBlock(sbp, ADD);
}
}
}
@@ -240,21 +228,20 @@ void SensorBase::setLastCapture(CaptureBasePtr cap)
void SensorBase::updateLastCapture()
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
- if (getProblem())
+ if (not getProblem())
+ return;
+
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
- TrajectoryRevIter frame_rev_it = trajectory->rbegin();
- while (frame_rev_it != trajectory->rend())
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
+ if (capture and not capture->isRemoving())
{
- auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture and not capture->isRemoving())
- {
- // found the most recent Capture made by this sensor !
- last_capture_ = capture;
- return;
- }
- frame_rev_it++;
+ // found the most recent Capture made by this sensor !
+ last_capture_ = capture;
+ return;
}
}
@@ -262,58 +249,110 @@ void SensorBase::updateLastCapture()
last_capture_ = nullptr;
}
-CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
+CaptureBaseConstPtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
{
// we search for the most recent Capture of this sensor before _ts
if (not getProblem())
return nullptr;
- // 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.
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
+ {
+ if (frame_rev_iter->second->getTimeStamp() <= _ts)
+ {
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ return capture;
+ }
+ }
+
+ return nullptr;
+}
+
+CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts)
+{
+ // we search for the most recent Capture of this sensor before _ts
+ if (not getProblem())
+ return nullptr;
// 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())
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ if (frame_rev_iter->second->getTimeStamp() <= _ts)
{
- auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
return capture;
}
- frame_rev_it++;
}
return nullptr;
}
-StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
+StateBlockConstPtr SensorBase::getP(const TimeStamp _ts) const
+{
+ return getStateBlockDynamic('P', _ts);
+}
+
+StateBlockConstPtr SensorBase::getO(const TimeStamp _ts) const
+{
+ return getStateBlockDynamic('O', _ts);
+}
+
+StateBlockConstPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
+{
+ return getStateBlockDynamic('I', _ts);
+}
+
+StateBlockConstPtr SensorBase::getP() const
+{
+ return getStateBlockDynamic('P');
+}
+
+StateBlockConstPtr SensorBase::getO() const
+{
+ return getStateBlockDynamic('O');
+}
+
+StateBlockConstPtr SensorBase::getIntrinsic() const
+{
+ return getStateBlockDynamic('I');
+}
+
+StateBlockPtr SensorBase::getP(const TimeStamp _ts)
{
return getStateBlockDynamic('P', _ts);
}
-StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
+StateBlockPtr SensorBase::getO(const TimeStamp _ts)
{
return getStateBlockDynamic('O', _ts);
}
-StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
+StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
{
return getStateBlockDynamic('I', _ts);
}
-StateBlockPtr SensorBase::getP() const
+StateBlockPtr SensorBase::getP()
{
return getStateBlockDynamic('P');
}
-StateBlockPtr SensorBase::getO() const
+StateBlockPtr SensorBase::getO()
{
return getStateBlockDynamic('O');
}
-StateBlockPtr SensorBase::getIntrinsic() const
+StateBlockPtr SensorBase::getIntrinsic()
{
return getStateBlockDynamic('I');
}
@@ -356,7 +395,17 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
processor_list_.remove(_proc_ptr);
}
-StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
+StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key) const
+{
+ CaptureBaseConstPtr cap;
+
+ if (isStateBlockInCapture(_key, cap))
+ return cap->getStateBlock(_key);
+
+ return getStateBlock(_key);
+}
+
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key)
{
CaptureBasePtr cap;
@@ -366,7 +415,17 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
return getStateBlock(_key);
}
-StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
+StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
+{
+ CaptureBaseConstPtr cap;
+
+ if (isStateBlockInCapture(_key, _ts, cap))
+ return cap->getStateBlock(_key);
+
+ return getStateBlock(_key);
+}
+
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts)
{
CaptureBasePtr cap;
@@ -376,7 +435,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp
return getStateBlock(_key);
}
-bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBaseConstPtr& _cap) const
{
if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
{
@@ -388,7 +447,31 @@ bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) c
return false;
}
-bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap)
+{
+ if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
+ {
+ _cap = last_capture_;
+
+ return _cap != nullptr;
+ }
+ else
+ return false;
+}
+
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBaseConstPtr& _cap) const
+{
+ if (isStateBlockDynamic(_key))
+ {
+ _cap = findLastCaptureBefore(_ts);
+
+ return _cap != nullptr;
+ }
+ else
+ return false;
+}
+
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap)
{
if (isStateBlockDynamic(_key))
{
@@ -402,14 +485,14 @@ bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, C
bool SensorBase::isStateBlockInCapture(const char& _key) const
{
- CaptureBasePtr cap;
+ CaptureBaseConstPtr cap;
return isStateBlockInCapture(_key, cap);
}
bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const
{
- CaptureBasePtr cap;
+ CaptureBaseConstPtr cap;
return isStateBlockInCapture(_key, _ts, cap);
}
@@ -432,7 +515,7 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
this->setHardware(_hw_ptr);
this->getHardware()->addSensor(shared_from_this());
this->setProblem(_hw_ptr->getProblem());
- this->registerNewStateBlocks();
+ this->registerNewStateBlocks(getProblem());
}
else
{
@@ -454,7 +537,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
if (_metric && _state_blocks)
{
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
auto sb = getStateBlockDynamic(key);
if (sb)
@@ -470,9 +553,9 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
else if (_state_blocks)
{
_stream << _tabs << " " << "sb:";
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
- const auto &sb = getStateBlockDynamic(key);
+ auto sb = getStateBlockDynamic(key);
if (sb)
_stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
@@ -490,7 +573,7 @@ void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
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
+CheckLog SensorBase::localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
if (_verbose)
@@ -514,6 +597,7 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
std::stringstream inconsistency_explanation;
auto hwd_ptr = getHardware();
+
// check problem and hardware pointers
inconsistency_explanation << "Sensor problem pointer " << getProblem().get()
<< " different from Hardware problem pointer " << hwd_ptr->getProblem().get() << "\n";
@@ -523,9 +607,10 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
<< " ---> Hwd" << " @ " << hwd_ptr
<< " -X-> Sen" << id();
auto hwd_sen_list = hwd_ptr->getSensorList();
- auto hwd_has_sen = std::find_if(hwd_sen_list.begin(), hwd_sen_list.end(), [&_sen_ptr](SensorBasePtr sen){ return sen == _sen_ptr;});
+ auto hwd_has_sen = std::find(hwd_sen_list.begin(), hwd_sen_list.end(), _sen_ptr);
log.assertTrue(hwd_has_sen != hwd_sen_list.end(), inconsistency_explanation);
+ // Check processors
for(auto prc : getProcessorList())
{
inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
@@ -533,6 +618,7 @@ 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())
{
@@ -550,9 +636,9 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
return log;
}
-bool SensorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool SensorBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto sen_ptr = std::static_pointer_cast<SensorBase>(_node_ptr);
+ auto sen_ptr = std::static_pointer_cast<const SensorBase>(_node_ptr);
auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index e0168dd1bc83a6f349f32f31995d08a776fa5699..c9d192a536151f0340a7fd1f8f9f5e88acf54c26 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -250,7 +250,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
*
* Notification is put back on problem notifications, it will be added next update() call.
*/
- for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ for (auto st : fac_ptr->getStateBlockPtrVector())
if (state_blocks_.count(st) == 0)
{
// Check if it was stored as a 'floating' state block
@@ -273,7 +273,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
factors_.insert(fac_ptr);
// state-factor map
- for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ for (auto st : fac_ptr->getStateBlockPtrVector())
{
assert(state_blocks_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
@@ -300,7 +300,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
factors_.erase(fac_ptr);
// state-factor map
- for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ for (auto st : fac_ptr->getStateBlockPtrVector())
{
assert(state_blocks_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 0f4d8283734110d8a7d19bd8b8a3681ad5f1641f..84f6f74682e43a17d39d74f55a5d1aedb43944ee 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "../../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/solver_ceres.h"
SolverManager::SolverManager()
{
@@ -49,14 +49,14 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
}
else
{
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
+ // ADD/UPDATE STATE BLOKS
+ for(auto state_block : _problem_ptr->getStateList())
{
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
+ if (state_block->getPendingStatus() == ADD_PENDING)
+ addStateUnit(state_block);
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
+ else if(state_block->getPendingStatus() == UPDATE_PENDING)
+ updateStateUnitStatus(state_block);
}
//std::cout << "state units updated!" << std::endl;
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 36cada937e0412330e7242d2092948807549fea9..047469dc899a8973b7bdf07df318b2079c155ef9 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -27,8 +27,9 @@ namespace wolf
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.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_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);
+ state_block_const_map_.emplace(_sb_type, _sb);
if (!isInStructure(_sb_type))
appendToStructure(_sb_type);
@@ -39,7 +40,7 @@ StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlo
return _sb;
}
-void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) const
+void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem)
{
if (_problem != nullptr)
{
@@ -79,7 +80,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
{
if (_metric && _state_blocks)
{
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
auto sb = getStateBlock(key);
if (sb)
@@ -98,9 +99,9 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
else if (_state_blocks)
{
_stream << _tabs << " " << "sb:";
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
- const auto &sb = getStateBlock(key);
+ auto sb = getStateBlock(key);
if (sb)
_stream << " " << key
<< " [" << (sb->isFixed() ? "Fix" : "Est")
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 9f3e729eafd84f37fe67655142fb77555034253f..ff1888db04f37a69588d84a569e847abe3d937d3 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -200,11 +200,11 @@ bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::M
bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const
{
- const auto &row_it = this->find(_row);
+ auto row_it = this->find(_row);
if(row_it != this->end())
return false;
- const auto &col_it = row_it->second.find(_col);
+ auto col_it = row_it->second.find(_col);
if(col_it != row_it->second.end())
return false;
@@ -215,10 +215,10 @@ bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_m
Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
{
- const auto &row_it = this->find(_row);
+ auto row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
- const auto &col_it = row_it->second.find(_col);
+ auto col_it = row_it->second.find(_col);
assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
return col_it->second;
@@ -226,10 +226,10 @@ Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const
{
- const auto &row_it = this->find(_row);
+ auto row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
- const auto &col_it = row_it->second.find(_col);
+ auto col_it = row_it->second.find(_col);
assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
return col_it->second;
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 4bf0282fd90354856dc43b896a12a97ad88b1d7c..24de3de294a6e65ea409d345a31a23393904ae50 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -21,13 +21,14 @@
//--------LICENSE_END--------
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
+#include <iterator>
namespace wolf {
TrajectoryBase::TrajectoryBase() :
- NodeBase("TRAJECTORY", "TrajectoryBase")
+ NodeBase("TRAJECTORY", "TrajectoryBase"),
+ frame_map_()
{
- frame_map_ = FrameMap();
}
TrajectoryBase::~TrajectoryBase()
@@ -47,75 +48,141 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
- // add to list
- // frame_map_.erase(_frame_ptr);
frame_map_.erase(_frame_ptr->getTimeStamp());
}
-void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
+void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
- for(auto fr_ptr : *this)
- fr_ptr->getFactorList(_fac_list);
+ for(auto fr_pair: frame_map_)
+ fr_pair.second->getFactorList(_fac_list);
}
+void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
+{
+ for(auto fr_pair: frame_map_)
+ fr_pair.second->getFactorList(_fac_list);
+}
-FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
+TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const
{
- FrameBasePtr closest_kf = nullptr;
//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
- {
- 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
- {
- //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;
- }
- }
- }
- }
- return closest_kf;
+ if(frame_map_.empty())
+ return TimeStamp::Invalid();
+
+ // Lower bound provides the first iterator that does not go before ts (maybe equal or greater)
+ auto lower_bound = frame_map_.lower_bound(_ts);
+
+ // If first frame does not go before ts, it is the closest one
+ if ( lower_bound == frame_map_.begin())
+ return frame_map_.begin()->first;
+
+ // If last frame goes before ts, it is the closest one as well
+ if ( lower_bound == frame_map_.end())
+ return frame_map_.rbegin()->first;
+
+ // Otherwise we have to compare lb and its previous
+ auto upper_bound = lower_bound;
+ lower_bound = std::prev(lower_bound);
+
+ auto lb_dist = fabs(lower_bound->first - _ts);
+ auto ub_dist = fabs(upper_bound->first - _ts);
+ if(lb_dist < ub_dist)
+ return lower_bound->first;
+ else
+ return upper_bound->first;
+
+ // unreachable
+ return TimeStamp::Invalid();
+}
+
+FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ auto closest_ts = closestTimeStampToTimeStamp(_ts);
+
+ assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
+
+ if (closest_ts.ok())
+ return frame_map_.at(closest_ts);
+
+ return nullptr;
+}
+
+FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
+{
+ auto closest_ts = closestTimeStampToTimeStamp(_ts);
+
+ assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
+
+ if (closest_ts.ok())
+ return frame_map_.at(closest_ts);
+
+ return nullptr;
+}
+
+FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
+{
+ auto frame_it = frame_map_.find(_frame->getTimeStamp());
+ WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
+
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_it, frame_map_.end()) <= i)
+ return nullptr;
+
+ return std::next(frame_it,i)->second;
+}
+
+FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame, const unsigned int& i)
+{
+ auto frame_it = frame_map_.find(_frame->getTimeStamp());
+ WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
+
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_it, frame_map_.end()) <= i)
+ return nullptr;
+
+ return std::next(frame_it,i)->second;
+}
+
+FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
+{
+ auto frame_it = frame_map_.find(_frame->getTimeStamp());
+ WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
+
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_map_.begin(), frame_it) < i)
+ return nullptr;
+
+ return std::prev(frame_it,i)->second;
+}
+
+FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame, const unsigned int& i)
+{
+ auto frame_it = frame_map_.find(_frame->getTimeStamp());
+ WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
+
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_map_.begin(), frame_it) < i)
+ return nullptr;
+
+ return std::prev(frame_it,i)->second;
}
+
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(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 : *this)
- if (F)
- F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ for (auto F_pair : frame_map_)
+ if (F_pair.second)
+ F_pair.second->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBaseConstPtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -131,13 +198,15 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, s
log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
return log;
}
-bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+
+bool TrajectoryBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto trj_ptr = std::static_pointer_cast<TrajectoryBase>(_node_ptr);
+ auto trj_ptr = std::static_pointer_cast<const TrajectoryBase>(_node_ptr);
auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs);
_log.compose(local_log);
- for (auto F : *this)
- F->check(_log, F, _verbose, _stream, _tabs + " ");
+ for (auto F_pair : getFrameMap())
+ if (F_pair.second)
+ F_pair.second->check(_log, F_pair.second, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
} // namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 5d6f1345ee09674750d129e64a89a87ca081902d..1a0f44507a010891a792ce2f01af16d2233ce6cb 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -26,7 +26,7 @@ namespace wolf
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->getFrameMap().size();
+ int n_f = getProblem()->getTrajectory()->size();
bool remove_first = (n_f > params_sw_->n_frames);
int n_fix = (n_f >= params_sw_->n_frames ?
params_sw_->n_fix_first_frames :
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index b2ab0e2fdc0d807e6953c0fab95c59ed391b5311..85b9c4cf7f92164f93aa1c10a46c22fa53c07ef8 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -36,7 +36,7 @@ TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeMan
void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+ int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames
// recent segment not complete
if (n_f <= params_swdr_->n_frames_recent)
@@ -46,10 +46,8 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
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;
+ FrameBasePtr remove_recent_frame = getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(params_swdr_->n_frames_recent);
+ FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame();
// compose motion captures for all processors motion
for (auto motion_provider_pair : getProblem()->getMotionProviderMap())
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
index 20dc65e0e7040cfe92c7d6a39b14f66092ef3f14..85d1b3f528aca99336dcffa5f420d6a7ca2de243 100644
--- a/src/utils/graph_search.cpp
+++ b/src/utils/graph_search.cpp
@@ -96,7 +96,7 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
FactorBasePtrList facs_by = frm->getConstrainedByList();
// Iterate over all factors_by
- for (auto && fac_by : facs_by)
+ for (auto fac_by : facs_by)
{
//WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
//WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
@@ -115,7 +115,7 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
frm->getFactorList(facs_own);
// Iterate over all factors_own
- for (auto && fac_own : facs_own)
+ for (auto fac_own : facs_own)
{
//WOLF_INFO_COND(fac_own, "fac_own: ", fac_own->id());
//WOLF_INFO_COND(fac_own->getFrameOtherList().empty(), "fac_own->getFrameOtherList() is empty");
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index a8dd748ea29aa2a63eb72003b5c37465d62c3d17..771d19b7414b2a4a7342c8afea09d08d19f1b750 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
// wolf
#include "core/processor/processor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/processor/factory_processor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
index 68a44ff61adaabb99cb16cc1bc1c72017de6d813..dcb0cf0c10e428a6dfce876a1765e8c0d245e1a2 100644
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -33,6 +33,7 @@
// wolf
#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/factory_sensor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
index 02e0ff3d8e5296398a5f9cfc52b66fd21cf15d85..0cb47d441d9b8a0786a9fcf8d00952a682072c4a 100644
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
// wolf
#include "core/sensor/sensor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
index 102729bd91dcfca4f51a567f47d790ce1de6c6b7..bfbe151c43315c21e5f636e2632fa9ceb614c9d4 100644
--- a/src/yaml/sensor_pose_yaml.cpp
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -31,7 +31,7 @@
// wolf
#include "core/sensor/sensor_pose.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index dd7732c47dbffb1e1e7e2502d0ecc78c60e9f926..6fa50395a7ee2af50d290b32e543b18bac534869 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,36 +1,7 @@
# Retrieve googletest from github & compile
add_subdirectory(gtest)
-
-## Added these two include_dirs: ######################
-#
-#CMAKE modules
-#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-#MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-#
-# Include Eigen
-#FIND_PACKAGE(Eigen 3 REQUIRED)
-#INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-#
-# Include Ceres
-#FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-#IF(Ceres_FOUND)
-# INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-#ENDIF(Ceres_FOUND)
-#
-## and now gtest_motion_2d works ######################
-
-
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
-############# USE THIS TEST AS AN EXAMPLE #################
-# #
-# Create a specific test executable for gtest_example #
-wolf_add_gtest(gtest_example gtest_example.cpp) #
-target_link_libraries(gtest_example ${PLUGIN_NAME}) #
-# #
-###########################################################
+# Add here the source (.cpp) files in dummy folder
set(SRC_DUMMY
dummy/processor_tracker_feature_dummy.cpp
dummy/processor_tracker_landmark_dummy.cpp
@@ -38,274 +9,230 @@ set(SRC_DUMMY
)
add_library(dummy SHARED ${SRC_DUMMY})
target_link_libraries(dummy ${PLUGIN_NAME})
-################# ADD YOUR TESTS BELOW ####################
+
+############# USE THIS TEST AS AN EXAMPLE #################
+# #
+# Create a specific test executable for gtest_example #
+wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
-# ==== IN ALPHABETICAL ORDER! ==== #
+# OPTIONAL: if the gtest depends on the dummy library #
+# (used for implementing pure virtual base classes), #
+# add a line #
+# target_link_libraries(gtest_example dummy) #
# #
+###########################################################
-# ------- First Core classes ----------
+################ ADD YOUR TESTS BELOW ####################
+# #
+# ==== IN ALPHABETICAL ORDER! ==== #
+# #
+##########################################################
+
+# ------- First Base classes ----------
# BufferFrame
wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
-target_link_libraries(gtest_buffer_frame ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_buffer_frame PUBLIC dummy)
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-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 ${PLUGIN_NAME})
# FactorBase class test
wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
-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 ${PLUGIN_NAME})
# Factory test
wolf_add_gtest(gtest_factory gtest_factory.cpp)
-target_link_libraries(gtest_factory ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_factory PUBLIC dummy)
# FactoryStateBlock factory test
wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
-target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
+target_link_libraries(gtest_factory_state_block PUBLIC dummy)
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_emplace PUBLIC dummy)
# FeatureBase classes test
wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-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 ${PLUGIN_NAME})
# GraphSearch class test
wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
-target_link_libraries(gtest_graph_search ${PLUGIN_NAME})
# HasStateBlocks classes test
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
-target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
# IsMotion classes test
wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
-target_link_libraries(gtest_motion_provider ${PLUGIN_NAME})
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-target_link_libraries(gtest_local_param ${PLUGIN_NAME})
# Logging test
wolf_add_gtest(gtest_logging gtest_logging.cpp)
-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 ${PLUGIN_NAME})
# Parameters server
-wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
-target_link_libraries(gtest_param_server ${PLUGIN_NAME})
+wolf_add_gtest(gtest_param_server gtest_param_server.cpp)
# YAML parser
wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
-target_link_libraries(gtest_parser_yaml ${PLUGIN_NAME})
# Problem class test
wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_problem PUBLIC dummy)
# ProcessorBase class test
wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_processor_base PUBLIC dummy)
# ProcessorMotion class test
wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
-target_link_libraries(gtest_rotation ${PLUGIN_NAME})
# SE3 test
wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
-target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
# SE2 test
wolf_add_gtest(gtest_SE2 gtest_SE2.cpp)
-target_link_libraries(gtest_SE2 ${PLUGIN_NAME})
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
-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 ${PLUGIN_NAME})
# SolverManager test
wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
-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 ${PLUGIN_NAME})
# StateBlock class test
wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
-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 ${PLUGIN_NAME})
# TrackMatrix class test
wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
-target_link_libraries(gtest_track_matrix ${PLUGIN_NAME})
# TrajectoryBase class test
wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
-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 ${PLUGIN_NAME})
# ------- Now Derived classes ----------
# FactorAbs(P/O/V) classes test
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
-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 ${PLUGIN_NAME})
# FactorBlockDifference class test
wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
-target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME})
# FactorDiffDrive class test
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
-target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
# FactorOdom2dAutodiff class test
wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
-target_link_libraries(gtest_factor_odom_2d_autodiff ${PLUGIN_NAME})
# FactorPose2d class test
wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
-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 ${PLUGIN_NAME})
# FactorRelativePose2d class test
wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
-target_link_libraries(gtest_factor_relative_pose_2d ${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 ${PLUGIN_NAME})
# FactorRelativePose3d class test
wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
-target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
# FactorRelativePose3dWithExtrinsics class test
wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${PLUGIN_NAME})
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
-target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME})
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-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 ${PLUGIN_NAME})
# Parameter prior test
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
# ProcessorFixedWingModel class test
wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
-target_link_libraries(gtest_processor_fixed_wing_model ${PLUGIN_NAME})
# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
-target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
# ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
-target_link_libraries(gtest_processor_loop_closure ${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 ${PLUGIN_NAME})
# ProcessorMotion in 2d
wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
-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 ${PLUGIN_NAME})
# FactorPose3dWithExtrinsics class test
wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_processor_and_factor_pose_3d_with_extrinsics ${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 ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_processor_tracker_feature_dummy PUBLIC 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 ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC dummy)
# SensorDiffDriveSelfcalib class test
wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
-target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
# SensorDiffDriveSelfcalib class test
wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
-target_link_libraries(gtest_sensor_pose ${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 ${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 ${PLUGIN_NAME})
diff --git a/test/dummy/factor_dummy.h b/test/dummy/factor_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..73174d1468e07807e26ab2a6dcf4579ad5a00ef8
--- /dev/null
+++ b/test/dummy/factor_dummy.h
@@ -0,0 +1,112 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_DUMMY_H
+#define FACTOR_DUMMY_H
+
+#include "core/factor/factor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorDummy);
+
+class FactorDummy : public FactorBase
+{
+ public:
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtr& _frame_other,
+ const CaptureBasePtr& _capture_other,
+ const FeatureBasePtr& _feature_other,
+ const LandmarkBasePtr& _landmark_other) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other ? FrameBasePtrList({_frame_other}) : FrameBasePtrList(),
+ _capture_other ? CaptureBasePtrList({_capture_other}) : CaptureBasePtrList(),
+ _feature_other ? FeatureBasePtrList({_feature_other}) : FeatureBasePtrList(),
+ _landmark_other ? LandmarkBasePtrList({_landmark_other}) : LandmarkBasePtrList(),
+ nullptr,
+ false)
+ {
+ //
+ }
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ nullptr,
+ false)
+ {
+ //
+ }
+ ~FactorDummy() override = default;
+
+ bool evaluate(double const* const* _parameters,
+ double* _residuals,
+ double** _jacobians) const override
+ {
+ return true;
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& _residual,
+ std::vector<Eigen::MatrixXd>& _jacobians) const override
+ {
+
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JacobianMethod::JAC_ANALYTIC;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockConstPtr>();
+ }
+
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return std::vector<StateBlockPtr>();
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return std::vector<unsigned int>();
+ }
+
+ unsigned int getSize() const override
+ {
+ return 0;
+ }
+};
+
+}
+
+#endif // FACTOR_DUMMY_H
\ No newline at end of file
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index c04043d857a43f6db5391b2c463b5328ab331a90..b8f8266c43dea8060c28d5677cbb1082527f11a8 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -53,7 +53,8 @@ class FactorFeatureDummy : public FactorBase
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockConstPtr>(0);}
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override {return std::vector<StateBlockPtr>(0);}
/** \brief Returns the factor residual size
**/
@@ -78,10 +79,10 @@ inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr
FactorBase("FactorFeatureDummy",
TOP_OTHER,
_feature_ptr,
- nullptr,
- nullptr,
- _feature_other_ptr,
- nullptr,
+ FrameBasePtrList(),
+ CaptureBasePtrList(),
+ FeatureBasePtrList({_feature_other_ptr}),
+ LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status)
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index f4c53fa10da0908a5cd82ef3c83fe9796070abee..ceff8247e73c02bb8ac9bb2ca7151486e3f69f87 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -42,14 +42,14 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
bool evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const override {return true;};
+ double* residuals,
+ double** jacobians) const override {return true;};
/** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
void evaluate(const std::vector<const double*>& _states_ptr,
- Eigen::VectorXd& residual_,
- std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+ Eigen::VectorXd& residual_,
+ std::vector<Eigen::MatrixXd>& jacobians_) const override {};
/** \brief Returns the jacobians computation method
**/
@@ -57,7 +57,11 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockConstPtr>(0);
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
{
return std::vector<StateBlockPtr>(0);
}
@@ -88,10 +92,10 @@ inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_p
FactorBase("FactorFeatureDummy",
TOP_OTHER,
_feature_ptr,
- nullptr,
- nullptr,
- nullptr,
- _landmark_other_ptr,
+ FrameBasePtrList(),
+ CaptureBasePtrList(),
+ FeatureBasePtrList(),
+ LandmarkBasePtrList({_landmark_other_ptr}),
_processor_ptr,
_apply_loss_function,
_status)
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 559756b02472653c655c44152c452ba8767ef6f2..2294c819aec95c1f2c6bed5c0e7b1d6cc857c614 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,73 @@
-cmake_minimum_required(VERSION 2.8.8)
-project(gtest_builder C CXX)
+if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+ message("CMake version less than 3.11.0")
-# We need thread support
-#find_package(Threads REQUIRED)
+ # Enable ExternalProject CMake module
+ include(ExternalProject)
-# Enable ExternalProject CMake module
-include(ExternalProject)
+ set(GTEST_FORCE_SHARED_CRT ON)
+ set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+ # Download GoogleTest
+ ExternalProject_Add(googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG v1.8.x
+ # TIMEOUT 1 # We'll try this
+ CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+ -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+ -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+ -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+ -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+ -DBUILD_GTEST=ON
+ PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+ # Disable install step
+ INSTALL_COMMAND ""
+ UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+ )
-# For some reason I need to disable PTHREADS
-# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
-# This is a known issue for MinGW :
-# https://github.com/google/shaderc/pull/174
-#if(MINGW)
- set(GTEST_DISABLE_PTHREADS ON)
-#endif()
+ # Get GTest source and binary directories from CMake project
-# Download GoogleTest
-ExternalProject_Add(googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG v1.8.x
- # TIMEOUT 1 # We'll try this
- CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
- -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
- -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
- -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
- -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
- -DBUILD_GTEST=ON
- PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
- # Disable install step
- INSTALL_COMMAND ""
- UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-)
+ # Specify include dir
+ ExternalProject_Get_Property(googletest source_dir)
+ set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-# Get GTest source and binary directories from CMake project
+ # Specify MainTest's link libraries
+ ExternalProject_Get_Property(googletest binary_dir)
+ set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+ # Create a libgtest target to be used as a dependency by test programs
+ add_library(libgtest IMPORTED STATIC GLOBAL)
+ add_dependencies(libgtest googletest)
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+ # Set libgtest properties
+ set_target_properties(libgtest PROPERTIES
+ "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+ "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+ )
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+else()
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
- "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
- "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+ message("CMake version equal or greater than 3.11.0")
+ include(FetchContent)
+
+ FetchContent_Declare(
+ googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG main)
+
+ SET(INSTALL_GTEST OFF) # Disable installation of googletest
+ FetchContent_MakeAvailable(googletest)
+
+endif()
+
function(wolf_add_gtest target)
add_executable(${target} ${ARGN})
- add_dependencies(${target} libgtest)
- target_link_libraries(${target} libgtest)
-
- #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+ if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+ add_dependencies(${target} libgtest)
+ target_link_libraries(${target} PUBLIC libgtest ${PLUGIN_NAME})
+ target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
+ else()
+ target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
+ endif()
add_test(NAME ${target} COMMAND ${target})
endfunction()
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index c8a04dcd16131c9e0549d5b34ed9d81d4c353ec8..fca61dad1708a8ad71ff73b8049cbf99c0d3d9bf 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -29,6 +29,7 @@
#include "core/utils/utils_gtest.h"
#include "core/factor/factor_base.h"
+#include "dummy/factor_dummy.h"
using namespace wolf;
using namespace Eigen;
@@ -55,58 +56,6 @@ class FactorBaseTest : public testing::Test
// virtual void TearDown(){}
};
-class FactorDummy : public FactorBase
-{
- public:
- FactorDummy(const FeatureBasePtr& _feature,
- const FrameBasePtr& _frame_other,
- const CaptureBasePtr& _capture_other,
- const FeatureBasePtr& _feature_other,
- const LandmarkBasePtr& _landmark_other) :
- FactorBase("Dummy",
- TOP_OTHER,
- _feature,
- _frame_other,
- _capture_other,
- _feature_other,
- _landmark_other,
- nullptr,
- false)
- {
- //
- }
- FactorDummy(const FeatureBasePtr& _feature,
- const FrameBasePtrList& _frame_other_list,
- const CaptureBasePtrList& _capture_other_list,
- const FeatureBasePtrList& _feature_other_list,
- const LandmarkBasePtrList& _landmark_other_list) :
- FactorBase("Dummy",
- TOP_OTHER,
- _feature,
- _frame_other_list,
- _capture_other_list,
- _feature_other_list,
- _landmark_other_list,
- nullptr,
- false)
- {
- //
- }
- ~FactorDummy() override = default;
-
- bool evaluate(double const* const* _parameters,
- double* _residuals,
- double** _jacobians) const override {return true;}
- void evaluate(const std::vector<const double*>& _states_ptr,
- Eigen::VectorXd& _residual,
- std::vector<Eigen::MatrixXd>& _jacobians) const override {}
- JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
- std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
- unsigned int getSize() const override {return 0;}
-
-};
-
TEST_F(FactorBaseTest, constructor_from_pointers)
{
FactorDummy fac(f0,nullptr,C0,f1,nullptr);
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 4f280a6ff5482d8d02961438b0aaa119990bf78d..c37bed03e30da209b9b644d49b92011afeeb5f93 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -83,7 +83,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- VectorXd getCalibration (const CaptureBasePtr _capture) const override
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override
{
return Base::getCalibration(_capture);
}
@@ -626,7 +626,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
- Vector3d data;
+ Vector2d data;
Matrix2d data_cov; data_cov.setIdentity();
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 26c9b3cc0fe7f5aff4eb533c93826c84cb9d0d55..eb61f798d41b2aec792b864dffb39cb1cdb7d098 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -170,28 +170,226 @@ TEST(FrameBase, Frames)
ASSERT_EQ(F9, T->closestFrameToTimeStamp(9));
// Next frame
- ASSERT_EQ(F1, F0->getNextFrame());
- ASSERT_EQ(F2, F1->getNextFrame());
- ASSERT_EQ(F3, F2->getNextFrame());
- ASSERT_EQ(F4, F3->getNextFrame());
- ASSERT_EQ(F5, F4->getNextFrame());
- ASSERT_EQ(F6, F5->getNextFrame());
- ASSERT_EQ(F7, F6->getNextFrame());
- ASSERT_EQ(F8, F7->getNextFrame());
- ASSERT_EQ(F9, F8->getNextFrame());
- ASSERT_EQ(nullptr, F9->getNextFrame());
+ EXPECT_EQ(F1, F0->getNextFrame(1));
+ EXPECT_EQ(F2, F0->getNextFrame(2));
+ EXPECT_EQ(F3, F0->getNextFrame(3));
+ EXPECT_EQ(F4, F0->getNextFrame(4));
+ EXPECT_EQ(F5, F0->getNextFrame(5));
+ EXPECT_EQ(F6, F0->getNextFrame(6));
+ EXPECT_EQ(F7, F0->getNextFrame(7));
+ EXPECT_EQ(F8, F0->getNextFrame(8));
+ EXPECT_EQ(F9, F0->getNextFrame(9));
+ EXPECT_EQ(nullptr, F0->getNextFrame(10));
+
+ EXPECT_EQ(F2, F1->getNextFrame(1));
+ EXPECT_EQ(F3, F1->getNextFrame(2));
+ EXPECT_EQ(F4, F1->getNextFrame(3));
+ EXPECT_EQ(F5, F1->getNextFrame(4));
+ EXPECT_EQ(F6, F1->getNextFrame(5));
+ EXPECT_EQ(F7, F1->getNextFrame(6));
+ EXPECT_EQ(F8, F1->getNextFrame(7));
+ EXPECT_EQ(F9, F1->getNextFrame(8));
+ EXPECT_EQ(nullptr, F1->getNextFrame(9));
+ EXPECT_EQ(nullptr, F1->getNextFrame(10));
+
+ EXPECT_EQ(F3, F2->getNextFrame(1));
+ EXPECT_EQ(F4, F2->getNextFrame(2));
+ EXPECT_EQ(F5, F2->getNextFrame(3));
+ EXPECT_EQ(F6, F2->getNextFrame(4));
+ EXPECT_EQ(F7, F2->getNextFrame(5));
+ EXPECT_EQ(F8, F2->getNextFrame(6));
+ EXPECT_EQ(F9, F2->getNextFrame(7));
+ EXPECT_EQ(nullptr, F2->getNextFrame(8));
+ EXPECT_EQ(nullptr, F2->getNextFrame(9));
+ EXPECT_EQ(nullptr, F2->getNextFrame(10));
+
+ EXPECT_EQ(F4, F3->getNextFrame(1));
+ EXPECT_EQ(F5, F3->getNextFrame(2));
+ EXPECT_EQ(F6, F3->getNextFrame(3));
+ EXPECT_EQ(F7, F3->getNextFrame(4));
+ EXPECT_EQ(F8, F3->getNextFrame(5));
+ EXPECT_EQ(F9, F3->getNextFrame(6));
+ EXPECT_EQ(nullptr, F3->getNextFrame(7));
+ EXPECT_EQ(nullptr, F3->getNextFrame(8));
+ EXPECT_EQ(nullptr, F3->getNextFrame(9));
+ EXPECT_EQ(nullptr, F3->getNextFrame(10));
+
+ EXPECT_EQ(F5, F4->getNextFrame(1));
+ EXPECT_EQ(F6, F4->getNextFrame(2));
+ EXPECT_EQ(F7, F4->getNextFrame(3));
+ EXPECT_EQ(F8, F4->getNextFrame(4));
+ EXPECT_EQ(F9, F4->getNextFrame(5));
+ EXPECT_EQ(nullptr, F4->getNextFrame(6));
+ EXPECT_EQ(nullptr, F4->getNextFrame(7));
+ EXPECT_EQ(nullptr, F4->getNextFrame(8));
+ EXPECT_EQ(nullptr, F4->getNextFrame(9));
+ EXPECT_EQ(nullptr, F4->getNextFrame(10));
+
+ EXPECT_EQ(F6, F5->getNextFrame(1));
+ EXPECT_EQ(F7, F5->getNextFrame(2));
+ EXPECT_EQ(F8, F5->getNextFrame(3));
+ EXPECT_EQ(F9, F5->getNextFrame(4));
+ EXPECT_EQ(nullptr, F5->getNextFrame(5));
+ EXPECT_EQ(nullptr, F5->getNextFrame(6));
+ EXPECT_EQ(nullptr, F5->getNextFrame(7));
+ EXPECT_EQ(nullptr, F5->getNextFrame(8));
+ EXPECT_EQ(nullptr, F5->getNextFrame(9));
+ EXPECT_EQ(nullptr, F5->getNextFrame(10));
+
+ EXPECT_EQ(F7, F6->getNextFrame(1));
+ EXPECT_EQ(F8, F6->getNextFrame(2));
+ EXPECT_EQ(F9, F6->getNextFrame(3));
+ EXPECT_EQ(nullptr, F6->getNextFrame(4));
+ EXPECT_EQ(nullptr, F6->getNextFrame(5));
+ EXPECT_EQ(nullptr, F6->getNextFrame(6));
+ EXPECT_EQ(nullptr, F6->getNextFrame(7));
+ EXPECT_EQ(nullptr, F6->getNextFrame(8));
+ EXPECT_EQ(nullptr, F6->getNextFrame(9));
+ EXPECT_EQ(nullptr, F6->getNextFrame(10));
+
+ EXPECT_EQ(F8, F7->getNextFrame(1));
+ EXPECT_EQ(F9, F7->getNextFrame(2));
+ EXPECT_EQ(nullptr, F7->getNextFrame(3));
+ EXPECT_EQ(nullptr, F7->getNextFrame(4));
+ EXPECT_EQ(nullptr, F7->getNextFrame(5));
+ EXPECT_EQ(nullptr, F7->getNextFrame(6));
+ EXPECT_EQ(nullptr, F7->getNextFrame(7));
+ EXPECT_EQ(nullptr, F7->getNextFrame(8));
+ EXPECT_EQ(nullptr, F7->getNextFrame(9));
+ EXPECT_EQ(nullptr, F7->getNextFrame(10));
+
+ EXPECT_EQ(F9, F8->getNextFrame(1));
+ EXPECT_EQ(nullptr, F8->getNextFrame(2));
+ EXPECT_EQ(nullptr, F8->getNextFrame(3));
+ EXPECT_EQ(nullptr, F8->getNextFrame(4));
+ EXPECT_EQ(nullptr, F8->getNextFrame(5));
+ EXPECT_EQ(nullptr, F8->getNextFrame(6));
+ EXPECT_EQ(nullptr, F8->getNextFrame(7));
+ EXPECT_EQ(nullptr, F8->getNextFrame(8));
+ EXPECT_EQ(nullptr, F8->getNextFrame(9));
+ EXPECT_EQ(nullptr, F8->getNextFrame(10));
+
+ EXPECT_EQ(nullptr, F9->getNextFrame(1));
+ EXPECT_EQ(nullptr, F9->getNextFrame(2));
+ EXPECT_EQ(nullptr, F9->getNextFrame(3));
+ EXPECT_EQ(nullptr, F9->getNextFrame(4));
+ EXPECT_EQ(nullptr, F9->getNextFrame(5));
+ EXPECT_EQ(nullptr, F9->getNextFrame(6));
+ EXPECT_EQ(nullptr, F9->getNextFrame(7));
+ EXPECT_EQ(nullptr, F9->getNextFrame(8));
+ EXPECT_EQ(nullptr, F9->getNextFrame(9));
+ EXPECT_EQ(nullptr, F9->getNextFrame(10));
// Prev frame
- ASSERT_EQ(nullptr, F0->getPreviousFrame());
- ASSERT_EQ(F0, F1->getPreviousFrame());
- ASSERT_EQ(F1, F2->getPreviousFrame());
- ASSERT_EQ(F2, F3->getPreviousFrame());
- ASSERT_EQ(F3, F4->getPreviousFrame());
- ASSERT_EQ(F4, F5->getPreviousFrame());
- ASSERT_EQ(F5, F6->getPreviousFrame());
- ASSERT_EQ(F6, F7->getPreviousFrame());
- ASSERT_EQ(F7, F8->getPreviousFrame());
- ASSERT_EQ(F8, F9->getPreviousFrame());
+ EXPECT_EQ(F8, F9->getPreviousFrame(1));
+ EXPECT_EQ(F7, F9->getPreviousFrame(2));
+ EXPECT_EQ(F6, F9->getPreviousFrame(3));
+ EXPECT_EQ(F5, F9->getPreviousFrame(4));
+ EXPECT_EQ(F4, F9->getPreviousFrame(5));
+ EXPECT_EQ(F3, F9->getPreviousFrame(6));
+ EXPECT_EQ(F2, F9->getPreviousFrame(7));
+ EXPECT_EQ(F1, F9->getPreviousFrame(8));
+ EXPECT_EQ(F0, F9->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F9->getPreviousFrame(10));
+
+ EXPECT_EQ(F7, F8->getPreviousFrame(1));
+ EXPECT_EQ(F6, F8->getPreviousFrame(2));
+ EXPECT_EQ(F5, F8->getPreviousFrame(3));
+ EXPECT_EQ(F4, F8->getPreviousFrame(4));
+ EXPECT_EQ(F3, F8->getPreviousFrame(5));
+ EXPECT_EQ(F2, F8->getPreviousFrame(6));
+ EXPECT_EQ(F1, F8->getPreviousFrame(7));
+ EXPECT_EQ(F0, F8->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F8->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F8->getPreviousFrame(10));
+
+ EXPECT_EQ(F6, F7->getPreviousFrame(1));
+ EXPECT_EQ(F5, F7->getPreviousFrame(2));
+ EXPECT_EQ(F4, F7->getPreviousFrame(3));
+ EXPECT_EQ(F3, F7->getPreviousFrame(4));
+ EXPECT_EQ(F2, F7->getPreviousFrame(5));
+ EXPECT_EQ(F1, F7->getPreviousFrame(6));
+ EXPECT_EQ(F0, F7->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(10));
+
+ EXPECT_EQ(F5, F6->getPreviousFrame(1));
+ EXPECT_EQ(F4, F6->getPreviousFrame(2));
+ EXPECT_EQ(F3, F6->getPreviousFrame(3));
+ EXPECT_EQ(F2, F6->getPreviousFrame(4));
+ EXPECT_EQ(F1, F6->getPreviousFrame(5));
+ EXPECT_EQ(F0, F6->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(10));
+
+ EXPECT_EQ(F4, F5->getPreviousFrame(1));
+ EXPECT_EQ(F3, F5->getPreviousFrame(2));
+ EXPECT_EQ(F2, F5->getPreviousFrame(3));
+ EXPECT_EQ(F1, F5->getPreviousFrame(4));
+ EXPECT_EQ(F0, F5->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(10));
+
+ EXPECT_EQ(F3, F4->getPreviousFrame(1));
+ EXPECT_EQ(F2, F4->getPreviousFrame(2));
+ EXPECT_EQ(F1, F4->getPreviousFrame(3));
+ EXPECT_EQ(F0, F4->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(10));
+
+ EXPECT_EQ(F2, F3->getPreviousFrame(1));
+ EXPECT_EQ(F1, F3->getPreviousFrame(2));
+ EXPECT_EQ(F0, F3->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(10));
+
+ EXPECT_EQ(F1, F2->getPreviousFrame(1));
+ EXPECT_EQ(F0, F2->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(10));
+
+ EXPECT_EQ(F0, F1->getPreviousFrame(1));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(10));
+
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(1));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(10));
}
#include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 225a49326c54d4e743bd7cd6c43bf839785b2e3d..04d838944dd50139da61d890c0171e0b41a33cb0 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -95,8 +95,9 @@ void show(const ProblemPtr& problem)
using std::endl;
cout << std::setprecision(4);
- for (FrameBasePtr F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
+ auto F = F_pair.second;
cout << "----- Key Frame " << F->id() << " -----" << endl;
if (!F->getCaptureList().empty())
{
@@ -324,8 +325,9 @@ TEST(Odom2d, VoteForKfAndSolve)
// Check the 3 KFs' states and covariances
MatrixXd computed_cov;
int n = 0;
- for (auto F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
+ auto F = F_pair.second;
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);
@@ -397,7 +399,6 @@ TEST(Odom2d, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data
- auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
std::cout << "t: " << t << std::endl;
for (int n=1; n<=N; n++)
@@ -405,7 +406,8 @@ TEST(Odom2d, KF_callback)
t += dt;
// re-use capture with updated timestamp
- capture->setTimeStamp(t);
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ //capture->setTimeStamp(t);
// Processor
capture->process();
@@ -442,7 +444,7 @@ TEST(Odom2d, KF_callback)
FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
// there must be 2KF and one F
- ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
+ ASSERT_EQ(problem->getTrajectory()->size(), 2);
// The last KF must have TS = 0.08
ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
@@ -450,7 +452,7 @@ TEST(Odom2d, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_2);
ASSERT_TRUE(problem->check(0));
t += dt;
- capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 80fe044c410c9b07f5e059461875a97199d1c6de..522246910cc6bd3944f6924a832fbf028cb2b75c 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -144,8 +144,9 @@ TEST(Problem, SetOrigin_PO_2d)
TrajectoryBasePtr T = P->getTrajectory();
FrameBasePtr F = P->getLastFrame();
CaptureBasePtr C = F->getCaptureList().front();
- FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
- FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ auto feature_list = C->getFeatureList();
+ FeatureBasePtr fo = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
FactorBasePtrList fac_list;
F->getFactorList(fac_list);
@@ -202,10 +203,9 @@ TEST(Problem, SetOrigin_PO_3d)
TrajectoryBasePtr T = P->getTrajectory();
FrameBasePtr F = P->getLastFrame();
CaptureBasePtr C = F->getCaptureList().front();
- // FeatureBasePtr fo = C->getFeatureList().front();
- // FeatureBasePtr fp = C->getFeatureList().back();
- FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
- FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ auto feature_list = C->getFeatureList();
+ FeatureBasePtr fo = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
FactorBasePtrList fac_list;
F->getFactorList(fac_list);
@@ -253,7 +253,7 @@ TEST(Problem, emplaceFrame_factory)
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
+ ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 3);
ASSERT_EQ(f0->getStateVector().size(), 3 );
ASSERT_EQ(f1->getStateVector().size(), 7 );
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index a5e0beabb5447222a27bd02138144997d3389a00..23c0afdf5a4bcf4ef2586d7e63b692ae4c953906 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -149,7 +149,7 @@ TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
MatrixXd J_D(7,7), J_d(7,7);
- JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS);
+ JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, 1000*Constants::EPS);
WOLF_DEBUG("J_D:\n ", J_D);
WOLF_DEBUG("J_d:\n ", J_d);
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 7a142212ad896a2fd2b6d4a930ce9b0b3e475d13..b1dbdfee8a84823106a59e699da1cd28bb532982 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -74,15 +74,17 @@ class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
{
+ auto feature_list = cap->getFeatureList();
return ftr->getCapture() == cap &&
- std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+ std::find(feature_list.begin(), feature_list.end(), ftr) != feature_list.end();
}
bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
{
+ auto factor_list = ftr->getFactorList();
return fac->getFeature() == ftr &&
- std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+ std::find(factor_list.begin(), factor_list.end(), fac) != factor_list.end();
}
@@ -249,7 +251,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
ASSERT_EQ(fac->getCaptureOther(),nullptr);
ASSERT_EQ(fac->getFeatureOther(),ftr_other);
ASSERT_EQ(fac->getLandmarkOther(),nullptr);
- ASSERT_TRUE(std::find(ftr_other->getConstrainedByList().begin(),ftr_other->getConstrainedByList().end(), fac) != ftr_other->getConstrainedByList().end());
+ ASSERT_TRUE(ftr_other->isConstrainedBy(fac));
}
TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 8017ad6049b31efec2f1efa5d5126c542429ab0a..4ed3c0846017e15fe5882607ff1807fbbbbc93f5 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -85,22 +85,25 @@ class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
{
+ auto feature_list = cap->getFeatureList();
return ftr->getCapture() == cap &&
- std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+ std::find(feature_list.begin(), feature_list.end(), ftr) != feature_list.end();
}
bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
{
+ auto factor_list = ftr->getFactorList();
return fac->getFeature() == ftr &&
- std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+ std::find(factor_list.begin(), factor_list.end(), fac) != factor_list.end();
}
bool isLandmarkLinked(LandmarkBasePtr lmk, MapBasePtr map)
{
+ auto landmark_list = map->getLandmarkList();
return lmk->getMap() == map &&
- std::find(map->getLandmarkList().begin(), map->getLandmarkList().end(), lmk) != map->getLandmarkList().end();
+ std::find(landmark_list.begin(), landmark_list.end(), lmk) != landmark_list.end();
}
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index c0004ad3992e256e7a3e5d883ebaea1707295775..8c5a2685c5155a9c5a5efbe28fb2ea26a7c899a6 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -84,8 +84,8 @@ TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
ts += 1.0;
- if (P->getTrajectory()->getFrameMap().size() > 10)
- (*P->getTrajectory()->begin())->remove();
+ if (P->getTrajectory()->size() > 10)
+ P->getTrajectory()->getFirstFrame()->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
break;
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 8c21ca410b7b9111f11994ebad9f74cfc91cfdba..099297e77a3e3a4d9e2663b078b50919d1a52d24 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -79,8 +79,8 @@ TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
ts += 1.0;
- if (P->getTrajectory()->getFrameMap().size() > 10)
- (*P->getTrajectory()->begin())->remove();
+ if (P->getTrajectory()->size() > 10)
+ P->getTrajectory()->getFirstFrame()->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
break;
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 130a3495c8469a0b4dbcecb8ebbc68b55412835c..aabfada74ff97166ff7468ffe48cd8c46db4034f 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -42,7 +42,7 @@ class TrackMatrixTest : public testing::Test
FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
- FeatureBasePtr f0, f1, f2, f3, f4;
+ FeatureBasePtr f0, f1, f2, f3, f4, f5;
ProblemPtr problem;
void SetUp() override
@@ -71,6 +71,7 @@ class TrackMatrixTest : public testing::Test
f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f5 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
F0->link(problem);
@@ -507,6 +508,37 @@ TEST_F(TrackMatrixTest, trackAtKeyframes)
ASSERT_EQ(trk_kf_1.size(), 0);
}
+TEST_F(TrackMatrixTest, trackIds)
+{
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C1);
+ f3->link(C1);
+ f4->link(C2);
+ f5->link(C2);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
+ track_matrix.newTrack(f3);
+ track_matrix.add(f0->trackId(), f4);
+ track_matrix.add(f2->trackId(), f5);
+
+ /* KC0 C1 C2
+ *
+ * f0---f1---f4 trk 0
+ * | |
+ * f2---f5 trk 1
+ * |
+ * f3 trk 2
+ */
+
+ ASSERT_EQ(track_matrix.trackIds().size(), 3);
+ ASSERT_EQ(track_matrix.trackIds(C0).size(), 1);
+ ASSERT_EQ(track_matrix.trackIds(C1).size(), 3);
+ ASSERT_EQ(track_matrix.trackIds(C2).size(), 2);
+}
+
int main(int argc, char **argv)
{