diff --git a/.clang-format b/.clang-format
index bc951c7539c78c4fa00efde0030dcf12b126686a..8f284dd854c7257bc8e4af2bb9e9a056acf81d6c 100644
--- a/.clang-format
+++ b/.clang-format
@@ -6,7 +6,7 @@ AccessModifierOffset: -2
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: true
AlignConsecutiveDeclarations: true
-AlignEscapedNewlines: Left
+AlignEscapedNewlines: Right
AlignOperands: true
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
@@ -19,7 +19,7 @@ AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
-BinPackArguments: true
+BinPackArguments: false
BinPackParameters: false
BreakBeforeBraces: Custom
BraceWrapping:
diff --git a/.gitignore b/.gitignore
index 2a19e53e62aaa9c251b2f92657392e589597f1d6..c81d93b330db1ee8455e4c073e979746edbc3728 100644
--- a/.gitignore
+++ b/.gitignore
@@ -8,6 +8,7 @@ build_debug/
build_release/
build-debug/
build-release/
+build-test/
lib/
.idea/
./Wolf.user
@@ -34,7 +35,5 @@ src/examples/map_apriltag_save.yaml
\.vscode/
.clangd
-wolfcore.found
-/wolf.found
.ccls*
compile_commands.json
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 8337cb1148e9f8669aecc9a6ab7558c5a08446b9..b6b0a3b5c9c447bf5402a403714351831a9cf342 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,3 +1,13 @@
+workflow:
+ rules:
+ - if: '$CI_PIPELINE_SOURCE == "web"'
+ - if: $CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS && $CI_PIPELINE_SOURCE == "push"
+ when: never
+ - if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
+ - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
+ when: never
+ - if: '$CI_COMMIT_BRANCH'
+
stages:
- license
- build_and_test
@@ -89,7 +99,7 @@ stages:
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
- make -j$(nproc)
- - ctest -j$(nproc)
+ - ctest -j$(nproc) --output-on-failure
# run demos
- ../bin/hello_wolf
- ../bin/hello_wolf_autoconf
@@ -126,6 +136,8 @@ deploy_imu:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
DEPLOY_CI_ROS: "false"
@@ -138,6 +150,8 @@ deploy_gnss:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
@@ -151,6 +165,8 @@ deploy_vision:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
DEPLOY_CI_ROS: "false"
@@ -163,6 +179,8 @@ deploy_laser:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
@@ -176,6 +194,8 @@ deploy_apriltag:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
@@ -189,6 +209,8 @@ deploy_bodydynamics:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
@@ -203,6 +225,8 @@ deploy_imu_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
DEPLOY_CI_ROS: "false"
@@ -215,6 +239,8 @@ deploy_gnss_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
@@ -228,6 +254,8 @@ deploy_vision_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
DEPLOY_CI_ROS: "false"
@@ -240,6 +268,8 @@ deploy_laser_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
@@ -253,6 +283,8 @@ deploy_apriltag_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
WOLF_VISION_BRANCH: main
@@ -266,6 +298,8 @@ deploy_bodydynamics_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
WOLF_IMU_BRANCH: main
@@ -286,6 +320,8 @@ deploy_wolf_ros_node:
stage: deploy_ros
rules:
- if: $CI_COMMIT_BRANCH != "main" && $DEPLOY_CI_ROS == "true"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
@@ -311,6 +347,8 @@ deploy_wolf_ros_node_main:
stage: deploy_ros
rules:
- if: $CI_COMMIT_BRANCH == "main" && $DEPLOY_CI_ROS == "true"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_IMU_BRANCH: main
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e69a3926a1a15adc529a45d8c37c7aa3a5437ff5..897854374187e8d3fb517b1ec1bf5f849c719f51 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,7 +16,7 @@ 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(LIB_INSTALL_DIR lib)
-SET(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+SET(INCLUDE_INSTALL_DIR include/wolf)
# Build type
IF (NOT CMAKE_BUILD_TYPE)
@@ -33,7 +33,7 @@ include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
if(COMPILER_SUPPORTS_CXX14)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+ set(CMAKE_CXX_STANDARD 14)
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
@@ -49,6 +49,10 @@ IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
+IF(NOT BUILD_GMOCK)
+ OPTION(BUILD_GMOCK "Build GMock" OFF)
+ENDIF(NOT BUILD_GMOCK)
+
IF(NOT BUILD_DEMOS)
OPTION(BUILD_DEMOS "Build Demos" ON)
ENDIF(NOT BUILD_DEMOS)
@@ -80,6 +84,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
FIND_PACKAGE(Threads REQUIRED MODULE)
FIND_PACKAGE(Ceres REQUIRED CONFIG)
FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
+FIND_PACKAGE(spdlog REQUIRED CONFIG)
if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
endif()
@@ -89,7 +94,7 @@ FIND_PACKAGE(yaml-cpp REQUIRED CONFIG)
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)
+SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
# Create the specified output directory if it does not exist.
IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
@@ -100,168 +105,162 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
"${WOLF_CONFIG_DIR} exists, but is not a directory.")
ENDIF()
+
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message(STATUS "WOLF CONFIG DIRECTORY ${WOLF_CONFIG_DIR}")
-message(STATUS "WOLF CONFIG FILE ${WOLF_CONFIG_DIR}/config.h")
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-# include spdlog (logging library)
-FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
-IF (SPDLOG_INCLUDE_DIR)
- # INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
- MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
-ELSE (SPDLOG_INCLUDE_DIR)
- MESSAGE(FATAL_ERROR "Could not find spdlog")
-ENDIF (SPDLOG_INCLUDE_DIR)
-
-# ============ INCLUDES ============
-INCLUDE_DIRECTORIES("include") # In this same project
# ============ HEADERS ============
SET(HDRS_CAPTURE
- include/core/capture/capture_base.h
- include/core/capture/capture_motion.h
- include/core/capture/capture_odom_2d.h
- include/core/capture/capture_odom_3d.h
- include/core/capture/capture_pose.h
- include/core/capture/capture_void.h
- include/core/capture/capture_diff_drive.h
+ include/${PROJECT_NAME}/capture/capture_base.h
+ include/${PROJECT_NAME}/capture/capture_diff_drive.h
+ include/${PROJECT_NAME}/capture/capture_landmarks_external.h
+ include/${PROJECT_NAME}/capture/capture_motion.h
+ include/${PROJECT_NAME}/capture/capture_odom_2d.h
+ include/${PROJECT_NAME}/capture/capture_odom_3d.h
+ include/${PROJECT_NAME}/capture/capture_pose.h
+ include/${PROJECT_NAME}/capture/capture_void.h
)
SET(HDRS_COMMON
- include/core/common/factory.h
- include/core/common/node_base.h
- include/core/common/time_stamp.h
- include/core/common/wolf.h
- include/core/common/params_base.h
+ include/${PROJECT_NAME}/common/factory.h
+ include/${PROJECT_NAME}/common/node_base.h
+ include/${PROJECT_NAME}/common/time_stamp.h
+ include/${PROJECT_NAME}/common/wolf.h
+ include/${PROJECT_NAME}/common/params_base.h
)
SET(HDRS_FACTOR
- include/core/factor/factor_analytic.h
- include/core/factor/factor_autodiff.h
- include/core/factor/factor_base.h
- include/core/factor/factor_block_absolute.h
- include/core/factor/factor_block_difference.h
- include/core/factor/factor_diff_drive.h
- include/core/factor/factor_distance_3d.h
- include/core/factor/factor_pose_2d.h
- include/core/factor/factor_pose_3d.h
- include/core/factor/factor_quaternion_absolute.h
- include/core/factor/factor_relative_pose_2d.h
- include/core/factor/factor_relative_pose_2d_with_extrinsics.h
- include/core/factor/factor_relative_pose_3d.h
- include/core/factor/factor_pose_3d_with_extrinsics.h
- include/core/factor/factor_relative_pose_3d_with_extrinsics.h
- include/core/factor/factor_velocity_local_direction_3d.h
+ include/${PROJECT_NAME}/factor/factor_analytic.h
+ include/${PROJECT_NAME}/factor/factor_autodiff.h
+ include/${PROJECT_NAME}/factor/factor_base.h
+ include/${PROJECT_NAME}/factor/factor_block_absolute.h
+ include/${PROJECT_NAME}/factor/factor_block_difference.h
+ include/${PROJECT_NAME}/factor/factor_diff_drive.h
+ include/${PROJECT_NAME}/factor/factor_distance_3d.h
+ include/${PROJECT_NAME}/factor/factor_pose_2d.h
+ include/${PROJECT_NAME}/factor/factor_pose_3d.h
+ include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
+ include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
+ include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
+ include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
+ include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
+ include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_2d.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_2d_with_extrinsics.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_3d.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_3d_with_extrinsics.h
+ include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
- include/core/feature/feature_base.h
- include/core/feature/feature_diff_drive.h
- include/core/feature/feature_match.h
- include/core/feature/feature_motion.h
- include/core/feature/feature_odom_2d.h
- include/core/feature/feature_pose.h
+ include/${PROJECT_NAME}/feature/feature_base.h
+ include/${PROJECT_NAME}/feature/feature_diff_drive.h
+ include/${PROJECT_NAME}/feature/feature_match.h
+ include/${PROJECT_NAME}/feature/feature_motion.h
+ include/${PROJECT_NAME}/feature/feature_odom_2d.h
+ include/${PROJECT_NAME}/feature/feature_pose.h
)
SET(HDRS_FRAME
- include/core/frame/frame_base.h
+ include/${PROJECT_NAME}/frame/frame_base.h
)
SET(HDRS_HARDWARE
- include/core/hardware/hardware_base.h
+ include/${PROJECT_NAME}/hardware/hardware_base.h
)
SET(HDRS_LANDMARK
- include/core/landmark/landmark_base.h
- include/core/landmark/landmark_match.h
+ include/${PROJECT_NAME}/landmark/landmark_base.h
+ include/${PROJECT_NAME}/landmark/landmark_match.h
)
SET(HDRS_MATH
- include/core/math/SE2.h
- include/core/math/SE3.h
- include/core/math/rotations.h
- include/core/math/covariance.h
+ include/${PROJECT_NAME}/math/SE2.h
+ include/${PROJECT_NAME}/math/SE3.h
+ include/${PROJECT_NAME}/math/rotations.h
+ include/${PROJECT_NAME}/math/covariance.h
)
SET(HDRS_MAP
- include/core/map/factory_map.h
- include/core/map/map_base.h
+ include/${PROJECT_NAME}/map/factory_map.h
+ include/${PROJECT_NAME}/map/map_base.h
)
SET(HDRS_PROBLEM
- include/core/problem/problem.h
+ include/${PROJECT_NAME}/problem/problem.h
)
SET(HDRS_PROCESSOR
- include/core/processor/motion_buffer.h
- include/core/processor/motion_provider.h
- include/core/processor/processor_base.h
- include/core/processor/processor_diff_drive.h
- include/core/processor/processor_fixed_wing_model.h
- include/core/processor/factory_processor.h
- include/core/processor/processor_loop_closure.h
- include/core/processor/processor_motion.h
- include/core/processor/processor_odom_2d.h
- include/core/processor/processor_odom_3d.h
- include/core/processor/processor_pose.h
- include/core/processor/processor_tracker.h
- include/core/processor/processor_tracker_feature.h
- include/core/processor/processor_tracker_landmark.h
- include/core/processor/track_matrix.h
+ include/${PROJECT_NAME}/processor/motion_buffer.h
+ include/${PROJECT_NAME}/processor/motion_provider.h
+ include/${PROJECT_NAME}/processor/processor_base.h
+ include/${PROJECT_NAME}/processor/processor_diff_drive.h
+ include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
+ include/${PROJECT_NAME}/processor/factory_processor.h
+ include/${PROJECT_NAME}/processor/processor_landmark_external.h
+ include/${PROJECT_NAME}/processor/processor_loop_closure.h
+ include/${PROJECT_NAME}/processor/processor_motion.h
+ include/${PROJECT_NAME}/processor/processor_odom_2d.h
+ include/${PROJECT_NAME}/processor/processor_odom_3d.h
+ include/${PROJECT_NAME}/processor/processor_pose.h
+ include/${PROJECT_NAME}/processor/processor_tracker.h
+ include/${PROJECT_NAME}/processor/processor_tracker_feature.h
+ include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
+ include/${PROJECT_NAME}/processor/track_matrix.h
)
SET(HDRS_SENSOR
- include/core/sensor/sensor_base.h
- include/core/sensor/sensor_diff_drive.h
- include/core/sensor/factory_sensor.h
- include/core/sensor/sensor_motion_model.h
- include/core/sensor/sensor_odom_2d.h
- include/core/sensor/sensor_odom_3d.h
- include/core/sensor/sensor_pose.h
+ include/${PROJECT_NAME}/sensor/sensor_base.h
+ include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
+ include/${PROJECT_NAME}/sensor/factory_sensor.h
+ include/${PROJECT_NAME}/sensor/sensor_motion_model.h
+ include/${PROJECT_NAME}/sensor/sensor_odom_2d.h
+ include/${PROJECT_NAME}/sensor/sensor_odom_3d.h
+ include/${PROJECT_NAME}/sensor/sensor_pose.h
)
SET(HDRS_SOLVER
- include/core/solver/solver_manager.h
- include/core/solver/factory_solver.h
+ include/${PROJECT_NAME}/solver/solver_manager.h
+ include/${PROJECT_NAME}/solver/factory_solver.h
)
SET(HDRS_STATE_BLOCK
- include/core/state_block/factory_state_block.h
- include/core/state_block/has_state_blocks.h
- include/core/state_block/local_parametrization_angle.h
- include/core/state_block/local_parametrization_base.h
- include/core/state_block/local_parametrization_homogeneous.h
- include/core/state_block/local_parametrization_quaternion.h
- include/core/state_block/state_angle.h
- include/core/state_block/state_block.h
- include/core/state_block/state_composite.h
- include/core/state_block/state_homogeneous_3d.h
- include/core/state_block/state_quaternion.h
+ include/${PROJECT_NAME}/state_block/factory_state_block.h
+ include/${PROJECT_NAME}/state_block/has_state_blocks.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_angle.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_base.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_homogeneous.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_quaternion.h
+ include/${PROJECT_NAME}/state_block/state_angle.h
+ include/${PROJECT_NAME}/state_block/state_block.h
+ include/${PROJECT_NAME}/state_block/state_block_derived.h
+ include/${PROJECT_NAME}/state_block/state_composite.h
+ include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
+ include/${PROJECT_NAME}/state_block/state_quaternion.h
)
SET(HDRS_TRAJECTORY
- include/core/trajectory/trajectory_base.h
+ include/${PROJECT_NAME}/trajectory/trajectory_base.h
)
SET(HDRS_TREE_MANAGER
- include/core/tree_manager/factory_tree_manager.h
- include/core/tree_manager/tree_manager_base.h
- include/core/tree_manager/tree_manager_sliding_window.h
- include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
+ include/${PROJECT_NAME}/tree_manager/factory_tree_manager.h
+ include/${PROJECT_NAME}/tree_manager/tree_manager_base.h
+ include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window.h
+ include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window_dual_rate.h
)
SET(HDRS_UTILS
- include/core/utils/check_log.h
- include/core/utils/converter.h
- include/core/utils/eigen_assert.h
- include/core/utils/graph_search.h
- include/core/utils/loader.h
- include/core/utils/logging.h
- include/core/utils/params_server.h
- include/core/utils/singleton.h
- include/core/utils/utils_gtest.h
- include/core/utils/converter_utils.h
+ include/${PROJECT_NAME}/utils/check_log.h
+ include/${PROJECT_NAME}/utils/converter.h
+ include/${PROJECT_NAME}/utils/eigen_assert.h
+ include/${PROJECT_NAME}/utils/graph_search.h
+ include/${PROJECT_NAME}/utils/loader.h
+ include/${PROJECT_NAME}/utils/logging.h
+ include/${PROJECT_NAME}/utils/params_server.h
+ include/${PROJECT_NAME}/utils/singleton.h
+ include/${PROJECT_NAME}/utils/utils_gtest.h
+ include/${PROJECT_NAME}/utils/converter_utils.h
)
SET(HDRS_YAML
- include/core/yaml/parser_yaml.h
- include/core/yaml/yaml_conversion.h
+ include/${PROJECT_NAME}/yaml/parser_yaml.h
+ include/${PROJECT_NAME}/yaml/yaml_conversion.h
)
# ============ SOURCES ============
SET(SRCS_CAPTURE
src/capture/capture_base.cpp
+ src/capture/capture_diff_drive.cpp
+ src/capture/capture_landmarks_external.cpp
src/capture/capture_motion.cpp
src/capture/capture_odom_2d.cpp
src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
src/capture/capture_void.cpp
- src/capture/capture_diff_drive.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
@@ -299,6 +298,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
src/processor/processor_fixed_wing_model.cpp
+ src/processor/processor_landmark_external.cpp
src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
@@ -326,6 +326,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp
+ src/state_block/state_block_derived.cpp
src/state_block/state_composite.cpp
)
SET(SRCS_TRAJECTORY
@@ -354,13 +355,13 @@ SET(SRCS_YAML
IF (Ceres_FOUND)
SET(HDRS_CERES_WRAPPER
#ceres_wrapper/qr_manager.h
- include/core/ceres_wrapper/cost_function_wrapper.h
- include/core/ceres_wrapper/create_numeric_diff_cost_function.h
- include/core/ceres_wrapper/local_parametrization_wrapper.h
- include/core/ceres_wrapper/iteration_update_callback.h
- include/core/ceres_wrapper/solver_ceres.h
- include/core/solver/solver_manager.h
- include/core/solver_suitesparse/sparse_utils.h
+ include/${PROJECT_NAME}/ceres_wrapper/cost_function_wrapper.h
+ include/${PROJECT_NAME}/ceres_wrapper/create_numeric_diff_cost_function.h
+ include/${PROJECT_NAME}/ceres_wrapper/local_parametrization_wrapper.h
+ include/${PROJECT_NAME}/ceres_wrapper/iteration_update_callback.h
+ include/${PROJECT_NAME}/ceres_wrapper/solver_ceres.h
+ include/${PROJECT_NAME}/solver/solver_manager.h
+ include/${PROJECT_NAME}/solver_suitesparse/sparse_utils.h
)
SET(SRCS_CERES_WRAPPER
#ceres_wrapper/qr_manager.cpp
@@ -413,6 +414,7 @@ endif()
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)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC spdlog::spdlog)
IF (Ceres_FOUND)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ceres)
ENDIF(Ceres_FOUND)
@@ -457,9 +459,9 @@ install(
${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
+target_include_directories(${PLUGIN_NAME} PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/conf>
$<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
)
@@ -493,7 +495,7 @@ INSTALL(FILES ${HDRS_SENSOR}
INSTALL(FILES ${HDRS_SOLVER}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver)
#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION ${INCLUDE_INSTALL_DIR}/core/solver_suitesparse)
+# DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver_suitesparse)
INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
INSTALL(FILES ${HDRS_TRAJECTORY}
@@ -505,12 +507,9 @@ INSTALL(FILES ${HDRS_UTILS}
INSTALL(FILES ${HDRS_YAML}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/yaml)
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
+INSTALL(FILES ${WOLF_CONFIG_DIR}/config.h
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
-
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen MODULE)
diff --git a/Testing/Temporary/.gitignore b/Testing/Temporary/.gitignore
deleted file mode 100644
index 6ccdef7c09c93207bc46508bb9fd8c873e94e6c9..0000000000000000000000000000000000000000
--- a/Testing/Temporary/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-/CTestCostData.txt
-/LastTest.log
diff --git a/cmake_modules/wolfcoreConfig.cmake.in b/cmake_modules/wolfcoreConfig.cmake.in
index 735adef5984b0989b98947873a15e22a910172b4..749bc679d1abdfe403c3bfd3bb4115d5c2c19600 100644
--- a/cmake_modules/wolfcoreConfig.cmake.in
+++ b/cmake_modules/wolfcoreConfig.cmake.in
@@ -8,6 +8,7 @@ FIND_DEPENDENCY(Threads REQUIRED)
FIND_DEPENDENCY(Ceres REQUIRED)
FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
FIND_DEPENDENCY(yaml-cpp REQUIRED)
+FIND_DEPENDENCY(spdlog REQUIRED)
include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 29a27f5d9c1662a6e6ab7bd4c9d7c02515476c4e..707bb45b02d2880cf461bd161d3f3137f9897737 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -80,7 +80,7 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
- SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
// Ceres wrapper
SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);
@@ -147,8 +147,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
// store
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index bc8982ca2ce696719aab35105f059639d6c72f23..ed612ffcad7f3e1299f8e02d434c61969705b718 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -84,7 +84,7 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
- SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(Eigen::VectorXd::Zero(1)), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
Eigen::SparseMatrix<double> Lambda(0,0);
@@ -151,8 +151,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
diff --git a/demos/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp
index c2c11187eedd016c49116f84ee14141067d7fa47..2eee6e3b8d43638a010b7959d700b7d1b4fa05a0 100644
--- a/demos/hello_wolf/landmark_point_2d.cpp
+++ b/demos/hello_wolf/landmark_point_2d.cpp
@@ -27,12 +27,13 @@
*/
#include "landmark_point_2d.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
- LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
+ LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
{
setId(_id);
}
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 7073082e34be71b45e60b7671211e1617432a124..affd707e29a7eacadee870b584ff586920049b17 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -28,6 +28,7 @@
#include "sensor_range_bearing.h"
#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
@@ -36,7 +37,7 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing);
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
SensorBase("SensorRangeBearing",
- std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
+ std::make_shared<StatePoint2d>(_extrinsics.head<2>(), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
nullptr,
_noise_std)
diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp
index b5f159c731741dabe4ed00bbe8eaa0e7968be7f3..56df88821b3441b9e1526f713741ffbfc49acedc 100644
--- a/demos/solver/test_SPQR.cpp
+++ b/demos/solver/test_SPQR.cpp
@@ -27,8 +27,8 @@
*/
#include <iostream>
-#include <eigen3/Eigen/SPQRSupport>
-#include <eigen3/Eigen/CholmodSupport>
+#include <Eigen/SPQRSupport>
+#include <Eigen/CholmodSupport>
#include "SuiteSparseQR.hpp"
using namespace Eigen;
diff --git a/demos/solver/test_ccolamd.cpp b/demos/solver/test_ccolamd.cpp
index 98f496f26b5de691a38384d42b3349d62fc8b20e..126fa7f869f203188a3c589bf68fced8f4e593ff 100644
--- a/demos/solver/test_ccolamd.cpp
+++ b/demos/solver/test_ccolamd.cpp
@@ -43,9 +43,9 @@
#include "solver/ccolamd_ordering.h"
// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
using namespace Eigen;
using namespace wolf;
diff --git a/demos/solver/test_ccolamd_blocks.cpp b/demos/solver/test_ccolamd_blocks.cpp
index 14bb174286235f063a375cca4e28f1e4dfeca537..2d82633432d7620f108b8ef8532c4d08f0a17b9f 100644
--- a/demos/solver/test_ccolamd_blocks.cpp
+++ b/demos/solver/test_ccolamd_blocks.cpp
@@ -37,9 +37,9 @@
#include <queue>
// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
// ccolamd
#include "solver/ccolamd_ordering.h"
diff --git a/demos/solver/test_iQR.cpp b/demos/solver/test_iQR.cpp
index 53a37cef1a62ec1ffb281a6fdbd741fc73cb59f8..ba4ae194a00c6e9c8a78affc1a54fc02b1f52bb1 100644
--- a/demos/solver/test_iQR.cpp
+++ b/demos/solver/test_iQR.cpp
@@ -44,9 +44,9 @@
#include <queue>
// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
-#include <eigen3/Eigen/SPQRSupport>
+#include <Eigen/OrderingMethods>
+#include <Eigen/SparseQR>
+#include <Eigen/SPQRSupport>
// ccolamd
#include "solver/ccolamd_ordering.h"
diff --git a/demos/solver/test_iQR_wolf.cpp b/demos/solver/test_iQR_wolf.cpp
index a8df237237669b54aa837d32f35bf8c156fa33eb..4ffc4a0bffada8742a20dd9d00b220c7544be156 100644
--- a/demos/solver/test_iQR_wolf.cpp
+++ b/demos/solver/test_iQR_wolf.cpp
@@ -38,8 +38,8 @@
#include <queue>
// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
+#include <Eigen/OrderingMethods>
+#include <Eigen/SparseQR>
#include <Eigen/SPQRSupport>
// ccolamd
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index e634dac6f2de62d79296525dd6ca2631b4041108..d85e2cdebeacd0bbc1d39220cacd562f018e291b 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -181,10 +181,10 @@ int main(int argc, char *argv[])
Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(std::make_shared<StateBlock>(gps_pose.head(2)), std::make_shared<StateBlock>(gps_pose.tail(1)), gps_std);
- SensorLaser2D laser_1_sensor(std::make_shared<StateBlock>(laser_1_pose.head(2)), std::make_shared<StateBlock>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- SensorLaser2D laser_2_sensor(std::make_shared<StateBlock>(laser_2_pose.head(2)), std::make_shared<StateBlock>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
+ SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)), std::make_shared<StateAngle>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std);
+ SensorLaser2D laser_1_sensor(std::make_shared<StatePoint2d>(laser_1_pose.head(2)), std::make_shared<StateAngle>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
+ SensorLaser2D laser_2_sensor(std::make_shared<StatePoint2d>(laser_2_pose.head(2)), std::make_shared<StateAngle>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
// Initial pose
pose_odom << 2, 8, 0;
diff --git a/demos/solver/test_incremental_ccolamd_blocks.cpp b/demos/solver/test_incremental_ccolamd_blocks.cpp
index 740407a9a560f0efdbdb8267ee7925c22fbf12bf..f0eabf66cde8d920265f3af6a5b4e8a994fa5858 100644
--- a/demos/solver/test_incremental_ccolamd_blocks.cpp
+++ b/demos/solver/test_incremental_ccolamd_blocks.cpp
@@ -37,9 +37,9 @@
#include <queue>
// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
// ccolamd
#include "solver/ccolamd_ordering.h"
diff --git a/demos/solver/test_permutations.cpp b/demos/solver/test_permutations.cpp
index 0765d8ae77996a72b027a62380804243aa0aedfa..6724ea83f167bbacbfceed245a01b7c5cfdeea60 100644
--- a/demos/solver/test_permutations.cpp
+++ b/demos/solver/test_permutations.cpp
@@ -37,7 +37,7 @@
#include <queue>
// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
+#include <Eigen/OrderingMethods>
using namespace Eigen;
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
index f0009526c6f9fcb8a61ae84916690a62190e32c6..29dd647045c69da17bfd41a11bd4d047216b2583 100644
--- a/doc/doxygen.conf
+++ b/doc/doxygen.conf
@@ -757,7 +757,8 @@ WARN_LOGFILE =
INPUT = ../doc/main.dox \
../src \
- ../include/core
+ ../include/core \
+ conf/core/internal
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..27af193e1f73de46ec3f934c0f89b0ff0f2a9750
--- /dev/null
+++ b/include/core/capture/capture_landmarks_external.h
@@ -0,0 +1,61 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+//Wolf includes
+#include "core/capture/capture_base.h"
+
+namespace wolf {
+
+struct LandmarkDetection
+{
+ int id; // id of landmark
+ Eigen::VectorXd measure; // either pose or position
+ Eigen::MatrixXd covariance; // covariance of the measure
+ double quality; // [0, 1] quality of the detection
+};
+
+WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
+
+//class CaptureLandmarksExternal
+class CaptureLandmarksExternal : public CaptureBase
+{
+ protected:
+ std::vector<LandmarkDetection> detections_;
+
+ public:
+
+ CaptureLandmarksExternal(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const std::vector<int>& _ids = {},
+ const std::vector<Eigen::VectorXd>& _detections = {},
+ const std::vector<Eigen::MatrixXd>& _covs = {},
+ const std::vector<double>& _qualities = {});
+
+ ~CaptureLandmarksExternal() override;
+
+ std::vector<LandmarkDetection> getDetections() const {return detections_;};
+
+ void addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& quality);
+};
+
+} //namespace wolf
\ No newline at end of file
diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h
index e0d70f26f95c2d5dfd10131e48103586c52fd20d..132a4aba706160bb30e1f3099f3b546dbe2f4bda 100644
--- a/include/core/ceres_wrapper/sparse_utils.h
+++ b/include/core/ceres_wrapper/sparse_utils.h
@@ -30,7 +30,7 @@
#define SPARSE_UTILS_H_
// eigen includes
-//#include <eigen3/Eigen/Sparse>
+//#include <Eigen/Sparse>
namespace wolf
{
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 790ec306644c12e607a4657859bedc8e360284aa..03be899855188f350530c1e721a68c275121e6b1 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -305,6 +305,7 @@ class Factory
public:
static bool registerCreator(const std::string& _type, CreatorCallback createFn);
static bool unregisterCreator(const std::string& _type);
+ static bool isCreatorRegistered(const std::string& _type);
static TypeBasePtr create(const std::string& _type, TypeInput... _input);
std::string getClass() const;
static void printAddress();
@@ -356,6 +357,12 @@ inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string
return get().callbacks_.erase(_type) == 1;
}
+template<class TypeBase, typename... TypeInput>
+inline bool Factory<TypeBase, TypeInput...>::isCreatorRegistered(const std::string& _type)
+{
+ return get().callbacks_.count(_type) == 1;
+}
+
template<class TypeBase, typename... TypeInput>
inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
{
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 1894148de2ea576614b14d6de81a4f138c4e037c..0ca91c9c6bb5db6577e268e9f159da0f09a89a4e 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -123,6 +123,31 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
return topology_;
}
+ std::string getTopologyString() const
+ {
+ switch (topology_)
+ {
+ case TOP_GEOM:
+ return "GEOM";
+ break;
+ case TOP_ABS:
+ return "ABS";
+ break;
+ case TOP_LMK:
+ return "LMK";
+ break;
+ case TOP_LOOP:
+ return "LOOP";
+ break;
+ case TOP_OTHER:
+ return "OTHER";
+ break;
+ default:
+ return "UNDEFINED";
+ }
+ }
+
+
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;
diff --git a/include/core/factor/factor_relative_pose_2d.h b/include/core/factor/factor_relative_pose_2d.h
index 4a608feb01c29b413b19608987767853eecf5407..d156213cd912cb8b82bef159804e184cac5018f6 100644
--- a/include/core/factor/factor_relative_pose_2d.h
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -142,11 +142,11 @@ class FactorRelativePose2d : public FactorAnalytic
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
- const std::vector<bool>& _compute_jacobian) const override;
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::MatrixXd>& jacobians,
- const std::vector<bool>& _compute_jacobian) const override;
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
@@ -175,8 +175,8 @@ inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
- const std::vector<bool>& _compute_jacobian) const
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
@@ -211,8 +211,8 @@ inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::MatrixXd>& jacobians,
- const std::vector<bool>& _compute_jacobian) const
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index b0eea0f2b8f91f00a9a2bc8a7bda6ce1be66dc45..f0f70a5c15341c48f03e1b36ce86eb6e1e1415db 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -65,6 +65,8 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
+
+ Eigen::Vector3d residual() const;
};
inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
@@ -132,31 +134,49 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
- T o_ref = _o_ref[0];
- T o_target = _o_target[0];
- T o_sensor = _o_sensor[0];
+ const T& o_ref = _o_ref[0];
+ const T& o_target = _o_target[0];
+ const T& o_sensor = _o_sensor[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
- // Expected measurement
- // r1_p_r1_s1 = ps
- // r2_p_r2_s2 = ps
- // r1_R_s1 = R(os)
- // r2_R_s2 = R(os)
- // w_R_r1 = R(o1)
- // w_R_r2 = R(o2)
- // ----------------------------------------
-
- // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
- // s1_R_r1*r1_R_w*w_p_r1_w +
- // s1_R_r1*r1_R_w*w_p_w_r2 +
- // s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
-
- // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
-
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
- expected_measurement(2) = o_target - o_ref;
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ // Expected measurement
+ // r1_p_s1 = p_sensor
+ // r2_p_s2 = p_sensor
+ // r1_R_s1 = R(o_sensor)
+ // r2_R_s2 = R(o_sensor)
+ // w_p_r1 = p_ref
+ // w_p_r2 = p_target
+ // w_R_r1 = R(o_ref)
+ // w_R_r2 = R(o_target)
+ // ----------------------------------------
+ // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 + w_R_r2*r2_p_s2 - w_p_r1) - r1_p_s1)
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
+ expected_measurement(2) = o_target - o_ref;
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ // Expected measurement
+ // r1_p_s1 = p_sensor
+ // r2_p_s2 = p_sensor
+ // r1_R_s1 = R(o_sensor)
+ // r2_R_s2 = R(o_sensor)
+ // w_p_r1 = p_ref
+ // w_p_r2 = p_target
+ // w_R_r1 = R(o_ref)
+ // w_R_r2 = R(o_target)
+ // ----------------------------------------
+ // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1)
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target));
+ expected_measurement(2) = o_target - o_ref - o_sensor;
+ }
// Error
er = expected_measurement - getMeasurement().cast<T>();
@@ -186,6 +206,34 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
return true;
}
+inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const
+{
+ Eigen::Vector3d res;
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
+ p_sensor = getCapture()->getSensorP()->getState();
+ o_sensor = getCapture()->getSensorO()->getState();
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ p_ref = getFrameOther()->getP()->getState();
+ o_ref = getFrameOther()->getO()->getState();
+ p_target = getCapture()->getFrame()->getP()->getState();
+ o_target = getCapture()->getFrame()->getO()->getState();
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ p_ref = getCapture()->getFrame()->getP()->getState();
+ o_ref = getCapture()->getFrame()->getO()->getState();
+ p_target = getLandmarkOther()->getP()->getState();
+ o_target = getLandmarkOther()->getO()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
+
+ return res;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index 6003c446732399f1abe6c4127dfb257543c82c2c..4ea300d5c13d35aed9ff1c60ba57c822cd4386c6 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/*
- * factor_relative_pose_3d.h
- *
- * Created on: Oct 7, 2016
- * Author: jsola
- */
#ifndef FACTOR_RELATIVE_POSE_3D_H_
#define FACTOR_RELATIVE_POSE_3D_H_
@@ -48,24 +42,31 @@ class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
+ FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+
~FactorRelativePose3d() override = default;
template<typename T>
- bool operator ()(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
+ bool operator ()(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
T* _residuals) const;
template<typename T>
- bool expectation(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
+ bool expectation(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const;
- Eigen::VectorXd expectation() const;
+ Eigen::Vector7d expectation() const;
private:
template<typename T>
@@ -118,39 +119,64 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
+inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ nullptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ _lmk_ptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _lmk_ptr->getP(), // landmark P
+ _lmk_ptr->getO(), // landmark Q
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO()) // current frame Q
+{
+ MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
template<typename T>
-inline bool FactorRelativePose3d::expectation(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
- T* _expectation_dp,
- T* _expectation_dq) const
+inline bool FactorRelativePose3d::expectation(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
+ T* _expectation_dp,
+ T* _expectation_dq) const
{
- Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
- Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
- Eigen::Map<const Eigen::Matrix<T,3,1> > p_past(_p_past);
- Eigen::Map<const Eigen::Quaternion<T> > q_past(_q_past);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Quaternion<T> > q_target(_q_target);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
-// std::cout << "p_current: " << p_current(0) << " "
-// << p_current(1) << " "
-// << p_current(2) << "\n";
-// std::cout << "q_current: " << q_current.coeffs()(0) << " "
-// << q_current.coeffs()(1) << " "
-// << q_current.coeffs()(2) << " "
-// << q_current.coeffs()(3) << "\n";
-// std::cout << "p_past: " << p_past(0) << " "
-// << p_past(1) << " "
-// << p_past(2) << "\n";
-// std::cout << "q_past: " << q_past.coeffs()(0) << " "
-// << q_past.coeffs()(1) << " "
-// << q_past.coeffs()(2) << " "
-// << q_past.coeffs()(3) << "\n";
+// std::cout << "p_target: " << p_target(0) << " "
+// << p_target(1) << " "
+// << p_target(2) << "\n";
+// std::cout << "q_target: " << q_target.coeffs()(0) << " "
+// << q_target.coeffs()(1) << " "
+// << q_target.coeffs()(2) << " "
+// << q_target.coeffs()(3) << "\n";
+// std::cout << "p_ref: " << p_ref(0) << " "
+// << p_ref(1) << " "
+// << p_ref(2) << "\n";
+// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
+// << q_ref.coeffs()(1) << " "
+// << q_ref.coeffs()(2) << " "
+// << q_ref.coeffs()(3) << "\n";
// estimate motion increment, dp, dq;
- expectation_dp = q_past.conjugate() * (p_current - p_past);
- expectation_dq = q_past.conjugate() * q_current;
+ expectation_dp = q_ref.conjugate() * (p_target - p_ref);
+ expectation_dq = q_ref.conjugate() * q_target;
// std::cout << "exp_p: " << expectation_dp(0) << " "
// << expectation_dp(1) << " "
@@ -163,34 +189,38 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_current,
return true;
}
-inline Eigen::VectorXd FactorRelativePose3d::expectation() const
+inline Eigen::Vector7d FactorRelativePose3d::expectation() const
{
- Eigen::VectorXd exp(7);
+ Eigen::Vector7d exp;
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();
- const Eigen::VectorXd frame_past_ori = frm_past->getO()->getState();
-
-// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
-// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
-
- expectation(frame_current_pos.data(),
- frame_current_ori.data(),
- frame_past_pos.data(),
- frame_past_ori.data(),
+ auto lmk = getLandmarkOther();
+
+ const Eigen::VectorXd target_pos = ( frm_past ? frm_current->getP()->getState() : lmk->getP()->getState());
+ const Eigen::VectorXd target_ori = ( frm_past ? frm_current->getO()->getState() : lmk->getO()->getState());
+ const Eigen::VectorXd ref_pos = ( frm_past ? frm_past->getP()->getState() : frm_current->getP()->getState());
+ const Eigen::VectorXd ref_ori = ( frm_past ? frm_past->getO()->getState() : frm_current->getO()->getState());
+
+// std::cout << "target_pos: " << target_pos.transpose() << std::endl;
+// std::cout << "target_ori: " << target_ori.coeffs().transpose() << std::endl;
+// std::cout << "ref_pos: " << ref_pos.transpose() << std::endl;
+// std::cout << "ref_ori: " << ref_ori.coeffs().transpose() << std::endl;
+
+ expectation(target_pos.data(),
+ target_ori.data(),
+ ref_pos.data(),
+ ref_ori.data(),
exp.data(),
exp.data()+3);
return exp;
}
template<typename T>
-inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
- T* _residuals) const
+inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
+ T* _residuals) const
{
/* Residual expression
@@ -232,7 +262,7 @@ inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
Eigen::Map<Eigen::Matrix<T,6,1> > residuals(_residuals);
Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7) ;
- expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
+ expectation(_p_target, _q_target, _p_ref, _q_ref, expected.data(), expected.data()+3);
// measured motion increment, dp_m, dq_m
// Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
index 19245f6bcf630f737e4a01aa78b4a11030d8264a..66468bbcafb2b4480df5077ca6b2a16d6ff61839 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -58,7 +58,7 @@ class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativeP
/** \brief Class Destructor
*/
- ~FactorRelativePose3dWithExtrinsics() override;
+ ~FactorRelativePose3dWithExtrinsics() override = default;
/** brief : compute the residual from the state blocks being iterated by the solver.
**/
@@ -116,6 +116,8 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
_feature_ptr->getCapture()->getSensorP(),
_feature_ptr->getCapture()->getSensorO())
{
+ assert(_feature_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_feature_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
}
inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
@@ -143,45 +145,59 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
{
}
-inline FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
-{
- //
-}
-
template<typename T>
inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
- const T* const _o_ref,
- const T* const _p_target,
- const T* const _o_target,
- const T* const _p_sensor,
- const T* const _o_sensor,
- T* _residuals) const
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _o_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const
{
// Maps
- Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_sensor);
- Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_sensor);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
+ Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
- Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_target);
- Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_target);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_t(_o_target);
Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
+ // WOLF_INFO("p_sensor: ", p_r_s.transpose());
+ // WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
+ // WOLF_INFO("p_ref: ", p_w_r.transpose());
+ // WOLF_INFO("o_ref: ", q_w_r.coeffs().transpose());
+ // WOLF_INFO("p_target: ", p_w_t.transpose());
+ // WOLF_INFO("o_target: ", q_w_t.coeffs().transpose());
+
// Expected measurement
- Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate();
- Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l;
- Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l);
+ Eigen::Quaternion<T> expected_q;
+ Eigen::Matrix<T,3,1> expected_p;
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t + q_w_t * p_r_s - p_w_r) - p_r_s);
+ expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t * q_r_s;
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
+ expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t;
+ }
// Measurement
- Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>());
+ Eigen::Vector3d p_meas(getMeasurement().head<3>());
Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 );
Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>();
//Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
// Error
Eigen::Matrix<T, 6, 1> err;
- err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - p_c_l);
- //err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l); // true error function for which the covariance should be computed
- err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
+ // err.head(3) = q_l_c_meas * (p_meas.cast<T>() - expected_p);
+ err.head(3) = (p_meas - expected_p);
+ err.tail(3) = wolf::log_q(q_l_c_meas * expected_q); // true error function for which the covariance should be computed
+ //err.tail(3) = T(2)*(q_l_c_meas * expected_q).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
@@ -192,15 +208,27 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
{
Eigen::Vector6d res;
- double * p_camera, * o_camera, * p_ref, * o_ref, * p_target, * o_target;
- p_camera = getCapture()->getSensorP()->getState().data();
- o_camera = getCapture()->getSensorO()->getState().data();
- p_ref = getCapture()->getFrame()->getP()->getState().data();
- o_ref = getCapture()->getFrame()->getO()->getState().data();
- p_target = getLandmarkOther()->getP()->getState().data();
- o_target = getLandmarkOther()->getO()->getState().data();
-
- operator() (p_camera, o_camera, p_ref, o_ref, p_target, o_target, res.data());
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
+ p_sensor = getCapture()->getSensorP()->getState();
+ o_sensor = getCapture()->getSensorO()->getState();
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ p_ref = getFrameOther()->getP()->getState();
+ o_ref = getFrameOther()->getO()->getState();
+ p_target = getCapture()->getFrame()->getP()->getState();
+ o_target = getCapture()->getFrame()->getO()->getState();
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ p_ref = getCapture()->getFrame()->getP()->getState();
+ o_ref = getCapture()->getFrame()->getO()->getState();
+ p_target = getLandmarkOther()->getP()->getState();
+ o_target = getLandmarkOther()->getO()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
diff --git a/include/core/factor/factor_relative_position_2d.h b/include/core/factor/factor_relative_position_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8a2c7a0e52a7b58f8cda42586c56abff31793e79
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d.h
@@ -0,0 +1,258 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_analytic.h"
+#include "core/landmark/landmark_base.h"
+#include "core/math/rotations.h"
+#include <Eigen/StdVector>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition2d);
+
+//class
+class FactorRelativePosition2d : public FactorAnalytic
+{
+ public:
+
+ /** \brief Constructor of category FAC_FRAME
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP())
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_FEATURE
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ _ftr_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_other_ptr->getFrame()->getP() )
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_LANDMARK
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP())
+ {
+ //
+ }
+
+ ~FactorRelativePosition2d() override = default;
+
+ /** \brief Returns the factor residual size
+ **/
+ unsigned int getSize() const override
+ {
+ return 2;
+ }
+
+ /** \brief Returns the residual evaluated in the states provided
+ *
+ * Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
+ **/
+ Eigen::VectorXd evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
+
+ /** \brief Returns the jacobians evaluated in the states provided
+ *
+ * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
+ * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+ *
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+ *
+ **/
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+
+ /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ *
+ **/
+ void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
+
+};
+
+/// IMPLEMENTATION ///
+
+inline Eigen::VectorXd FactorRelativePosition2d::evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
+{
+ Eigen::VectorXd residual(2);
+ Eigen::VectorXd expected_measurement(2);
+ // Expected measurement
+ Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
+ expected_measurement = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
+ // Residual
+ residual = expected_measurement - getMeasurement();
+ residual = getMeasurementSquareRootInformationUpper() * residual;
+ return residual;
+}
+
+inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 3);
+ assert(_st_vector.size() == 3);
+ assert(_compute_jacobian.size() == 3);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
+ (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
+ (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
+ }
+}
+
+inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 3);
+ assert(_st_vector.size() == 3);
+ assert(_compute_jacobian.size() == 3);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
+ (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
+ (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
+ }
+}
+
+inline void FactorRelativePosition2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+{
+ assert(jacobians.size() == 3);
+
+ jacobians[0] = (Eigen::MatrixXd(2,2) <<
+ -cos(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
+
+ jacobians[1] = (Eigen::MatrixXd(2,1) <<
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+ + (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
+ - (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0))).finished();
+
+ jacobians[2] = (Eigen::MatrixXd(2,2) <<
+ cos(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_2d_with_extrinsics.h b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd36867f4a93e08fe671f0491a07276ed00b6624
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
@@ -0,0 +1,128 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition2dWithExtrinsics);
+
+//class
+class FactorRelativePosition2dWithExtrinsics : public FactorAutodiff<FactorRelativePosition2dWithExtrinsics, 2, 2, 1, 2, 2, 1>
+{
+ public:
+
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top = TOP_LMK,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorRelativePosition2dWithExtrinsics() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const;
+};
+
+inline FactorRelativePosition2dWithExtrinsics::FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, _lmk_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_other_ptr->getP(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+{
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+}
+
+template<typename T>
+inline bool FactorRelativePosition2dWithExtrinsics::operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const
+{
+ // MAPS
+ Eigen::Map<Eigen::Matrix<T,2,1> > res(_residuals);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
+ T o_ref = _o_ref[0];
+ T o_sensor = _o_sensor[0];
+
+ // Expected measurement
+ Eigen::Matrix<T, 2, 1> expected_measurement = Eigen::Rotation2D<T>(-o_sensor) *
+ (-p_sensor + Eigen::Rotation2D<T>(-o_ref) * (p_target - p_ref));
+
+ // Residuals
+ res = getMeasurementSquareRootInformationUpper() * (expected_measurement - getMeasurement());
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXs J(3,6);
+// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
+// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
+// if (sizeof(er(0)) != sizeof(double))
+// {
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_3d.h b/include/core/factor/factor_relative_position_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d76dcb3406059f37218d0ace894ec1b3f9bc474
--- /dev/null
+++ b/include/core/factor/factor_relative_position_3d.h
@@ -0,0 +1,154 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#pragma once
+
+#include "core/factor/factor_autodiff.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition3d);
+
+//class
+class FactorRelativePosition3d : public FactorAutodiff<FactorRelativePosition3d,3,3,4,3>
+{
+ public:
+ FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+
+ FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorRelativePosition3d() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p_ref,
+ const T* const _q_ref,
+ const T* const _p_target,
+ T* _residuals) const;
+
+ private:
+ template<typename T>
+ void printRes(const Eigen::Matrix<T, 3, 1>& r) const;
+};
+
+template<typename T>
+inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<T, 3, 1>& r) const
+{
+ std::cout << "Jet residual = " << std::endl;
+ std::cout << r(0) << std::endl;
+ std::cout << r(1) << std::endl;
+ std::cout << r(2) << std::endl;
+}
+
+template<>
+inline void FactorRelativePosition3d::printRes (const Eigen::Matrix<double,3,1> & r) const
+{
+ std::cout << "Scalar residual = " << std::endl;
+ std::cout << r.transpose() << std::endl;
+}
+
+inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ _frame_past_ptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _frame_past_ptr->getP(), // past frame P
+ _frame_past_ptr->getO(), // past frame Q
+ _ftr_current_ptr->getFrame()->getP()) // current frame P
+{
+ MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
+inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ nullptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ _lmk_ptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(), // frame P
+ _ftr_current_ptr->getFrame()->getO(), // frame Q
+ _lmk_ptr->getP()) // landmark P
+{
+ MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
+template<typename T>
+inline bool FactorRelativePosition3d::operator ()(const T* const _p_ref,
+ const T* const _q_ref,
+ const T* const _p_target,
+ T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
+ Eigen::Map<Eigen::Matrix<T,3,1> > residuals(_residuals);
+
+// std::cout << "p_ref: " << p_ref(0) << " "
+// << p_ref(1) << " "
+// << p_ref(2) << "\n";
+// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
+// << q_ref.coeffs()(1) << " "
+// << q_ref.coeffs()(2) << " "
+// << q_ref.coeffs()(3) << "\n";
+// std::cout << "p_target: " << p_target(0) << " "
+// << p_target(1) << " "
+// << p_target(2) << "\n";
+
+ residuals = getMeasurementSquareRootInformationUpper() * (getMeasurement() - (q_ref.conjugate() * (p_target - p_ref)));
+
+ return true;
+}
+
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_3d_with_extrinsics.h b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..f892ac25a4adcb6c31f43f76b4fc3022a62b56ae
--- /dev/null
+++ b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
@@ -0,0 +1,149 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition3dWithExtrinsics);
+
+//class
+class FactorRelativePosition3dWithExtrinsics : public FactorAutodiff<FactorRelativePosition3dWithExtrinsics, 3, 3, 4, 3, 3, 4>
+{
+ public:
+
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top = TOP_LMK,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorRelativePosition3dWithExtrinsics() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const;
+
+ Eigen::Vector3d residual() const;
+ double cost() const;
+};
+
+inline FactorRelativePosition3dWithExtrinsics::FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition3dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, _lmk_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_other_ptr->getP(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+{
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+}
+
+template<typename T>
+inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const
+{
+ // Maps
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
+ Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
+ Eigen::Map<Eigen::Matrix<T,3,1>> residuals(_residuals);
+
+ // WOLF_INFO("p_sensor: ", p_r_s.transpose());
+ // WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
+ // WOLF_INFO("p_ref: ", p_w_r.transpose());
+ // WOLF_INFO("o_ref: ", q_w_r.coeffs().transpose());
+ // WOLF_INFO("p_target: ", p_w_t.transpose());
+
+ // Expected measurement
+ Eigen::Matrix<T,3,1> expected_p;
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
+
+ // Residual
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (getMeasurement() - expected_p);
+
+ return true;
+}
+
+inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const
+{
+ Eigen::Vector3d res;
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target;
+ p_sensor = getCapture()->getSensorP()->getState();
+ o_sensor = getCapture()->getSensorO()->getState();
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ p_ref = getFrameOther()->getP()->getState();
+ o_ref = getFrameOther()->getO()->getState();
+ p_target = getCapture()->getFrame()->getP()->getState();
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ p_ref = getCapture()->getFrame()->getP()->getState();
+ o_ref = getCapture()->getFrame()->getO()->getState();
+ p_target = getLandmarkOther()->getP()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data());
+
+ return res;
+}
+
+inline double FactorRelativePosition3dWithExtrinsics::cost() const
+{
+ return residual().squaredNorm();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 8d64663585af0982d7ac3209a3fcb808c753080a..0f0271a1b12733b5e37dce59061af22c497e7daf 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -139,17 +139,17 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
bool isConstrainedBy(const FactorBaseConstPtr& _factor) const;
template <class C>
- CaptureBaseConstPtr getCaptureOfType() const;
+ std::shared_ptr<const C> getCaptureOfType() const;
template <class C>
- CaptureBasePtr getCaptureOfType();
+ std::shared_ptr<C> getCaptureOfType();
CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
CaptureBasePtr getCaptureOfType(const std::string& type);
template <class C>
- CaptureBaseConstPtrList getCapturesOfType() const;
+ std::list<std::shared_ptr<const C>> getCapturesOfType() const;
template <class C>
- CaptureBasePtrList getCapturesOfType();
+ std::list<std::shared_ptr<C>> getCapturesOfType();
CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
CaptureBasePtrList getCapturesOfType(const std::string& type);
@@ -290,40 +290,52 @@ inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
}
template <class C>
-inline CaptureBaseConstPtr FrameBase::getCaptureOfType() const
+inline std::shared_ptr<const C> FrameBase::getCaptureOfType() const
{
for (auto capture_ptr : getCaptureList())
- if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
- return capture_ptr;
+ {
+ auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
+ if (cap_derived)
+ return cap_derived;
+ }
return nullptr;
}
template <class C>
-inline CaptureBasePtr FrameBase::getCaptureOfType()
+inline std::shared_ptr<C> FrameBase::getCaptureOfType()
{
for (auto capture_ptr : getCaptureList())
- if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
- return capture_ptr;
+ {
+ auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
+ if (cap_derived)
+ return cap_derived;
+ }
return nullptr;
}
template <class C>
-inline CaptureBaseConstPtrList FrameBase::getCapturesOfType() const
+inline std::list<std::shared_ptr<const C>> FrameBase::getCapturesOfType() const
{
- CaptureBaseConstPtrList captures;
+ std::list<std::shared_ptr<const C>> captures;
for (auto capture_ptr : getCaptureList())
- if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
- captures.push_back(capture_ptr);
+ {
+ auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
+ if (cap_derived)
+ captures.push_back(cap_derived);
+ }
return captures;
}
template <class C>
-inline CaptureBasePtrList FrameBase::getCapturesOfType()
+inline std::list<std::shared_ptr<C>> FrameBase::getCapturesOfType()
{
- CaptureBasePtrList captures;
+ std::list<std::shared_ptr<C>> captures;
for (auto capture_ptr : getCaptureList())
- if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
- captures.push_back(capture_ptr);
+ {
+ auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
+ if (cap_derived)
+ captures.push_back(cap_derived);
+ }
return captures;
}
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index af09f86002c697ad026ad9942d218026808555c4..da7113fa583bd7838bda5ee9a96c610731814dcd 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -113,7 +113,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
LandmarkBaseConstPtrList getLandmarkList() const;
LandmarkBasePtrList getLandmarkList();
-
+ LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
+ LandmarkBasePtr getLandmark(const unsigned int& _id);
+
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");
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 95631a8a6fb82ac22595b64d3e1d5b4fe7849cb4..39f67c0358fc1f3c127adc1ec96844d005011588 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -320,6 +320,13 @@ inline void compose(const VectorComposite& _x1,
_c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
}
+inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
+{
+ VectorComposite c("PO", {2,1});
+ compose(x1, x2, c);
+ return c;
+}
+
inline void compose(const VectorComposite& _x1,
const VectorComposite& _x2,
VectorComposite& _c,
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 9eb2e5fd9b1c644742a7d32d055591231f10c7aa..7917935549619fbc58cc917e834d5ae735d90d41 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -108,7 +108,7 @@ inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
MatrixSizeCheck<3, 1>::check(p);
MatrixSizeCheck<3, 1>::check(ip);
- ip = - q.conjugate() * p ;
+ ip = -(q.conjugate() * p) ;
iq = q.conjugate() ;
}
@@ -135,6 +135,21 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
return id;
}
+inline void inverse(const VectorComposite& v, VectorComposite& c)
+{
+ Map<const Quaternion<double> > qv( & v.at('O')(0) );
+ Map<Quaternion<double> > qc( & c['O'](0) );
+ inverse(v.at('P'), qv, c['P'], qc);
+}
+
+inline VectorComposite inverse(const VectorComposite& v)
+{
+ VectorComposite c("PO", {3,4});
+ inverse(v, c);
+ return c;
+}
+
+
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
@@ -231,6 +246,13 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector
compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
}
+inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
+{
+ VectorComposite c("PO", {3,4});
+ compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
+ return c;
+}
+
inline void compose(const VectorComposite& x1,
const VectorComposite& x2,
VectorComposite& c,
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 9dfb7cae3e62adac06662e9414c75764045148e2..e9134b5593e30d93aa514f49eecb597759b0cf18 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -86,7 +86,11 @@ class Problem : public std::enable_shared_from_this<Problem>
StateStructure frame_structure_;
PriorOptionsPtr prior_options_;
- private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
+ std::atomic_bool transformed_;
+ VectorComposite transformation_;
+ mutable std::mutex mut_transform_;
+
+ private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
@@ -362,8 +366,16 @@ class Problem : public std::enable_shared_from_this<Problem>
// All branches -------------------------------------------
- // perturb states
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
void perturb(double amplitude = 0.01);
+ // transform states
+ void transform(const VectorComposite& _transformation);
+ bool isTransformed() const;
+ void resetTransformed();
+ VectorComposite getTransformation() const;
// Covariances
void clearCovariance();
@@ -423,7 +435,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S, T:F, M:L ; 2: H:S:p, T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
- * \param constr_by: show factors pointing to F, f and L.
+ * \param constr_by : show factors pointing to F, C, f and L.
* \param metric : show metric info (status, time stamps, state vectors, measurements)
* \param state_blocks : show state blocks
*/
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 74f01419ea00bfd01842364b5f79e46df02b525f..fbd3261c29afceed19fbc21666e79dd996b945aa 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -37,6 +37,7 @@ struct ParamsProcessorBase;
// wolf
#include "core/common/factory.h"
+#include "core/utils/params_server.h"
// std
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index 4763d0bc45d513611d34775754898da9290b7aec..7e0a1203e454c7b2ed02c9e056499ddb4900aee7 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -100,7 +100,7 @@ class MotionBuffer : public std::list<Motion>
const Motion& getMotion(const TimeStamp& _ts) const;
void getMotion(const TimeStamp& _ts, Motion& _motion) const;
void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
- void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0);
+ void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0, bool show_covs = 0);
};
} // namespace wolf
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index d7bd26d00df45c7f14d0ff5ae01966e271af03ef..c2602142571f2f8ba7283c5ac9ecea12f98f187e 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -86,8 +86,12 @@ class MotionProvider
protected:
StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
- VectorComposite odometry_;
ParamsMotionProviderPtr params_motion_provider_;
+
+ private:
+ VectorComposite odometry_;
+ mutable std::mutex mut_odometry_;
+
};
inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) :
@@ -97,11 +101,6 @@ inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMo
//
}
-inline wolf::VectorComposite MotionProvider::getOdometry ( ) const
-{
- return odometry_;
-}
-
}
///// IMPLEMENTATION ///////
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 20ed2330334aea821c7ca8bc6510f7438c555063..462e5ee22a47f00f874f861dcd4d0ddcd37d8c17 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -63,30 +63,30 @@ class ProcessorDiffDrive : public ProcessorOdom2d
protected:
// Motion integration
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
- ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
- double radians_per_tick_;
+ ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
+ double radians_per_tick_;
};
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c574a6e9a3a0a8c94254329dfaaa766be5f8db4
--- /dev/null
+++ b/include/core/processor/processor_landmark_external.h
@@ -0,0 +1,159 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#pragma once
+
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker.h"
+#include "core/processor/track_matrix.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
+
+struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
+{
+ bool use_orientation; ///< use orientation measure or not when emplacing factors
+ double match_dist_th; ///< for considering tracked detection: distance threshold to previous detection
+ unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin (sufficient condition if more than min_features_for_keyframe)
+ double filter_quality_th; ///< min quality to consider the detection
+ unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
+ double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than min_features_for_keyframe)
+
+ ParamsProcessorLandmarkExternal() = default;
+ ParamsProcessorLandmarkExternal(std::string _unique_name,
+ const wolf::ParamsServer & _server):
+ ParamsProcessorTracker(_unique_name, _server)
+ {
+ use_orientation = _server.getParam<bool> (prefix + _unique_name + "/use_orientation");
+ filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter/quality_th");
+ filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter/track_length_th");
+ match_dist_th = _server.getParam<double> (prefix + _unique_name + "/match_dist_th");
+ time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
+
+//Class
+class ProcessorLandmarkExternal : public ProcessorTracker
+{
+ public:
+ ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
+ ~ProcessorLandmarkExternal() override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
+
+ void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ ParamsProcessorLandmarkExternalPtr params_tfle_;
+ TrackMatrix track_matrix_;
+ std::set<SizeStd> lmks_ids_origin_;
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
+ */
+ void preProcess() override;
+
+
+ /** \brief Process known Features
+ * \return The number of successful matches.
+ *
+ * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in incoming
+ */
+ unsigned int processKnown() override;
+
+ /**\brief Process new Features
+ *
+ */
+ unsigned int processNew(const int& _max_features) override { return 0;};
+
+ /** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+ bool voteForKeyFrame() const override;
+
+ /** \brief Emplaces a new factor for each known feature in Capture \b last
+ */
+ void establishFactors() override;
+
+ /** \brief Emplaces a new factor
+ * \param _feature feature pointer
+ * \param _landmark pointer to the landmark
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
+
+ /** \brief Emplaces a landmark or modifies (if needed) a landmark
+ * \param _feature_ptr pointer to the Feature
+ */
+ LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+
+ /** \brief Modifies (if needed) a landmark
+ * \param _landmark pointer to the landmark
+ * \param _feature pointer to the Feature.
+ */
+ void modifyLandmark(LandmarkBasePtr _landmark,
+ FeatureBasePtr _feature);
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * It does nothing for now
+ */
+ void postProcess() override {};
+
+ void advanceDerived() override;
+ void resetDerived() override;
+
+ double detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const;
+};
+
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
+ ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
+ params_tfle_(_params_tfle),
+ lmks_ids_origin_()
+{
+ //
+}
+
+inline ProcessorLandmarkExternal::~ProcessorLandmarkExternal()
+{
+ //
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 7c0cf9b2c776be5808c853c54c5d5c3f0cca5ff7..c286d247e34676cbc687fff1ac8db2872b068980 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -30,7 +30,7 @@
#define PROCESSOR_MOTION_H_
// Wolf
-#include <core/processor/motion_provider.h>
+#include "core/processor/motion_provider.h"
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
#include "core/common/time_stamp.h"
@@ -167,12 +167,13 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep ;
- protected:
+ protected:
ParamsProcessorMotionPtr params_motion_;
- ProcessingStep processing_step_; ///< State machine controlling the processing step
+ ProcessingStep processing_step_; ///< State machine controlling the processing step
+ bool bootstrapping_; ///< processor is bootstrapping
- // This is the main public interface
- public:
+ // This is the main public interface
+ public:
ProcessorMotion(const std::string& _type,
std::string _state_structure,
int _dim,
@@ -270,6 +271,10 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
bool voteForKeyFrame() const override;
double updateDt();
+ /** \brief Make one step of motion integration
+ *
+ * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
+ */
void integrateOneStep();
void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
@@ -289,6 +294,11 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
*/
virtual void preProcess(){ };
+ /**
+ * @brief Bootstrap the processor with some initialization steps
+ */
+ virtual void bootstrap(){};
+
/** Post-process
*
* This is called by process() after finishing the processing algorithm.
@@ -471,16 +481,17 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) = 0;
- /** \brief emplace a feature corresponding to given capture and add the feature to this capture
- * \param _capture_motion: the parent capture
- */
- virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) = 0;
- /** \brief emplace a factor and link it in the wolf tree
- * \param _feature_motion: the parent feature
- * \param _frame_origin: the frame constrained by this motion factor
+ /** \brief emplace the features and factors corresponding to given capture and link them to the capture
+ * \param _capture_own: the parent capture
+ * \param _capture_origin: the capture constrained by this motion factor
+ *
+ * Typical factors to add for a ProcessorMotionDerived can be:
+ * - A preintegrated motion factor -- this is the main factor
+ * - A calibration drift factor -- only for dynamic sensor calibration parameters
+ * - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot models
*/
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+ virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0;
virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index fea39c4d6808a7a798a83d7b8b99dfbb47abec9d..1612a4bf5ce75ff875b09017e9624d60d8ae6a3c 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -71,44 +71,42 @@ class ProcessorOdom2d : public ProcessorMotion
bool voteForKeyFrame() const override;
protected:
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
-
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin_ptr) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom2dPtr params_odom_2d_;
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 02cdbd5ef4480ccd1a0ba28ab0db5c44886fa58c..fca115c8358bdc22bd60013d1f7f3fbc9f1828d2 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -90,47 +90,45 @@ class ProcessorOdom3d : public ProcessorMotion
public:
// Motion integration
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
-
- bool voteForKeyFrame() const override;
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
-
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
-
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
+ bool voteForKeyFrame() const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom3dPtr params_odom_3d_;
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index bbccccf950aa02f499d215a21f5488a724330435..ab109ffdbf4302cfeb517be5e184ae3499d77fa2 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -88,7 +88,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
* - createLandmark() : creates a Landmark using a new Feature <=== IMPLEMENT
* - findLandmarks() : find the new Landmarks again in \b incoming <=== IMPLEMENT
* - establishFactors() : which calls the pure virtual:
- * - createFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
+ * - emplaceFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
*
* Should you need extra functionality for your derived types, you can overload these two methods,
*
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 8e6c6db7c6657037e7df0eddeea9b683635e5b71..a8572e7d66887ea60d6d38ed427f268c748662c2 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -58,15 +58,10 @@ static
SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \
{ \
Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \
- \
assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
- \
auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
- \
auto sensor = std::make_shared<SensorClass>(extrinsics, params); \
- \
sensor ->setName(_unique_name); \
- \
return sensor; \
} \
\
@@ -74,13 +69,9 @@ static
SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\
{ \
assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
- \
auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \
- \
auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \
- \
sensor ->setName(_unique_name); \
- \
return sensor; \
} \
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index e653fe458d39107c143da729a006c727df3959e3..1fbf559364af15d85a74f8b53a13f0c74355b188 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -39,8 +39,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
struct ParamsSensorPose : public ParamsSensorBase
{
- double std_p = 1; // m
- double std_o = 1; // rad
+ double std_px = 1; // m
+ double std_py = 1; // m
+ double std_pz = 1; // m
+ double std_ox = 1; // rad
+ double std_oy = 1; // rad
+ double std_oz = 1; // rad
ParamsSensorPose()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
@@ -48,14 +52,56 @@ struct ParamsSensorPose : public ParamsSensorBase
ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- std_p = _server.getParam<double>(prefix + _unique_name + "/std_p");
- std_o = _server.getParam<double>(prefix + _unique_name + "/std_o");
+ /* Here we allow isotropic specs for position and orientation
+ * and also particularizations for eaxh axis
+ * depending on how the info was ptovided in the YAML file.
+ * Combinations are possible.
+ *
+ * E.g. if the yaml file contains
+ *
+ * std_p : 1
+ *
+ * then the result is std_px = 1, std_py = 1, std_pz = 1
+ *
+ * If the yaml file contains (in any order)
+ *
+ * std_p : 1
+ * std_pz : 0.1
+ *
+ * then the result is std_px = 1, std_py = 1, std_pz = 0.1
+ */
+
+ // first the isotropic options
+ if (_server.hasParam(prefix + _unique_name + "/std_p"))
+ std_px = std_py = std_pz = _server.getParam<double>(prefix + _unique_name + "/std_p");
+ if (_server.hasParam(prefix + _unique_name + "/std_o"))
+ std_ox = std_oy = std_oz = _server.getParam<double>(prefix + _unique_name + "/std_o");
+
+ // then individual axes specifications
+ if (_server.hasParam(prefix + _unique_name + "/std_px"))
+ std_px = _server.getParam<double>(prefix + _unique_name + "/std_px");
+ if (_server.hasParam(prefix + _unique_name + "/std_py"))
+ std_py = _server.getParam<double>(prefix + _unique_name + "/std_py");
+ if (_server.hasParam(prefix + _unique_name + "/std_pz"))
+ std_pz = _server.getParam<double>(prefix + _unique_name + "/std_pz");
+
+ if (_server.hasParam(prefix + _unique_name + "/std_ox"))
+ std_ox = _server.getParam<double>(prefix + _unique_name + "/std_ox");
+ if (_server.hasParam(prefix + _unique_name + "/std_oy"))
+ std_oy = _server.getParam<double>(prefix + _unique_name + "/std_oy");
+ if (_server.hasParam(prefix + _unique_name + "/std_oz"))
+ std_oz = _server.getParam<double>(prefix + _unique_name + "/std_oz");
+
}
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
- + "std_p: " + std::to_string(std_p) + "\n"
- + "std_o: " + std::to_string(std_o) + "\n";
+ + "std_px: " + std::to_string(std_px) + "\n"
+ + "std_py: " + std::to_string(std_py) + "\n"
+ + "std_pz: " + std::to_string(std_pz) + "\n"
+ + "std_ox: " + std::to_string(std_ox) + "\n";
+ + "std_oy: " + std::to_string(std_oy) + "\n";
+ + "std_oz: " + std::to_string(std_oz) + "\n";
}
~ParamsSensorPose() override = default;
};
@@ -65,8 +111,7 @@ WOLF_PTR_TYPEDEFS(SensorPose);
class SensorPose : public SensorBase
{
protected:
- double std_p_; // standard deviation of translation measurements
- double std_o_; // standard deviation of orientation measurements
+ ParamsSensorPosePtr params_pose_;
public:
SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
@@ -75,20 +120,20 @@ class SensorPose : public SensorBase
~SensorPose() override;
- double getStdP() const;
- double getStdO() const;
+ // double getStdP() const;
+ // double getStdO() const;
};
-inline double SensorPose::getStdP() const
-{
- return std_p_;
-}
+// inline double SensorPose::getStdP() const
+// {
+// return std_p_;
+// }
-inline double SensorPose::getStdO() const
-{
- return std_o_;
-}
+// inline double SensorPose::getStdO() const
+// {
+// return std_o_;
+// }
// inline Matrix6d SensorPose::computeDataCov() const
// {
diff --git a/include/core/solver_suitesparse/ccolamd_ordering.h b/include/core/solver_suitesparse/ccolamd_ordering.h
index 4683373559f7df440b834c01c7552628e4f4b7b1..38ec767eff5d4d4e21afee0050f26b840b769c76 100644
--- a/include/core/solver_suitesparse/ccolamd_ordering.h
+++ b/include/core/solver_suitesparse/ccolamd_ordering.h
@@ -33,9 +33,9 @@
#include <iostream>
// Eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
// ccolamd
#include "ccolamd.h"
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index 83c392894a0a63b06c9b86a8d8b31907a2803bf9..c5fc858227b6da93dbcfe8424a1bed48319f5228 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -46,8 +46,8 @@
#include "solver/cost_function_sparse.h"
// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
+#include <Eigen/OrderingMethods>
+#include <Eigen/SparseQR>
#include <Eigen/StdVector>
#include "core/factor/factor_pose_2d.h"
diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h
index 393a2fb275507aa1e9b5b6c67172a6b589be4f9f..e9bc9f76e8a0de8a88f4b7c6f3a4fe202e2398a0 100644
--- a/include/core/solver_suitesparse/sparse_utils.h
+++ b/include/core/solver_suitesparse/sparse_utils.h
@@ -30,7 +30,7 @@
#define SPARSE_UTILS_H_
// eigen includes
-#include <eigen3/Eigen/Sparse>
+#include <Eigen/Sparse>
namespace wolf
{
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index c440584c0539625d93b4fa4bc5bbafaa1cad1461..5c90da7b1c297d89b9db00364008e696e7316cfa 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -136,30 +136,19 @@ inline std::string FactoryStateBlock::getClass() const
return "FactoryStateBlock";
}
-template<>
-inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen::VectorXd& _state, bool _fixed)
-{
- typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
-
- if (creator_callback_it == get().callbacks_.end())
- // not found: return StateBlock base
- return std::make_shared<StateBlock>(_state, _fixed);
-
- // Invoke the creation function
- return (creator_callback_it->second)(_state, _fixed);
-}
-
-#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
- namespace{ const bool WOLF_UNUSED StateBlockType##Registered = \
- FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); } \
-
-#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
- namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
- FactoryStateBlock::registerCreator(#Key, StateBlockType::create); } \
-
-
-
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED StateBlockType##Registered = \
+ FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \
+ }
+#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
+ FactoryStateBlock::registerCreator(#Key, StateBlockType::create); \
+ }
}
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ac82297f5e36b5130151bd1b0464a110264b5fe7..f11047c17b751f327ec6c01177194170b0d39f4d 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -40,6 +40,8 @@ namespace wolf
class HasStateBlocks
{
+ friend Problem;
+
public:
HasStateBlocks();
HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
@@ -80,10 +82,6 @@ class HasStateBlocks
template<typename SB, typename ... Args>
std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
- // Emplace base state blocks.
- template<typename ... Args>
- StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
-
// Register/remove state blocks to/from wolf::Problem
virtual void registerNewStateBlocks(ProblemPtr _problem);
virtual void removeStateBlocks(ProblemPtr _problem);
@@ -99,12 +97,17 @@ class HasStateBlocks
unsigned int getSize(const StateStructure& _structure="") const;
unsigned int getLocalSize(const StateStructure& _structure="") const;
- // Perturb state
- void perturb(double amplitude = 0.01);
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
+ void perturb(double amplitude = 0.01);
- protected:
- void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+ protected:
+ // transform state
+ void transform(const VectorComposite& _transformation);
+ void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
private:
StateStructure structure_;
@@ -182,17 +185,6 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_typ
return sb;
}
-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 && 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);
-
- return sb;
-}
-
inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 3cc093665df69b32ed10d2d49921fdacedea7871..30a1c4dc3c5b16d0a5ddd238d7f3569e5abc8987 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -38,19 +38,28 @@ namespace wolf
class StateAngle : public StateBlock
{
public:
- StateAngle(double _angle = 0.0, bool _fixed = false);
+ StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true);
+ StateAngle(const Vector1d& _angle, bool _fixed = false, bool _transformable = true);
~StateAngle() override;
+ virtual void transform(const VectorComposite& _transformation) override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateAngle::StateAngle(double _angle, bool _fixed) :
- StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>())
+inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
+ StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
state_(0) = pi2pi(_angle);
}
+inline StateAngle::StateAngle(const Vector1d& _angle, bool _fixed, bool _transformable) :
+ StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
+{
+ state_(0) = pi2pi(_angle(0));
+}
+
inline StateAngle::~StateAngle()
{
//
@@ -62,6 +71,11 @@ inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fi
return std::make_shared<StateAngle>(_state(0), _fixed);
}
+inline void StateAngle::transform(const VectorComposite& _transformation)
+{
+ if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
+}
+
} /* namespace wolf */
#endif /* STATE_ANGLE_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 8f4b0468b8d9fdc519e11137a8dc491330ec3ac7..aa78dff969a29e4f5d3d583643c6e46a32980430 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -31,6 +31,7 @@ class LocalParametrizationBase;
//Wolf includes
#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
//std includes
#include <iostream>
@@ -65,6 +66,8 @@ public:
std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not
std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not
+ bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame
+
public:
/** \brief Constructor from size
*
@@ -72,7 +75,7 @@ public:
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
*/
- StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+ StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
/** \brief Constructor from vector
*
@@ -80,7 +83,7 @@ public:
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
**/
- StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+ StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
///< Explicitly not copyable/movable
StateBlock(const StateBlock& o) = delete;
@@ -173,19 +176,29 @@ public:
virtual Eigen::VectorXd identity() const;
Eigen::VectorXd zero() const;
- /** \brief perturb state
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.1);
+ bool isTransformable() const {
+ return transformable_;
+ }
+
+ void setTransformable(bool _trf = true) {transformable_ = _trf;}
+
+ void setNonTransformable() {transformable_ = false;}
+
+ virtual void transform(const VectorComposite& _transformation) = 0;
void plus(const Eigen::VectorXd& _dv);
bool isValid(double tolerance = Constants::EPS);
- static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
-
};
+
} // namespace wolf
// IMPLEMENTATION
@@ -195,31 +208,30 @@ public:
namespace wolf {
-inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-// notifications_{Notification::ADD},
+inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
fixed_(_fixed),
state_size_(_state.size()),
state_(_state),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
- local_param_updated_(false)
+ local_param_updated_(false),
+ transformable_(_transformable)
{
// std::cout << "constructed +sb" << std::endl;
}
-inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-// notifications_{Notification::ADD},
+inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
fixed_(_fixed),
state_size_(_size),
state_(Eigen::VectorXd::Zero(_size)),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
- local_param_updated_(false)
+ local_param_updated_(false),
+ transformable_(_transformable)
{
- //
-// std::cout << "constructed +sb" << std::endl;
+ // std::cout << "constructed +sb" << std::endl;
}
inline StateBlock::~StateBlock()
@@ -338,10 +350,6 @@ inline Eigen::VectorXd StateBlock::zero() const
return identity();
}
-inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fixed)
-{
- return std::make_shared<StateBlock>(_state, _fixed);
-}
inline bool StateBlock::isValid(double tolerance)
{
return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d1377520098ae65e5a43b361f48d6061266de37
--- /dev/null
+++ b/include/core/state_block/state_block_derived.h
@@ -0,0 +1,160 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef STATE_BLOCK_DERIVED_H_
+#define STATE_BLOCK_DERIVED_H_
+
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+
+/**
+ * @brief State block for general parameters
+ *
+ * This state block cannot be transformed with a geometric frame transformation.
+ *
+ * This state block cannot have a local parametrization
+ *
+ * @tparam size the size of the state block's vector
+ */
+template <size_t size>
+class StateParams : public StateBlock
+{
+ public:
+ StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false) {}
+ void transform(const VectorComposite& _transformation) override
+ {
+ // non transformable! --> do nothing
+ }
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
+};
+
+typedef StateParams<1> StateParams1;
+typedef StateParams<2> StateParams2;
+typedef StateParams<3> StateParams3;
+typedef StateParams<4> StateParams4;
+typedef StateParams<5> StateParams5;
+typedef StateParams<6> StateParams6;
+typedef StateParams<7> StateParams7;
+typedef StateParams<8> StateParams8;
+typedef StateParams<9> StateParams9;
+typedef StateParams<10> StateParams10;
+
+/**
+ * @brief State block for general 2D points and positions
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ *
+ * This state block cannot have a local parametrization
+ */
+class StatePoint2d : public StateBlock
+{
+ public:
+ StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
+ }
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
+};
+
+/**
+ * @brief State block for general 2D vectors
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * Being a vector, the geometric frame transformation will rotate the vector.
+ *
+ * This state block cannot have a local parametrization
+ */
+class StateVector2d : public StateBlock
+{
+ public:
+ StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
+ }
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
+};
+
+/**
+ * @brief State block for general 3D points and positions
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ *
+ * This state block cannot have a local parametrization
+ */
+class StatePoint3d : public StateBlock
+{
+ public:
+ StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_)
+ setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
+ }
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
+};
+
+/**
+ * @brief State block for general 3D vectors
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * Being a vector, the geometric frame transformation will rotate the vector.
+ *
+ * This state block cannot have a local parametrization
+ */
+class StateVector3d : public StateBlock
+{
+ public:
+ StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
+ }
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
+};
+
+
+} // namespace wolf
+
+#endif // STATE_BLOCK_DERIVED_H_
\ No newline at end of file
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index e3569b5b4ed30159e0593de789b3b7a6c3644387..8f1d6de1f98c9afa906640f29d4a065188c7a04f 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -246,7 +246,7 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
* S["V"]["V"] S["V"]["W"]
* S["W"]["V"] S["W"]["W"]
*/
- MatrixComposite propagate(const MatrixComposite & _Cov); // This performs Jac * this * Jac.tr
+ MatrixComposite propagate(const MatrixComposite & _Cov); // This performs this * _Cov * this.tr
/**
* \brief Matrix-vector product
@@ -294,10 +294,6 @@ class StateBlockComposite
template<typename SB, typename ... Args>
std::shared_ptr<SB> emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor);
- // Emplace base state blocks.
- template<typename ... Args>
- inline StateBlockPtr emplace(const char& _sb_type, Args&&... _args_of_base_state_block_constructor);
-
unsigned int remove(const char& _sb_type);
bool has(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool has(const StateBlockPtr& _sb) const;
@@ -318,7 +314,10 @@ class StateBlockComposite
// Plus operator
void plus(const VectorComposite& _dx);
- // Perturb state with random noise
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
void perturb(double amplitude = 0.01);
// 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.
@@ -367,18 +366,6 @@ inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char &_sb_ty
return sb;
}
-template<typename ... Args>
-inline StateBlockPtr wolf::StateBlockComposite::emplace(const char &_sb_type,
- Args &&... _args_of_base_state_block_constructor)
-{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
-
- add(_sb_type, sb);
-
- return sb;
-}
-
}
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 6ee8a85c28725805ff5a28fb2149950f7cb8e132..8af6059aa6badcfc91665307e92df8fd9379694b 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -37,25 +37,27 @@ namespace wolf {
class StateHomogeneous3d : public StateBlock
{
public:
- StateHomogeneous3d(bool _fixed = false);
- StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
+ StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
+ StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
~StateHomogeneous3d() override;
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
+ virtual void transform(const VectorComposite& _transformation) override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
- StateBlock(_state, _fixed)
+inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) :
+ StateBlock(_state, _fixed, nullptr, _transformable)
{
assert(_state.size() == 4 && "Homogeneous 3d must be size 4.");
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
}
-inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed) :
- StateBlock(4, _fixed)
+inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) :
+ StateBlock(4, _fixed, nullptr, _transformable)
{
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
state_ << 0, 0, 0, 1; // Set origin
@@ -77,6 +79,12 @@ inline Eigen::VectorXd StateHomogeneous3d::identity() const
return Eigen::Quaterniond::Identity().coeffs();
}
+inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
+{
+ if (transformable_)
+ setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!!
+}
+
inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
{
MatrixSizeCheck<4,1>::check(_state);
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index f22772908c411fba8a096fb764af40fe6b39316e..b73279f3f7347cf994f23ba8bde4d03e3db75b34 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -37,27 +37,29 @@ namespace wolf {
class StateQuaternion : public StateBlock
{
public:
- StateQuaternion(bool _fixed = false);
- StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
- StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
+ StateQuaternion(bool _fixed = false, bool _transformable = true);
+ StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
+ StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
~StateQuaternion() override;
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
+ virtual void transform(const VectorComposite& _transformation) override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
- StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) :
+ StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
{
// TODO: remove these lines after issue #381 is addressed
assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
state_.normalize();
}
-inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
- StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) :
+ StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
{
assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
@@ -66,8 +68,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix
state_.normalize();
}
-inline StateQuaternion::StateQuaternion(bool _fixed) :
- StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) :
+ StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
{
state_ = Eigen::Quaterniond::Identity().coeffs();
//
@@ -89,6 +91,12 @@ inline Eigen::VectorXd StateQuaternion::identity() const
return Eigen::Quaterniond::Identity().coeffs();
}
+inline void StateQuaternion::transform(const VectorComposite& _transformation)
+{
+ if (transformable_)
+ setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs());
+}
+
inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
{
MatrixSizeCheck<4,1>::check(_state);
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 1c53906c6c624ef4e88ac95785fa1871b9b5c122..12a8eee4da7b95af04fc475ac219310f8cecfd20 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -28,8 +28,8 @@
#include "core/state_block/state_composite.h"
// Eigen
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
// std
#include <regex>
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index e5283ccd5b5f669e000fda1904245c99e58955ee..0cde0badc595350bacf33f466abf21ede7b6a67f 100644
--- a/include/core/utils/params_server.h
+++ b/include/core/utils/params_server.h
@@ -51,6 +51,8 @@ public:
void addParams(std::map<std::string, std::string> _params);
+ bool hasParam(std::string _key) const;
+
// template<typename T>
// T getParam(std::string key, std::string def_value) const {
// if(params_.find(key) != params_.end()){
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index b89b9102ab64073b729744ec5d6c7d4d8098b143..9c46dd20c96535308e5f851e218641498787a631 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -19,16 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file utils_gtest.h
- * \brief Some utils for gtest
- * \author Jeremie Deray
- * Created on: 26/09/2016
- * Eigen macros extension by: Joan Sola on 26/04/2017
- */
-
-#ifndef WOLF_UTILS_GTEST_H
-#define WOLF_UTILS_GTEST_H
+#pragma once
#include <gtest/gtest.h>
@@ -144,24 +135,58 @@ TEST(Test, Foo)
}, \
C_expect, C_actual);
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
-#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- MatrixXd er = lhs - rhs; \
+#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, C_actual);
+
+#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 3 and rhs.size() == 3); \
+ Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- MatrixXd er = lhs - rhs; \
+#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 3 and rhs.size() == 3); \
+ Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-} // namespace testing
+#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
-#endif /* WOLF_UTILS_GTEST_H */
+} // namespace testing
\ No newline at end of file
diff --git a/include/core/yaml/yaml_conversion.h b/include/core/yaml/yaml_conversion.h
index 76991419a0ec370174055479cbb9dfdb910e0356..8ae7e0384e8aae80cdffc161e4c48e74525df36b 100644
--- a/include/core/yaml/yaml_conversion.h
+++ b/include/core/yaml/yaml_conversion.h
@@ -33,8 +33,8 @@
#include <yaml-cpp/yaml.h>
// Eigen
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
// stl
#include <iostream>
diff --git a/prova.txt b/prova.txt
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index e6d4a1e660bd0664f2fa1196c2ab558840664263..a154cdbb4b62606dc5260310cbfd5b4052bda54c 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -158,13 +158,15 @@ FactorBasePtrList CaptureBase::getFactorList()
void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
for (auto f_ptr : getFeatureList())
- f_ptr->getFactorList(_fac_list);
+ if (not f_ptr->isRemoving())
+ f_ptr->getFactorList(_fac_list);
}
void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto f_ptr : getFeatureList())
- f_ptr->getFactorList(_fac_list);
+ if (not f_ptr->isRemoving())
+ f_ptr->getFactorList(_fac_list);
}
FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..154dcc83f488b39a91c69f0906a0f59e2fdd56d7
--- /dev/null
+++ b/src/capture/capture_landmarks_external.cpp
@@ -0,0 +1,53 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/capture/capture_landmarks_external.h"
+
+namespace wolf{
+
+CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const std::vector<int>& _ids,
+ const std::vector<Eigen::VectorXd>& _detections,
+ const std::vector<Eigen::MatrixXd>& _covs,
+ const std::vector<double>& _qualities) :
+ CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
+{
+ if (_ids.size() != _detections.size() or
+ _ids.size() != _covs.size() or
+ _ids.size() != _qualities.size())
+ throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
+
+ for (auto i = 0; i < _detections.size(); i++)
+ addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
+}
+
+CaptureLandmarksExternal::~CaptureLandmarksExternal()
+{
+ //
+}
+
+void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
+{
+ detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
+}
+
+} // namespace wolf
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 9ac1675b8b9ddc388f85b7fd6d8d7e34a274e65b..9610063417bd9a283b01d90068abf51e655e2f68 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -141,42 +141,44 @@ void FactorBase::remove(bool viral_remove_empty_parent)
CaptureBaseConstPtr FactorBase::getCapture() const
{
auto ftr = getFeature();
- if (ftr != nullptr) return ftr->getCapture();
+ if (ftr != nullptr and not ftr->isRemoving())
+ return ftr->getCapture();
else return nullptr;
}
CaptureBasePtr FactorBase::getCapture()
{
auto ftr = getFeature();
- if (ftr != nullptr) return ftr->getCapture();
+ if (ftr != nullptr and not ftr->isRemoving())
+ return ftr->getCapture();
else return nullptr;
}
FrameBaseConstPtr FactorBase::getFrame() const
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getFrame();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
else return nullptr;
}
FrameBasePtr FactorBase::getFrame()
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getFrame();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
else return nullptr;
}
SensorBaseConstPtr FactorBase::getSensor() const
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getSensor();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
else return nullptr;
}
SensorBasePtr FactorBase::getSensor()
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getSensor();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
else return nullptr;
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index aae21b6eb2fa9d5552e8212259299b71e0e0c440..390d6d51019aea6a65208707b1b8671194dc060e 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -351,13 +351,15 @@ FactorBasePtrList FrameBase::getFactorList()
void FrameBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
for (auto c_ptr : getCaptureList())
- c_ptr->getFactorList(_fac_list);
+ if (not c_ptr->isRemoving())
+ c_ptr->getFactorList(_fac_list);
}
void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto c_ptr : getCaptureList())
- c_ptr->getFactorList(_fac_list);
+ if (not c_ptr->isRemoving())
+ c_ptr->getFactorList(_fac_list);
}
bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index e7acd4f0d90ef0883eea1af949928b0458e25a44..a47ecce3e0a77a90bc0a2fe97de18c7860ca472a 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -23,9 +23,10 @@
#include "core/landmark/landmark_base.h"
#include "core/factor/factor_base.h"
#include "core/map/map_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/factory_state_block.h"
#include "core/common/factory.h"
#include "core/yaml/yaml_conversion.h"
@@ -144,6 +145,7 @@ YAML::Node LandmarkBase::saveToYaml() const
node["orientation"] = getO()->getState();
node["orientation fixed"] = getO()->isFixed();
}
+ node["transformable"] = getP()->isTransformable();
return node;
}
@@ -216,7 +218,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
Eigen::VectorXd pos = _node["position"] .as< Eigen::VectorXd >();
bool pos_fixed = _node["position fixed"] .as< bool >();
- StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed);
+ StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
StateBlockPtr ori_sb = nullptr;
if (_node["orientation"])
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea..0c002934af723fd242babeb22cabd4de15823e9f 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -65,21 +65,46 @@ void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
landmark_list_.remove(_landmark_ptr);
}
-void MapBase::load(const std::string& _map_file_dot_yaml)
+LandmarkBaseConstPtr MapBase::getLandmark(const unsigned int& _id) const
{
- YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBaseConstPtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
+
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
+
+ return (*lmk_it);
+}
+
+LandmarkBasePtr MapBase::getLandmark(const unsigned int& _id)
+{
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBasePtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
- unsigned int nlandmarks = map["nlandmarks"].as<unsigned int>();
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
- assert(nlandmarks == map["landmarks"].size() && "Number of landmarks in map file does not match!");
+ return (*lmk_it);
+}
- for (unsigned int i = 0; i < nlandmarks; i++)
+void MapBase::load(const std::string& _map_file_dot_yaml)
+{
+ YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+
+ for (unsigned int i = 0; i < map["landmarks"].size(); i++)
{
YAML::Node lmk_node = map["landmarks"][i];
LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
lmk_ptr->link(shared_from_this());
}
-
}
void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name)
@@ -87,10 +112,8 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
YAML::Emitter emitter;
emitter << YAML::BeginMap;
- emitter << "map name" << _map_name;
- emitter << "date-time" << dateTimeNow(); // Get date and time for archiving purposes
-
- emitter << "nlandmarks" << getLandmarkList().size();
+ emitter << "map_name" << _map_name;
+ emitter << "date_time" << dateTimeNow(); // Get date and time for archiving purposes
emitter << "landmarks" << YAML::BeginSeq;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6fe2afb34530e550d8a361e56bcaae2c07938349..e95ce825308eefa1b4c3295e60e2abffd2cc4847 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -44,7 +44,8 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
map_ptr_(_map),
motion_provider_map_(),
frame_structure_(_frame_structure),
- prior_options_(std::make_shared<PriorOptions>())
+ prior_options_(std::make_shared<PriorOptions>()),
+ transformed_(false)
{
dim_ = _dim;
if (_frame_structure == "PO" and _dim == 2)
@@ -104,26 +105,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
if (plugins_path.back() != '/'){
plugins_path += '/'; // only works for UNIX systems
}
- /*try
- {
- plugins_path = _server.getParam<std::string>("plugins_path");
- }
- catch (MissingValueException& e)
- {
- 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"
-
- std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
- WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
- auto l = std::make_shared<LoaderRaw>(plugin);
- l->load();
- loaders.push_back(l);
- }
// load raw libs
std::vector<std::string> raw_libs;
@@ -143,20 +124,35 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
loaders.push_back(l);
}
- // Install sensors and processors
+ // Install sensors and processors -- load plugins only if sensors and processor are not registered
auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
- for(auto sen : sensors)
- problem->installSensor(sen["type"],
- sen["name"],
- _server);
-
- auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
- for(auto prc : processors)
- problem->installProcessor(prc["type"],
- prc["name"],
- prc["sensor_name"],
- _server);
-
+ for (auto sen : sensors)
+ {
+ if (not AutoConfFactorySensor::isCreatorRegistered(sen["type"]))
+ {
+ auto plugin_name = sen["plugin"];
+ std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
+ problem->installSensor(sen["type"], sen["name"], _server);
+ }
+ auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
+ for (auto prc : processors)
+ {
+ if (not AutoConfFactoryProcessor::isCreatorRegistered(prc["type"]))
+ {
+ auto plugin_name = prc["plugin"];
+ std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
+ problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server);
+ }
// Map (optional)
std::string map_type, map_plugin;
@@ -176,6 +172,11 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
auto map = AutoConfFactoryMap::create(map_type, _server);
map->setProblem(problem);
problem->setMap(map);
+ // load map from file (optional)
+ if (_server.hasParam("map/filename"))
+ {
+ problem->loadMap(_server.getParam<std::string>("map/filename"));
+ }
// Tree manager
std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
@@ -586,7 +587,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
}
}
- // check for empty blocks and fill them with the the prior, or with zeros in the worst case
+ // check for empty blocks and fill them with the prior, or with zeros in the worst case
VectorComposite state_last;
@@ -1044,14 +1045,14 @@ void Problem::setPriorOptions(const std::string& _mode,
assert((_mode == "nothing" || _mode == "initial_guess" || _mode == "fix" || _mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
// Store options (optionals depending on the mode)
- WOLF_TRACE("prior mode: ", _mode);
+ WOLF_DEBUG("prior mode: ", _mode);
prior_options_->mode = _mode;
if (prior_options_->mode != "nothing")
{
assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
- WOLF_TRACE("prior state: ", _state);
+ WOLF_DEBUG("prior state: ", _state);
prior_options_->state = _state;
if (prior_options_->mode == "factor")
@@ -1073,7 +1074,7 @@ void Problem::setPriorOptions(const std::string& _mode,
const auto& cov_blk = (sig_blk.array() * sig_blk.array()).matrix().asDiagonal();
Q.emplace(key,key,cov_blk);
}
- WOLF_TRACE("prior covariance:" , Q);
+ WOLF_DEBUG("prior covariance:" , Q);
prior_options_->cov = Q;
}
}
@@ -1260,4 +1261,42 @@ void Problem::perturb(double amplitude)
L->perturb(amplitude);
}
+void Problem::transform(const VectorComposite& _transformation)
+{
+ std::lock_guard<std::mutex> lock_transform (mut_transform_); // Protect especially from SolverManager::update()
+
+ transformation_ = _transformation;
+ transformed_.store(true);
+
+ // Sensors
+ for (auto S : getHardware()->getSensorList())
+ S->transform(_transformation);
+
+ // Frames and Captures
+ for (auto F : getTrajectory()->getFrameMap())
+ {
+ F.second->transform(_transformation);
+ for (auto& C : F.second->getCaptureList())
+ C->transform(_transformation);
+ }
+
+ // Landmarks
+ for (auto L : getMap()->getLandmarkList())
+ L->transform(_transformation);
+}
+
+bool Problem::isTransformed() const
+{
+ return transformed_.load();
+}
+void Problem::resetTransformed()
+{
+ transformed_.store(false);
+}
+VectorComposite Problem::getTransformation() const
+{
+ std::lock_guard<std::mutex> lock(mut_transform_);
+ return transformation_;
+}
+
} // namespace wolf
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index f61c1b1f9ef7ba4d4f3609fed41fedef53e3cbb8..6afd1c5c76c9c838e3072711b69ca199da0a48b9 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -144,14 +144,14 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before
}
-void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs)
+void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs, bool show_covs)
{
using std::cout;
using std::endl;
- if (!show_data && !show_delta && !show_delta_int && !show_jacs)
+ if (!show_data && !show_delta && !show_delta_int && !show_jacs && !show_covs)
{
- cout << "Buffer state [" << this->size() << "] : <";
+ cout << "Buffer size: " << this->size() << " ; time stamps: <";
for (Motion mot : *this)
cout << " " << mot.ts_;
cout << " >" << endl;
@@ -165,17 +165,20 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
if (show_data)
{
cout << " data: " << mot.data_.transpose() << endl;
- cout << " data cov: \n" << mot.data_cov_ << endl;
+ if (show_covs)
+ cout << " data cov: \n" << mot.data_cov_ << endl;
}
if (show_delta)
{
cout << " delta: " << mot.delta_.transpose() << endl;
- cout << " delta cov: \n" << mot.delta_cov_ << endl;
+ if (show_covs)
+ cout << " delta cov: \n" << mot.delta_cov_ << endl;
}
if (show_delta_int)
{
cout << " delta integrated: " << mot.delta_integr_.transpose() << endl;
- cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
+ if (show_covs)
+ cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
}
if (show_jacs)
{
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index e2723db61a38f9ee6b98a362eeafac8289875e03..d3a7db55582378db0621f029e8cdf16c7f677355 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -37,8 +37,18 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion
void MotionProvider::setOdometry(const VectorComposite& _odom)
{
+ std::lock_guard<std::mutex> lock(mut_odometry_);
+
assert(_odom.includesStructure(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom.");
for (auto key : state_structure_)
odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_
}
+
+wolf::VectorComposite MotionProvider::getOdometry ( ) const
+{
+ std::lock_guard<std::mutex> lock(mut_odometry_);
+
+ return odometry_;
+}
+
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index db06d0078261369000aed77e8d8f4d290cd2a355..310fb5f107e46cd416a3dff9c7f9a84abd6fe334 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -166,28 +166,22 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
return cap_motion;
}
-FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
+void ProcessorDiffDrive::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- auto key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
-
- return key_feature_ptr;
+ auto feature = FeatureBase::emplace<FeatureDiffDrive>(_capture_own,
+ _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_,
+ _capture_own->getCalibrationPreint(),
+ _capture_own->getBuffer().back().jacobian_calib_);
+
+ auto ftr_motion = std::static_pointer_cast<FeatureMotion>(feature);
+ FactorBase::emplace<FactorDiffDrive>(ftr_motion,
+ ftr_motion,
+ _capture_origin,
+ shared_from_this(),
+ params_prc_diff_drv_selfcal_->apply_loss_function);
}
-FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin)
-{
- auto ftr_motion = std::static_pointer_cast<FeatureMotion>(_feature);
- auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion,
- ftr_motion,
- _capture_origin,
- shared_from_this(),
- params_prc_diff_drv_selfcal_->apply_loss_function);
- return fac_odom;
-}
} /* namespace wolf */
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2ac3ff66ae6d3ed22787b7b8166cf9c4c5b8965a
--- /dev/null
+++ b/src/processor/processor_landmark_external.cpp
@@ -0,0 +1,497 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/processor/processor_landmark_external.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/math/SE2.h"
+#include "core/math/SE3.h"
+
+using namespace Eigen;
+
+namespace wolf
+{
+
+void ProcessorLandmarkExternal::preProcess()
+{
+ assert(new_features_incoming_.empty());
+
+ auto dim = getProblem()->getDim();
+ auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
+ if (not cap_landmarks)
+ throw std::runtime_error("ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
+
+ // Extract features from capture detections
+ auto landmark_detections = cap_landmarks->getDetections();
+ std::set<int> ids;
+ for (auto detection : landmark_detections)
+ {
+ WOLF_WARN_COND(ids.count(detection.id), "ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
+
+ // Filter by quality
+ if (detection.quality < params_tfle_->filter_quality_th or ids.count(detection.id))
+ continue;
+
+ // measure and covariance
+ VectorXd meas;
+ MatrixXd cov;
+ if (not params_tfle_->use_orientation)
+ {
+ assert(detection.measure.size() >= dim);
+ assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure.head(dim);
+ cov = detection.covariance.topLeftCorner(dim,dim);
+ }
+ else
+ {
+ assert(detection.measure.size() == (dim == 2 ? 3 : 7));
+ assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure;
+ cov = detection.covariance;
+ }
+
+ // emplace feature
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
+ "FeatureLandmarkExternal",
+ meas,
+ cov);
+ ftr->setLandmarkId(detection.id);
+
+ if (detection.id != -1 and detection.id != 0)
+ ids.insert(detection.id);
+
+ //unmatched_detections_incoming_.push_back(ftr);
+
+ // add new feature
+ new_features_incoming_.push_back(ftr);
+ }
+ WOLF_INFO("ProcessorLandmarkExternal::preprocess: found ", new_features_incoming_.size(), " new features");
+}
+
+unsigned int ProcessorLandmarkExternal::processKnown()
+{
+ // clear list of known features
+ known_features_incoming_.clear();
+
+ // Track features from last_ptr_ to incoming_ptr_
+ if (last_ptr_)
+ {
+ WOLF_INFO("Searching ", known_features_last_.size(), " tracked features...");
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
+ auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+
+ for (auto feat_last : known_features_last_)
+ {
+ auto feat_candidate_it = new_features_incoming_.begin();
+ while (feat_candidate_it != new_features_incoming_.end())
+ {
+ if ((*feat_candidate_it)->landmarkId() == feat_last->landmarkId() and
+ detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < params_tfle_->match_dist_th)
+ {
+ // grow track
+ track_matrix_.add(feat_last, *feat_candidate_it);
+
+ // feature is known
+ known_features_incoming_.push_back((*feat_candidate_it));
+
+ // remove from unmatched
+ feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
+ break;
+ }
+ else
+ feat_candidate_it++;
+ }
+ }
+ WOLF_INFO("Tracked ", known_features_incoming_.size(), " features.");
+ }
+
+ // Add new features (not tracked) as known features
+ WOLF_INFO_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
+ while (not new_features_incoming_.empty())
+ {
+ auto ftr = new_features_incoming_.front();
+
+ // new track
+ track_matrix_.newTrack(ftr);
+
+ // feature is now known
+ known_features_incoming_.push_back(ftr);
+
+ // remove from unmatched
+ new_features_incoming_.pop_front();
+ }
+
+ // check all features have been emplaced
+ assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by trackFeatures()");
+
+ return known_features_incoming_.size();
+}
+
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const
+{
+ // Any not available info of poses, assume identity
+ if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or not _pose_sen.includesStructure("PO"))
+ {
+ if (getProblem()->getDim() == 2)
+ return (_ftr1->getMeasurement().head<2>() - _ftr2->getMeasurement().head<2>()).norm();
+ else
+ return (_ftr1->getMeasurement().head<3>() - _ftr2->getMeasurement().head<3>()).norm();
+ }
+ else
+ {
+ if (getProblem()->getDim() == 2)
+ {
+ auto pose_s1 = SE2::compose(_pose1, _pose_sen);
+ auto pose_s2 = SE2::compose(_pose2, _pose_sen);
+ auto p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
+ auto p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+ return (p1-p2).norm();
+ }
+ else
+ {
+ auto pose_s1 = SE3::compose(_pose1, _pose_sen);
+ auto pose_s2 = SE3::compose(_pose2, _pose_sen);
+ auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
+ auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+ return (p1-p2).norm();
+ }
+ }
+}
+
+bool ProcessorLandmarkExternal::voteForKeyFrame() const
+{
+ auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+
+ WOLF_INFO("Active feature tracks: " , track_ids_last.size() );
+
+ // no tracks longer than filter_track_length_th
+ auto n_tracks = 0;
+ auto n_new_tracks = 0;
+ for (auto track_id : track_ids_last)
+ {
+ if (track_matrix_.trackSize(track_id) >= params_tfle_->filter_track_length_th)
+ {
+ n_tracks++;
+ if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId()))
+ n_new_tracks++;
+ }
+ }
+
+ // Necessary condition: active valid tracks
+ bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
+ WOLF_INFO("vote_min_features: ", vote_min_features,
+ " - Active feature tracks longer than ", params_tfle_->filter_track_length_th, ": ", n_tracks,
+ " (should be equal or bigger than ", params_tfle_->min_features_for_keyframe, ")");
+
+ bool vote_new_features(true), vote_time_span(true);
+ if (vote_min_features and origin_ptr_)
+ {
+ // Sufficient condition: new valid tracks
+ vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
+ WOLF_INFO("vote_new_features: " , vote_new_features,
+ " - n_new_tracks = ", n_new_tracks,
+ " (should be equal or bigger than ", params_tfle_->new_features_for_keyframe, ")");
+
+ // Sufficient condition: time span
+ vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
+ WOLF_INFO("vote_time_span: " , vote_time_span,
+ " - time_span = ", last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
+ " (should be bigger than ", params_tfle_->time_span, ")");
+ }
+
+ bool vote = vote_min_features and (vote_new_features or vote_time_span);
+
+ WOLF_INFO( (vote ? "Vote ": "Do NOT vote ") , "for KF" );
+
+ return vote;
+}
+
+void ProcessorLandmarkExternal::establishFactors()
+{
+ if (origin_ptr_ == last_ptr_)
+ return;
+
+ // reset n_tracks_origin_
+ lmks_ids_origin_.clear();
+
+ // will emplace a factor (and landmark if needed) for each known feature in last with long tracks (params_tfle_->filter_track_length_th)
+ FactorBasePtrList fac_list;
+ auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+
+ for (auto track_id : track_ids_last)
+ {
+ // check track length
+ if (track_matrix_.trackSize(track_id) < params_tfle_->filter_track_length_th)
+ continue;
+
+ auto feature = track_matrix_.feature(track_id, last_ptr_);
+
+ // get landmark
+ LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
+
+ // emplace landmark
+ if (not lmk)
+ lmk = emplaceLandmark(feature);
+
+ // modify landmark
+ modifyLandmark(lmk, feature);
+
+ // emplace factor
+ auto fac = emplaceFactor(feature, lmk);
+
+ lmks_ids_origin_.insert(lmk->id());
+
+ if (fac)
+ fac_list.push_back(fac);
+ }
+
+ WOLF_INFO("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
+}
+
+FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
+{
+ assert(_feature);
+ assert(_landmark);
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceFactor unknown case");
+
+ // not reachable
+ return nullptr;
+
+}
+
+LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _feature)
+{
+ assert(_feature);
+ assert(_feature->getCapture());
+ assert(_feature->getCapture()->getFrame());
+ assert(_feature->getCapture()->getSensorP());
+ assert(_feature->getCapture()->getSensorO());
+ assert(getProblem());
+ assert(getProblem()->getMap());
+
+ if (getProblem()->getMap()->getLandmark(_feature->landmarkId()) != nullptr)
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
+
+ LandmarkBasePtr lmk;
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature->landmarkId());
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ std::make_shared<StateAngle>(lmk_o));
+ lmk->setId(_feature->landmarkId());
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature->landmarkId());
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ std::make_shared<StateQuaternion>(lmk_o));
+ lmk->setId(_feature->landmarkId());
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark unknown case");
+
+ return lmk;
+}
+
+void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
+ FeatureBasePtr _feature)
+{
+ assert(_feature);
+ assert(_feature->getCapture());
+ assert(_feature->getCapture()->getFrame());
+ assert(_feature->getCapture()->getSensorP());
+ assert(_feature->getCapture()->getSensorO());
+ assert(getProblem());
+
+ if (not _landmark)
+ throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ // nothing to do (we assume P already in landmark)
+ return;
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ // no orientation, add it
+ if (not _landmark->getO())
+ {
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+ _landmark->addStateBlock('O', std::make_shared<StateAngle>(lmk_o), getProblem());
+ }
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ // nothing to do (we assume P already in landmark)
+ return;
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ // no orientation, add it
+ if (not _landmark->getO())
+ {
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+ _landmark->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
+ }
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark unknown case");
+}
+
+void ProcessorLandmarkExternal::advanceDerived()
+{
+ // Reset here the list of correspondences.
+ known_features_last_ = std::move(known_features_incoming_);
+}
+void ProcessorLandmarkExternal::resetDerived()
+{
+ // Reset here the list of correspondences.
+ known_features_last_ = std::move(known_features_incoming_);
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorLandmarkExternal)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLandmarkExternal)
+} // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0ad4a754dd665d4533303f2e27569b0e7c151048..b1ed04c2be5814b217357383cc5feaafff1f64c4 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -47,6 +47,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
MotionProvider(_state_structure, _params_motion),
params_motion_(_params_motion),
processing_step_(RUNNING_WITHOUT_KF),
+ bootstrapping_(false),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
@@ -65,11 +66,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
calib_preint_(_calib_size),
jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
jacobian_delta_(delta_cov_size_, delta_cov_size_),
- jacobian_calib_(delta_cov_size_, calib_size_)
+ jacobian_calib_(delta_cov_size_, calib_size_),
+ jacobian_delta_calib_(delta_cov_size_, calib_size_)
{
jacobian_delta_preint_ .setIdentity(delta_cov_size_,delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros
jacobian_delta_ .setIdentity(delta_cov_size_,delta_cov_size_); //
jacobian_calib_ .setZero(delta_cov_size_,calib_size_);
+ jacobian_delta_calib_ .setZero(delta_cov_size_,calib_size_);
unmeasured_perturbation_cov_ =
params_motion_->unmeasured_perturbation_std
* params_motion_->unmeasured_perturbation_std
@@ -116,8 +119,7 @@ void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
// emplace new feature and factor (if origin has frame)
if (cap_prev->getOriginCapture() and cap_prev->getOriginCapture()->getFrame())
{
- auto new_feature = emplaceFeature(cap_target);
- emplaceFactor(new_feature, cap_prev->getOriginCapture());
+ emplaceFeaturesAndFactors(cap_prev->getOriginCapture(), cap_target);
}
}
@@ -192,7 +194,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
if (keyframe_from_callback)
buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- switch(processing_step_)
+ switch(processing_step_) // Things to do before integrating motion data
{
case FIRST_TIME_WITHOUT_KF :
{
@@ -202,19 +204,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
}
case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
{
- // cannot joint to the KF: create own origin
+ // cannot join to the KF: create own origin
setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_ON_INCOMING :
{
- // can joint to the KF
+ // can join to the KF
setOrigin(keyframe_from_callback);
break;
}
case FIRST_TIME_WITH_KF_AFTER_INCOMING :
{
- // cannot joint to the KF: create own origin
+ // cannot join to the KF: create own origin
setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
@@ -227,12 +229,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
}
- // integrate data
+ // integrate motion data
// Done at this place because setPrior() needs
integrateOneStep();
+ // perform bootstrap steps (usually only IMU requires this)
+ if (bootstrapping_) bootstrap();
- switch(processing_step_)
+ switch (processing_step_) // Things to do after integrating motion data
{
case RUNNING_WITH_KF_BEFORE_ORIGIN :
{
@@ -268,11 +272,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
- auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback); // k
+ auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback);
if (!capture_existing)
{
- WOLF_WARN("A KF before first motion capture (TS = ", timestamp_from_callback, "). ProcessorMotion cannot do anything.");
+ WOLF_WARN(getName(), ": Cannot join KF. The received KF (TS = ", timestamp_from_callback, ") is older than the first motion capture.");
break;
}
@@ -307,11 +311,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// and give the part of the buffer before the new keyframe to the capture for the KF callback
splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
- // create motion feature and add it to the capture
- auto feature_new = emplaceFeature(capture_for_keyframe_callback);
-
- // create motion factor and add it to the feature, and constrain to the other capture (origin)
- emplaceFactor(feature_new, capture_origin );
+ // // create motion feature and add it to the capture
+ // // create motion factor and add it to the feature, and constrain to the other capture (origin)
+ emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
// modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty())
@@ -321,9 +323,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
assert(capture_existing->getFeatureList().empty());// there was only one feature!
- auto new_feature_existing = emplaceFeature(capture_existing);
-
- emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
+ emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing);
}
break;
@@ -401,10 +401,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
- auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
-
// create motion factor and add it to the feature, and constrain to the other capture (origin)
- emplaceFactor(feature_for_keyframe_callback, capture_origin );
+ emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
// reset processor origin
origin_ptr_ = capture_for_keyframe_callback;
@@ -440,7 +438,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* x : any capture
* o : origin capture
* l : last capture -> we'll make a KF here
- * i : incoming capture
+ * i : incoming capture -> data has already been integrated into last capture
* n : new capture ->
* --- : buffer history
*
@@ -472,10 +470,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
keyframe ->link(getProblem());
// create motion feature and add it to the key_capture
- auto key_feature = emplaceFeature(last_ptr_);
-
// create motion factor and link it to parent feature and other frame (which is origin's frame)
- auto factor = emplaceFactor(key_feature, origin_ptr_);
+ emplaceFeaturesAndFactors(origin_ptr_, last_ptr_);
// create a new frame
auto frame_new = std::make_shared<FrameBase>(getTimeStamp(),
@@ -564,8 +560,6 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
auto calib_preint = last_ptr_->getCalibrationPreint();
VectorComposite state;
- //WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp());
- //WOLF_INFO("processorMotion origin timestamp: ", origin_ptr_->getTimeStamp());
if ( hasCalibration())
{
// Get current calibration -- from origin capture
@@ -610,7 +604,6 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
-// _x needs to have the size of the processor state
VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStructure& _structure) const
{
assert(_ts.ok());
@@ -763,6 +756,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
void ProcessorMotion::integrateOneStep()
{
+ /* Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
+ */
+
// Set dt
dt_ = updateDt();
assert(dt_ >= 0 && "Time interval _dt is negative!");
@@ -813,7 +809,9 @@ void ProcessorMotion::integrateOneStep()
jacobian_calib_);
// integrate odometry
- statePlusDelta(odometry_, delta_, dt_, odometry_);
+ VectorComposite odom;
+ statePlusDelta(getOdometry(), delta_, dt_, odom);
+ setOdometry(odom);
}
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
@@ -990,23 +988,23 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
if (checkTimeTolerance(keyframe_from_callback,
incoming_ptr_))
{
- WOLF_DEBUG("First time with a KF compatible.")
+ WOLF_DEBUG("PM ", getName(), ": First time with a KF compatible.")
processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
}
else if (keyframe_from_callback->getTimeStamp() < incoming_ptr_->getTimeStamp())
{
- WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
+ WOLF_DEBUG("PM ", getName(), ": First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
}
else
{
- WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
}
}
else
{
- WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
processing_step_ = FIRST_TIME_WITHOUT_KF;
}
@@ -1052,7 +1050,7 @@ TimeStamp ProcessorMotion::getTimeStamp ( ) const
origin_ptr_->isRemoving() or
not last_ptr_)
{
- WOLF_DEBUG("Processor has no time stamp. Returning a non-valid timestamp equal to 0");
+ WOLF_DEBUG("PM proc \"", getName(), "\" has no time stamp. Returning a non-valid timestamp equal to 0");
return TimeStamp::Invalid();
}
@@ -1066,7 +1064,7 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo
{
_stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
if (getOrigin())
- _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
<< getOrigin()->getFrame()->id() << std::endl;
if (getLast())
_stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 98f55c0f0d6c5ff9af08d627c1ef4d67b351783e..69dfe4e6ea31ebea40d9b9205db212e40067a4dc 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -164,29 +164,17 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
return cap_motion;
}
-FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
+void ProcessorOdom2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- auto fac_odom = FactorBase::emplace<FactorRelativePose2d>(_feature,
- _feature,
- _capture_origin->getFrame(),
- shared_from_this(),
- params_->apply_loss_function,
- TOP_MOTION);
- return fac_odom;
-}
-
-FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
+ Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_;
makePosDef(covariance);
- FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ProcessorOdom2d",
- _capture_motion->getBuffer().back().delta_integr_,
- covariance);
- return key_feature_ptr;
-}
+ FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>(
+ _capture_own, "ProcessorOdom2d", _capture_own->getBuffer().back().delta_integr_, covariance);
+ FactorBase::emplace<FactorRelativePose2d>(
+ feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION);
+}
} /* namespace wolf */
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 471dd627361266aad20356543345355a5d18bdbf..1870bb66b61021cb0c3b4851eaec97ca5b80d80b 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -55,9 +55,8 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
if (_data.size() == (long int)6)
{
// rotation in vector form
- _delta.head<3>() = _data.head<3>();
- Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3);
- q = v2q(_data.tail<3>());
+ _delta.head<3>() = _data.head<3>();
+ _delta.tail<4>() = v2q(_data.tail<3>()).coeffs();
}
else
{
@@ -214,15 +213,6 @@ CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own,
return cap_motion;
}
-FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ProcessorOdom3d",
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_);
- return key_feature_ptr;
-}
-
Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const
{
@@ -231,15 +221,16 @@ Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_prei
return delta_corrected;
}
-FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+
+void ProcessorOdom3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- auto fac_odom = FactorBase::emplace<FactorRelativePose3d>(_feature_motion,
- _feature_motion,
- _capture_origin->getFrame(),
- shared_from_this(),
- params_->apply_loss_function,
- TOP_MOTION);
- return fac_odom;
+ Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_;
+
+ FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>(
+ _capture_own, "ProcessorOdom3d", _capture_own->getBuffer().back().delta_integr_, covariance);
+
+ FactorBase::emplace<FactorRelativePose3d>(
+ feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION);
}
VectorXd ProcessorOdom3d::getCalibration (const CaptureBaseConstPtr _capture) const
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index b0e08b4b59240b359457565c95071d2b7c1385e6..9dc1fa5ab42f7dc88504b63977eb90611bc7bde2 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -90,8 +90,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -114,13 +115,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// 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();
+ if (not getProblem()->getMap()->getLandmarkList().empty())
+ processKnown();
// Issue KF callback with new KF
getProblem()->keyFrameCallback(keyframe, shared_from_this());
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -149,25 +152,28 @@ 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(),
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
- incoming_ptr_->link(keyframe);
+ incoming_ptr_->link(frame);
// Process info
- // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
- processKnown();
+ // If we have known landmarks or features. Process them.
+ if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
+ processKnown();
+
// Both Trackers: We have a last_ Capture with not enough features, so populate it.
processNew(params_tracker_->max_new_features);
// Establish factors
establishFactors();
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = keyframe;
+ last_frame_ptr_ = frame;
incoming_ptr_ = nullptr;
break;
@@ -199,8 +205,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Establish factors
establishFactors();
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
@@ -237,8 +244,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
getProblem()->getState(incoming_ptr_->getTimeStamp()));
incoming_ptr_ ->link(frame);
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
@@ -249,9 +257,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
// We do not create a KF
- // Advance this
- advanceDerived();
-
// Replace last frame for a new NON KEY frame in incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
@@ -259,6 +264,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->link(frame);
last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
+ // Advance this
+ advanceDerived();
// Update pointers
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index d55a2666cc00b3c8cdc53854ab43f7903f0b9d6e..c269ff41137f33cb6cfd14158b54228346782f28 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -200,11 +200,11 @@ void ProcessorTrackerFeature::establishFactors()
auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
- assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()");
+ assert((fac_ptr == nullptr or fac_ptr->getFeature() != nullptr) && "not emplaced factor returned by emplaceFactor()");
- WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
+ WOLF_DEBUG_COND(fac_ptr, "New factor: track: " , feature_in_last->trackId(),
+ " origin: " , feature_in_origin->id() ,
+ " from last: " , feature_in_last->id() );
}
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 996d48263c113555ba055422153bf10459bfb564..af06fd13624d22a6b0f83ab8b62f0769b1b0284f 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -56,14 +56,20 @@ SensorBase::SensorBase(const std::string& _type,
noise_cov_.setZero();
if (_p_ptr)
+ {
+ _p_ptr->setNonTransformable();
addStateBlock('P', _p_ptr, _p_dyn);
+ }
if (_o_ptr)
+ {
+ _o_ptr->setNonTransformable();
addStateBlock('O', _o_ptr, _o_dyn);
-
+ }
if (_intr_ptr)
+ {
addStateBlock('I', _intr_ptr, _intr_dyn);
-
+ }
}
SensorBase::SensorBase(const std::string& _type,
@@ -87,11 +93,15 @@ SensorBase::SensorBase(const std::string& _type,
setNoiseStd(_noise_std);
if (_p_ptr)
+ {
+ _p_ptr->setNonTransformable();
addStateBlock('P', _p_ptr, _p_dyn);
-
+ }
if (_o_ptr)
+ {
+ _o_ptr->setNonTransformable();
addStateBlock('O', _o_ptr, _o_dyn);
-
+ }
if (_intr_ptr)
addStateBlock('I', _intr_ptr, _intr_dyn);
}
@@ -541,8 +551,18 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
auto sb = getStateBlockDynamic(key);
if (sb)
- _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
- << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
+ {
+ _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", "
+ << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] = ( " << std::setprecision(3)
+ << sb->getState().transpose() << " )";
+ if (params_prior_map_.count(key))
+ {
+ auto ftr = params_prior_map_.at(key);
+ _stream << " ; mean = ( " << ftr->getMeasurement().transpose() << " )";
+ _stream << " , std = ( " << ftr->getMeasurementCovariance().diagonal().array().sqrt().transpose() << " )";
+ }
+ _stream << " @ " << sb << std::endl;
+ }
}
}
else if (_metric)
@@ -557,7 +577,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
auto sb = getStateBlockDynamic(key);
if (sb)
- _stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
+ _stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", " << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] @ " << sb;
}
_stream << std::endl;
}
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 4620caea4e169aa3ef64d233e6883fdea53bed02..256def47937b9cd42e94b72b2169e4a3e72fc897 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -27,21 +27,23 @@
*/
#include "core/sensor/sensor_diff_drive.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
namespace wolf
{
-SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
- ParamsSensorDiffDrivePtr _intrinsics) :
- SensorBase("SensorDiffDrive",
- std::make_shared<StateBlock>(_extrinsics.head(2), true),
- std::make_shared<StateAngle>(_extrinsics(2), true),
- std::make_shared<StateBlock>(3, false), 2),
- params_diff_drive_(_intrinsics)
+SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics, ParamsSensorDiffDrivePtr _intrinsics)
+ : SensorBase("SensorDiffDrive",
+ std::make_shared<StatePoint2d>(_extrinsics.head(2), true),
+ std::make_shared<StateAngle>(_extrinsics(2), true),
+ std::make_shared<StateParams3>(
+ Vector3d(_intrinsics->radius_left, _intrinsics->radius_right, _intrinsics->wheel_separation),
+ false),
+ 2),
+ params_diff_drive_(_intrinsics)
{
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
- getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
unfixIntrinsics();
if(params_diff_drive_->set_intrinsics_prior)
@@ -53,7 +55,6 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
setNoiseStd(noise_sigma);
-
}
SensorDiffDrive::~SensorDiffDrive()
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index 711545f66b48b99fcfaf891954db3ddba7b56d7a..9a4a9d1f1f41323e3209da09afcf968ef3dc69b4 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -20,17 +20,19 @@
//
//--------LICENSE_END--------
#include "core/sensor/sensor_odom_2d.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
namespace wolf {
SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics) :
- SensorBase("SensorOdom2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
+ SensorBase("SensorOdom2d", std::make_shared<StatePoint2d>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_rot_to_rot_(_intrinsics.k_rot_to_rot)
{
assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
+ getStateBlock('P')->setNonTransformable();
+ getStateBlock('O')->setNonTransformable();
//
}
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index b44561ff402bef4971b211cfa2eae43724457fc3..112dc72eb98a50dfdf9f34bbceba7d3994078758 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -28,14 +28,14 @@
#include "core/sensor/sensor_odom_3d.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/math/rotations.h"
namespace wolf {
SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& _intrinsics) :
- SensorBase("SensorOdom3d", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+ SensorBase("SensorOdom3d", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_disp_to_rot_(_intrinsics.k_disp_to_rot),
k_rot_to_rot_(_intrinsics.k_rot_to_rot),
@@ -43,6 +43,8 @@ SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSe
min_rot_var_(_intrinsics.min_rot_var)
{
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
+ getStateBlock('P')->setNonTransformable();
+ getStateBlock('O')->setNonTransformable();
noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
setNoiseCov(noise_cov_); // sets also noise_std_
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 87f2880b8f3e58e1898470572e630375bd792d8c..c9c0c90db18737e6052e83a2050ee9fd3a1ed71f 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/src/sensor/sensor_pose.cpp
@@ -28,21 +28,26 @@
#include "core/sensor/sensor_pose.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/math/rotations.h"
namespace wolf {
SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
- SensorBase("SensorPose", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
- std_p_(_intrinsics.std_p),
- std_o_(_intrinsics.std_o)
+ SensorBase("SensorPose",
+ std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true),
+ std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true),
+ nullptr,
+ 6)
{
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
- noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
- setNoiseCov(noise_cov_); // sets also noise_std_
+ params_pose_ = std::make_shared<ParamsSensorPose>(_intrinsics);
+
+ noise_std_ << params_pose_->std_px, params_pose_->std_py, params_pose_->std_pz, params_pose_->std_ox, params_pose_->std_oy, params_pose_->std_oz;
+
+ setNoiseStd(noise_std_); // sets also noise_cov_
}
SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index c9d192a536151f0340a7fd1f8f9f5e88acf54c26..a9d3ee25ad0cc1e5d7c2a8991c221835fbcd4334 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -57,6 +57,9 @@ SolverManager::~SolverManager()
void SolverManager::update()
{
+ // lock mutex to prevent Problem::transform() during this update()
+ std::lock_guard<std::mutex> lock_transform (wolf_problem_->mut_transform_);
+
// Consume notification maps
std::map<StateBlockPtr,Notification> sb_notification_map;
std::map<FactorBasePtr,Notification> fac_notification_map;
@@ -147,8 +150,9 @@ void SolverManager::update()
state_ptr->resetFixUpdated();
state_ptr->resetLocalParamUpdated();
}
+ wolf_problem_->resetTransformed();
- #ifdef _WOLF_DEBUG
+#ifdef _WOLF_DEBUG
assert(check("after update()"));
#endif
}
@@ -188,7 +192,15 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
{
// Avoid usuless copies
if (!stateblock_statevector.first->isFixed())
- stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
+ {
+ stateblock_statevector.first->setState(stateblock_statevector.second,
+ false); // false = do not raise the flag state_updated_
+
+ // Transform the whole set of state blocks if necessary
+ // (this may happen when Problem::transform() has been called during the execution of this solve())
+ if (wolf_problem_->isTransformed())
+ stateblock_statevector.first->transform(wolf_problem_->getTransformation());
+ }
}
auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
acc_duration_state_ += duration_state;
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 047469dc899a8973b7bdf07df318b2079c155ef9..91523db7c2ea51f7f47e680cfda5cac7cadacedf 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -76,6 +76,13 @@ void HasStateBlocks::perturb(double amplitude)
}
}
+void HasStateBlocks::transform(const VectorComposite& _transformation)
+{
+ for (auto& pair_key_sb : state_block_map_)
+ pair_key_sb.second->transform(_transformation);
+}
+
+
void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
if (_metric && _state_blocks)
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 513d6d4627627a029e3ebe61bab550497d369634..5fe479daf2fdab0245b223f76075f05d7e2e4001 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -74,7 +74,6 @@ void StateBlock::plus(const Eigen::VectorXd &_dv)
#include "core/state_block/state_homogeneous_3d.h"
namespace wolf{
-WOLF_REGISTER_STATEBLOCK(StateBlock);
WOLF_REGISTER_STATEBLOCK(StateQuaternion);
WOLF_REGISTER_STATEBLOCK(StateAngle);
WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5bc71bbc0252beb8c951a6bc37d5f5a66884ea0
--- /dev/null
+++ b/src/state_block/state_block_derived.cpp
@@ -0,0 +1,108 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/factory_state_block.h"
+
+namespace wolf
+{
+StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Point2d.");
+}
+
+StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Point3d.");
+}
+
+StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Vector2d.");
+}
+
+StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Vector3d.");
+}
+
+StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2)
+ return std::make_shared<StatePoint2d>(_state, _fixed);
+ else if (_state.size() == 3)
+ return std::make_shared<StatePoint3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Point.");
+}
+
+StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2)
+ return std::make_shared<StateVector2d>(_state, _fixed);
+ else if (_state.size() == 3)
+ return std::make_shared<StateVector3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Vector.");
+}
+
+template <size_t size>
+StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == size) return std::make_shared<StateParams<size>>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Params.");
+}
+
+namespace
+{
+const bool __attribute__((used)) create_pointRegisteredWithP =
+ FactoryStateBlock::registerCreator("P", wolf::create_point);
+const bool __attribute__((used)) create_vectorRegisteredWithP =
+ FactoryStateBlock::registerCreator("V", wolf::create_vector);
+} // namespace
+
+WOLF_REGISTER_STATEBLOCK(StatePoint2d);
+WOLF_REGISTER_STATEBLOCK(StateVector2d);
+WOLF_REGISTER_STATEBLOCK(StatePoint3d);
+WOLF_REGISTER_STATEBLOCK(StateVector3d);
+
+WOLF_REGISTER_STATEBLOCK(StateParams1);
+WOLF_REGISTER_STATEBLOCK(StateParams2);
+WOLF_REGISTER_STATEBLOCK(StateParams3);
+WOLF_REGISTER_STATEBLOCK(StateParams4);
+WOLF_REGISTER_STATEBLOCK(StateParams5);
+WOLF_REGISTER_STATEBLOCK(StateParams6);
+WOLF_REGISTER_STATEBLOCK(StateParams7);
+WOLF_REGISTER_STATEBLOCK(StateParams8);
+WOLF_REGISTER_STATEBLOCK(StateParams9);
+WOLF_REGISTER_STATEBLOCK(StateParams10);
+
+} // namespace wolf
\ No newline at end of file
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 24de3de294a6e65ea409d345a31a23393904ae50..244d6a505d66143823e6a67456c33972ff86a588 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -54,13 +54,15 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
for(auto fr_pair: frame_map_)
- fr_pair.second->getFactorList(_fac_list);
+ if (not fr_pair.second->isRemoving())
+ fr_pair.second->getFactorList(_fac_list);
}
void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
{
for(auto fr_pair: frame_map_)
- fr_pair.second->getFactorList(_fac_list);
+ if (not fr_pair.second->isRemoving())
+ fr_pair.second->getFactorList(_fac_list);
}
TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const
diff --git a/src/utils/converter_utils.cpp b/src/utils/converter_utils.cpp
index f77008e1bb528118d955789e15a914d329d804bc..b5412684689c739d9b5824c9ff2866a07d43e48b 100644
--- a/src/utils/converter_utils.cpp
+++ b/src/utils/converter_utils.cpp
@@ -23,8 +23,8 @@
// Eigen
#include <array>
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
// STD
#include <iostream>
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index ce4a79743d24302907e85117b812458ed4de6c83..df5cd7677ddc448918c4c5aec0c74fddef0073e2 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.cpp
@@ -47,3 +47,8 @@ void ParamsServer::addParams(std::map<std::string, std::string> _params)
{
params_.insert(_params.begin(), _params.end());
}
+
+bool ParamsServer::hasParam(std::string _key) const
+{
+ return (params_.count(_key) != 0);
+}
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
index bfbe151c43315c21e5f636e2632fa9ceb614c9d4..4ce97752fc8d5a59aed7c7b0a4b0c3f89de07405 100644
--- a/src/yaml/sensor_pose_yaml.cpp
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -49,8 +49,12 @@ static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_
{
ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
- params->std_p = config["std_p"].as<double>();
- params->std_o = config["std_o"].as<double>();
+ params->std_px = config["std_p"].as<double>();
+ params->std_py = config["std_p"].as<double>();
+ params->std_pz = config["std_p"].as<double>();
+ params->std_ox = config["std_o"].as<double>();
+ params->std_oy = config["std_o"].as<double>();
+ params->std_oz = config["std_o"].as<double>();
return params;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6fa50395a7ee2af50d290b32e543b18bac534869..299594805cb46a551145613baf30481f4e7aed22 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -173,6 +173,18 @@ wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
# FactorRelativePose3dWithExtrinsics class test
wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+# FactorRelativePosition2d class test
+wolf_add_gtest(gtest_factor_relative_position_2d gtest_factor_relative_position_2d.cpp)
+
+# FactorRelativePosition2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_position_2d_with_extrinsics gtest_factor_relative_position_2d_with_extrinsics.cpp)
+
+# FactorRelativePosition3d class test
+wolf_add_gtest(gtest_factor_relative_position_3d gtest_factor_relative_position_3d.cpp)
+
+# FactorRelativePosition3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_position_3d_with_extrinsics gtest_factor_relative_position_3d_with_extrinsics.cpp)
+
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
@@ -191,6 +203,9 @@ wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model
# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
+# ProcessorLandmarkExternal class test
+wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
+
# ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 54525c8682b4c80d95e3e38b2ef34784e4a84cfe..b72b6a3e677d29889cf80a14a81fc32d163944b2 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -29,6 +29,8 @@
#include "processor_tracker_landmark_dummy.h"
#include "core/landmark/landmark_base.h"
#include "factor_landmark_dummy.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
@@ -124,8 +126,8 @@ LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _f
//std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
"LandmarkBase",
- std::make_shared<StateBlock>(2),
- std::make_shared<StateBlock>(1));
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(0));
}
FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index be07e98047c8074979d036074f2df6ee490e9a90..614d6f7f4cc04430d4a595209a94039bbbacd5cb 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -19,19 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_SE3.cpp
- *
- * Created on: Feb 2, 2019
- * \author: jsola
- */
-
#include "core/math/SE3.h"
#include "core/utils/utils_gtest.h"
-
-
using namespace Eigen;
using namespace wolf;
using namespace SE3;
@@ -103,6 +94,55 @@ TEST(SE3, log_of_power)
ASSERT_MATRIX_APPROX(tau3, log(pose3), 1e-8);
}
+TEST(SE3, inverse)
+{
+ Vector3d p; p.setRandom();
+ Vector4d qvec; qvec.setRandom().normalized();
+ Quaterniond q(qvec);
+
+ Vector3d pi_true = -(q.conjugate() * p);
+ Quaterniond qi_true = q.conjugate();
+
+ // separate API
+ Vector3d pi_out;
+ Quaterniond qi_out;
+
+ inverse(p, q, pi_out, qi_out);
+ ASSERT_MATRIX_APPROX(pi_out, pi_true, 1e-8);
+ ASSERT_MATRIX_APPROX(qi_out.coeffs(), qi_true.coeffs(), 1e-8);
+
+ // Vector7d API
+ Vector7d pose; pose << p, q.coeffs();
+ Vector7d posei_true; posei_true << pi_true, qi_true.coeffs();
+ Vector7d posei_out;
+ inverse(pose, posei_out);
+ ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
+
+ posei_out = inverse(pose);
+ ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
+}
+
+TEST(SE3, inverseComposite)
+{
+ Vector3d p; p.setRandom();
+ Vector4d qvec; qvec.setRandom().normalized();
+ VectorComposite pose_vc("PO", {p, qvec});
+ Quaterniond q(qvec);
+
+ // ground truth
+ Vector3d pi_true = -(q.conjugate() * p);
+ Quaterniond qi_true = q.conjugate();
+
+ VectorComposite pose_vc_out("PO", {3, 4});
+ inverse(pose_vc, pose_vc_out);
+ ASSERT_MATRIX_APPROX(pose_vc_out.at('P'), pi_true, 1e-8);
+ ASSERT_MATRIX_APPROX(pose_vc_out.at('O'), qi_true.coeffs(), 1e-8);
+
+ VectorComposite pose_vc_out_bis = inverse(pose_vc);
+ ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('P'), pi_true, 1e-8);
+ ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('O'), qi_true.coeffs(), 1e-8);
+}
+
TEST(SE3, composeBlocks)
{
Vector3d p1, p2, pc;
@@ -144,7 +184,6 @@ TEST(SE3, composeEigenVectors)
compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
-
Vector7d x1; x1 << p1, q1.coeffs();
Vector7d x2; x2 << p2, q2.coeffs();
Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
@@ -176,6 +215,10 @@ TEST(SE3, composeVectorComposite)
ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
+
+ VectorComposite vc = compose(x1, x2);
+ ASSERT_MATRIX_APPROX(vc.at('P'), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(vc.at('O'), qc.coeffs(), 1e-8);
}
TEST(SE3, composeVectorComposite_Jacobians)
@@ -233,7 +276,6 @@ TEST(SE3, exp_0_Composite)
ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
-
}
TEST(SE3, plus_0_Composite)
@@ -397,6 +439,7 @@ TEST(SE3, interpolate_third)
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
}
+
TEST(SE3, toSE3_I)
{
Vector7d pose; pose << 0,0,0, 0,0,0,1;
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index aa36078108a936109484bd4a8d761c258105d236..aaf3aa4fbaca881a24b520ecb85795d5fb0f9b7d 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -29,6 +29,7 @@
#include "core/utils/utils_gtest.h"
#include "core/capture/capture_base.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -58,9 +59,9 @@ TEST(CaptureBase, ConstructorWithSensor)
TEST(CaptureBase, Static_sensor_params)
{
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0) );
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", p, o, i, 2));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
@@ -77,10 +78,10 @@ TEST(CaptureBase, Static_sensor_params)
TEST(CaptureBase, Dynamic_sensor_params)
{
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StateBlock>(2), std::make_shared<StateAngle>(), std::make_shared<StateBlock>(2), 2, true, true, true)); // last 3 'true' mark dynamic
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0) );
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
+ SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StatePoint2d>(Vector2d::Zero()), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Vector2d::Zero()), 2, true, true, true)); // last 3 'true' mark dynamic
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 0aa294035d9f712b706883636e46cd1e4ae62593..956f6eb4e76f44b333ea5253a0023c0ec4fadac5 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -28,7 +28,8 @@
#include "core/utils/utils_gtest.h"
-
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/sensor/sensor_odom_2d.h"
@@ -64,7 +65,7 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
@@ -89,7 +90,7 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(0,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -103,7 +104,7 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -121,7 +122,7 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -141,7 +142,7 @@ TEST(Emplace, EmplaceDerived)
{
ProblemPtr P = Problem::create("POV", 3);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
@@ -163,7 +164,7 @@ TEST(Emplace, ReturnDerived)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 7a1a330916e5ba4a5e7c28f784bd6bfd21278da7..a2f8dfaf9f6bfe3a08ae392937b186377476e28e 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -30,6 +30,7 @@
#include "dummy/factor_dummy_zero_1.h"
#include "dummy/factor_dummy_zero_12.h"
+#include "core/state_block/state_block_derived.h"
#include "core/factor/factor_relative_pose_2d.h"
#include "core/utils/utils_gtest.h"
#include "core/sensor/sensor_odom_2d.h"
@@ -44,7 +45,7 @@ using namespace Eigen;
TEST(FactorAutodiff, AutodiffDummy1)
{
- StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ StateBlockPtr sb = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
@@ -61,18 +62,18 @@ TEST(FactorAutodiff, AutodiffDummy1)
TEST(FactorAutodiff, AutodiffDummy12)
{
- StateBlockPtr sb1 = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
- StateBlockPtr sb2 = std::make_shared<StateBlock>(Eigen::Vector2d::Random());
- StateBlockPtr sb3 = std::make_shared<StateBlock>(Eigen::Vector3d::Random());
- StateBlockPtr sb4 = std::make_shared<StateBlock>(Eigen::Vector4d::Random());
- StateBlockPtr sb5 = std::make_shared<StateBlock>(Eigen::Vector5d::Random());
- StateBlockPtr sb6 = std::make_shared<StateBlock>(Eigen::Vector6d::Random());
- StateBlockPtr sb7 = std::make_shared<StateBlock>(Eigen::Vector7d::Random());
- StateBlockPtr sb8 = std::make_shared<StateBlock>(Eigen::Vector8d::Random());
- StateBlockPtr sb9 = std::make_shared<StateBlock>(Eigen::Vector9d::Random());
- StateBlockPtr sb10 = std::make_shared<StateBlock>(Eigen::Vector10d::Random());
- StateBlockPtr sb11 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(11));
- StateBlockPtr sb12 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(12));
+ StateBlockPtr sb1 = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateParams2>(Eigen::Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateParams3>(Eigen::Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateParams4>(Eigen::Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateParams5>(Eigen::Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateParams6>(Eigen::Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateParams7>(Eigen::Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateParams8>(Eigen::Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateParams9>(Eigen::Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateParams10>(Eigen::Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateParams<11>>(Eigen::VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateParams<12>>(Eigen::VectorXd::Random(12));
std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
std::vector<Eigen::MatrixXd> J;
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index c37bed03e30da209b9b644d49b92011afeeb5f93..2cbbc7e4c732a9c42959cac3d3a01cc769154d5a 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -105,16 +105,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
}
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
- {
- return Base::emplaceFeature(_capture_own);
- }
-
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override
- {
- return Base::emplaceFactor(_feature_motion, _capture_origin);
- }
+ // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+ // {
+ // return Base::emplaceFeature(_capture_own);
+ // }
+
+ // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ // CaptureBasePtr _capture_origin) override
+ // {
+ // return Base::emplaceFactor(_feature_motion, _capture_origin);
+ // }
ParamsProcessorDiffDrivePtr getParams()
{
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index cc4030a9e51617399f2b256047f24eb30a93f5df..e317161887bc11dfbb4937b47e9ceb5b4c2e6c5e 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -25,6 +25,8 @@
#include "core/factor/factor_relative_pose_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace Eigen;
@@ -32,6 +34,12 @@ using namespace wolf;
using std::cout;
using std::endl;
+
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l;
+
// Input odometry data and covariance
Matrix3d data_cov = 0.1*Matrix3d::Identity();
@@ -43,93 +51,164 @@ SolverCeres solver(problem_ptr);
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero());
FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()));
+
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dPtr fac = nullptr;
+void generateRandomProblemFrame()
+{
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Frame 1 pose
+ x_1(2) = pi2pi(x_0(2) + delta(2));
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta.head<2>();
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
+}
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // landmark pose
+ x_l(2) = pi2pi(x_f(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta.head<2>();
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
TEST(FactorRelativePose2d, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorRelativePose2d, fix_0_solve)
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2d, frame_solve_frame1)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
-
- // x1 groundtruth
- Vector3d x1;
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
-
- // Set states and measurement
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
+ // random setup
+ generateRandomProblemFrame();
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
-
- // Fix frm0, perturb frm1
+ // Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
- frm1->perturb(5);
+ frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2d, fix_1_solve)
+TEST(FactorRelativePose2d, frame_solve_frame0)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(1);
- // x1 groundtruth
- Vector3d x1;
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // Set states and measurement
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // Fix frm1, perturb frm0
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
frm1->fix();
- frm0->unfix();
- frm0->perturb(5);
+ lmk->unfix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePose2d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index ee2bc076da1c48876250197c2fa1179ede489306..b5b4c9d0f36318408b0e056a7bdc3b17def589e7 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -25,8 +25,9 @@
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/capture/capture_odom_2d.h"
#include "core/sensor/sensor_odom_2d.h"
-#include "core/processor/processor_odom_2d.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace Eigen;
@@ -36,6 +37,11 @@ using std::endl;
std::string wolf_root = _WOLF_ROOT_DIR;
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l, x_s;
+
// Input odometry data and covariance
Matrix3d data_cov = 0.1*Matrix3d::Identity();
@@ -44,212 +50,307 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Sensor
-auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
-auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
+auto sensor = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
-// Capture from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()));
-TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem_ptr->check(0));
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // Frame 1 pose
+ x_1(2) = pi2pi(x_0(2) + delta(2));
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>()) - Rotation2Dd(x_1(2)) * x_s.head<2>();
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+void generateRandomProblemLandmark()
{
- for (int i = 0; i < 1e2; i++)
- {
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // landmark pose
+ x_l(2) = pi2pi(x_f(2) + x_s(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>());
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+////////////// TESTS /////////////////////////////////////////////////////////////////////
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame1)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
// Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->fix();
- frm1->perturb(5);
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame0)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm0->perturb(1);
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+// JV: The position extrinsics in case of pair of frames is not observable
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Perturb frm0, fix the rest
+ // Perturb sensor P, fix the rest
frm1->fix();
- frm0->unfix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->fix();
- frm0->perturb(5);
+ frm0->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ //ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_o)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Perturb sensor P, fix the rest
+ // Perturb landmark, fix the rest
frm1->fix();
- frm0->fix();
- sensor_odom2d->getP()->unfix();
- sensor_odom2d->getO()->fix();
- sensor_odom2d->getP()->perturb(1);
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_p_solve)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
// Perturb sensor O, fix the rest
frm1->fix();
- frm0->fix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->unfix();
- sensor_odom2d->getO()->perturb(.2);
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
- //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+ //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 9369c090efecad4b74435214230fca2b3b01f959..3b7f25c8fba43f0d56f5146fd2d5168e83733631 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -19,18 +19,14 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_factor_relative_pose_3d.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_3d.h"
-#include "core/capture/capture_motion.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
using namespace Eigen;
@@ -38,72 +34,189 @@ using namespace wolf;
using std::cout;
using std::endl;
-Vector7d data2delta(Vector6d _data)
-{
- return (Vector7d() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
-}
+std::string wolf_root = _WOLF_ROOT_DIR;
-// Input odometry data and covariance
-Vector6d data(Vector6d::Random());
-Vector7d delta = data2delta(data);
-Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6d data_cov = data_var.asDiagonal();
+int N = 1e2;
-// perturbated priors
-Vector7d x0 = data2delta(Vector6d::Random()*0.25);
-Vector7d x1 = data2delta(data + Vector6d::Random()*0.25);
+// Vectors
+Vector7d delta, x_0, x_1, x_f, x_l;
+
+// Input odometry data and covariance
+Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
// Problem and solver
ProblemPtr problem_ptr = Problem::create("PO", 3);
+
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
-
-// Capture, feature and factor from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
-FactorRelativePose3dPtr ctr1 = FactorBase::emplace<FactorRelativePose3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
+
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, nullptr, Vector7d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose3dPtr fac = nullptr;
////////////////////////////////////////////////////////
-TEST(FactorRelativePose3d, check_tree)
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem_ptr->check(0));
+ // Random delta
+ delta = Vector7d::Random() * 1e2;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame 0 pose
+ x_0 = Vector7d::Random() * 1e2;
+ x_0.tail<4>().normalize();
+ auto q_0 = Quaterniond(x_0.tail<4>());
+
+ // Frame 1 pose
+ x_1.head<3>() = x_0.head<3>() + q_0 * delta.head<3>();
+ x_1.tail<4>() = (q_0 * q_delta).coeffs();
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST(FactorRelativePose3d, expectation)
+void generateRandomProblemLandmark()
{
- ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
+ // Random delta
+ delta = Vector7d::Random() * 10;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 10;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Landmark pose
+ x_l.head<3>() = x_f.head<3>() + q_f * delta.head<3>();
+ x_l.tail<4>() = (q_f * q_delta).coeffs();
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
}
-TEST(FactorRelativePose3d, fix_0_solve)
+TEST(FactorRelativePose3d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose3d, frame_solve_frame1)
{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
- // Fix frame 0, perturb frm1
- frm0->fix();
- frm1->unfix();
- frm1->setState(x1);
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(1);
- // solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6);
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST(FactorRelativePose3d, fix_1_solve)
+TEST(FactorRelativePose3d, frame_solve_frame0)
{
- // Fix frame 1, perturb frm0
- frm0->unfix();
- frm1->fix();
- frm0->setState(x0);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // solve for frm0
- std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6);
+TEST(FactorRelativePose3d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ frm1->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 0f89a11003bd0e297ed6b3df283b6aba536ca5ea..2e99b1d2b765998d11c7eba96f7241e29977a7c4 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -19,307 +19,353 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
-#include <core/utils/utils_gtest.h>
+#include "core/utils/utils_gtest.h"
-#include "core/common/wolf.h"
-#include "core/utils/logging.h"
-
-#include "core/state_block/state_quaternion.h"
#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/capture/capture_pose.h"
-#include "core/feature/feature_pose.h"
-
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
using namespace Eigen;
using namespace wolf;
-using std::static_pointer_cast;
-
-
-
-// Use the following in case you want to initialize tests with predefines variables or methods.
-class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
- public:
- Vector3d pos_camera, pos_robot, pos_landmark;
- Vector3d euler_camera, euler_robot, euler_landmark;
- Quaterniond quat_camera, quat_robot, quat_landmark;
- Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors
- Vector7d pose_camera, pose_robot, pose_landmark;
-
- ProblemPtr problem;
- SolverCeresPtr solver;
-
- SensorBasePtr S;
- FrameBasePtr F1;
- CapturePosePtr c1;
- FeaturePosePtr f1;
- LandmarkBasePtr lmk1;
- FactorRelativePose3dWithExtrinsicsPtr fac;
-
- Eigen::Matrix6d meas_cov;
-
- void SetUp() override
- {
- // configuration
-
- /* We have three poses to take into account:
- * - pose of the sensor (extrinsincs)
- * - pose of the landmark
- * - pose of the robot (Keyframe)
- *
- * The measurement provides the pose of the landmark wrt sensor's current pose in the world.
- * Camera's current pose in World is the composition of the robot pose with the sensor extrinsics.
- *
- * The robot has a sensor looking forward
- * S: pos = (0,0,0), ori = (0, 0, 0)
- *
- * There is a point at the origin
- * P: pos = (0,0,0)
- *
- * Therefore, P projects exactly at the origin of the sensor,
- * f: p = (0,0)
- *
- * We form a Wolf tree with 1 frames F1, 1 capture C1,
- * 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1:
- *
- * Frame F1, Capture C1, feature f1, landmark l1, constraint c1
- *
- * The frame pose F1, and the sensor pose S
- * in the robot frame are variables subject to optimization
- *
- * We perform a number of tests based on this configuration.*/
-
-
- // sensor is at origin of robot and looking forward
- // robot is at (0,0,0)
- // landmark is right in front of sensor. Its orientation wrt sensor is identity.
- pos_camera << 0,0,0;
- pos_robot << 0,0,0; //robot is at origin
- pos_landmark << 0,1,0;
- euler_camera << 0,0,0;
- //euler_camera << -M_PI_2, 0, -M_PI_2;
- euler_robot << 0,0,0;
- euler_landmark << 0,0,0;
- quat_camera = e2q(euler_camera);
- quat_robot = e2q(euler_robot);
- quat_landmark = e2q(euler_landmark);
- vquat_camera = quat_camera.coeffs();
- vquat_robot = quat_robot.coeffs();
- vquat_landmark = quat_landmark.coeffs();
- pose_camera << pos_camera, vquat_camera;
- pose_robot << pos_robot, vquat_robot;
- pose_landmark << pos_landmark, vquat_landmark;
-
- // Build problem
- problem = Problem::create("PO", 3);
- solver = std::make_shared<SolverCeres>(problem);
-
- // Create sensor to be able to initialize (a camera for instance)
- S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose",
- std::make_shared<StateBlock>(pos_camera, true),
- std::make_shared<StateQuaternion>(vquat_camera, true),
- std::make_shared<StateBlock>(Vector4d::Zero(), false), // fixed
- Vector1d::Zero());
-
- // ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>();
- // S = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera);
- // camera = std::static_pointer_cast<SensorCamera>(S);
-
-
- // F1 is be origin KF
- VectorComposite x0(pose_robot, "PO", {3,4});
- VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
- F1 = problem->setPriorFactor(x0, s0, 0.0);
-
-
- // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world)
- meas_cov = Eigen::Matrix6d::Identity();
- meas_cov.topLeftCorner(3,3) *= 1e-2;
- meas_cov.bottomRightCorner(3,3) *= 1e-3;
-
- //emplace feature and landmark
- c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
- f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
- lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
- std::make_shared<StateBlock>(pos_landmark),
- std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
- }
-};
-
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
-{
- auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
- lmk1,
- nullptr,
- false);
+using std::cout;
+using std::endl;
- ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
-}
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1e2;
+
+// Vectors
+Vector7d delta, x_0, x_1, x_f, x_l, x_s;
+
+// Input odometry data and covariance
+Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
-/////////////////////////////////////////////
-// Tree not ok with this gtest implementation
-/////////////////////////////////////////////
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor, Vector7d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem->check(1));
-
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
- ASSERT_TRUE(problem->check(1));
+ // Random delta
+ delta = Vector7d::Random() * 1e2;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame 0 pose
+ x_0 = Vector7d::Random() * 1e2;
+ x_0.tail<4>().normalize();
+ auto q_0 = Quaterniond(x_0.tail<4>());
+
+ // Random extrinsics
+ x_s = Vector7d::Random() * 1e2;
+ x_s.tail<4>().normalize();
+ auto q_s = Quaterniond(x_s.tail<4>());
+
+ // Frame 1 pose
+ auto q_1 = q_0 * q_s * q_delta * q_s.conjugate();
+ x_1.head<3>() = x_0.head<3>() + q_0 * (x_s.head<3>() + q_s * delta.head<3>()) - q_1 * x_s.head<3>();
+ x_1.tail<4>() = q_1.coeffs();
+
+ // WOLF_INFO("x_0: ", x_0.transpose());
+ // WOLF_INFO("x_s: ", x_s.transpose());
+ // WOLF_INFO("delta: ", delta.transpose());
+ // WOLF_INFO("x_1: ", x_1.transpose());
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
+void generateRandomProblemLandmark()
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix F1, perturbate state
- F1->unfix();
- F1->getP()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+ // Random delta
+ delta = Vector7d::Random() * 10;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 10;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Random extrinsics
+ x_s = Vector7d::Random() * 10;
+ x_s.tail<4>().normalize();
+ auto q_s = Quaterniond(x_s.tail<4>());
+
+ // Landmark pose
+ x_l.head<3>() = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta.head<3>());
+ x_l.tail<4>() = (q_f * q_s * q_delta).coeffs();
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, check_tree)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix F1, perturbate state
- F1->unfix();
- F1->getO()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+ ASSERT_TRUE(problem_ptr->check(0));
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix lmk1, perturbate state
- lmk1->unfix();
- lmk1->getP()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm0->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
+// JV: The position extrinsics in case of pair of frames is not observable
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ //ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // unfix F1, perturbate state
- lmk1->unfix();
- lmk1->getO()->perturb();
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
{
- // Change setup
- Vector3d p_w_r, p_r_c, p_c_l, p_w_l;
- Quaterniond q_w_r, q_r_c, q_c_l, q_w_l;
- p_w_r << 1, 2, 3;
- p_r_c << 4, 5, 6;
- p_c_l << 7, 8, 9;
- q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize();
- q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize();
- q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize();
-
- q_w_l = q_w_r * q_r_c * q_c_l;
- p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l);
-
- // Change feature (remove and emplace)
- Vector7d meas;
- meas << p_c_l, q_c_l.coeffs();
- f1->remove();
- f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov));
-
- // emplace factor
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // Change Landmark
- lmk1->getP()->setState(p_w_l);
- lmk1->getO()->setState(q_w_l.coeffs());
- ASSERT_TRUE(lmk1->getP()->stateUpdated());
- ASSERT_TRUE(lmk1->getO()->stateUpdated());
-
- // Change Frame
- F1->getP()->setState(p_w_r);
- F1->getO()->setState(q_w_r.coeffs());
- F1->fix();
- ASSERT_TRUE(F1->getP()->stateUpdated());
- ASSERT_TRUE(F1->getO()->stateUpdated());
-
- // Change sensor extrinsics
- S->getP()->setState(p_r_c);
- S->getO()->setState(q_r_c.coeffs());
- ASSERT_TRUE(S->getP()->stateUpdated());
- ASSERT_TRUE(S->getO()->stateUpdated());
-
- Vector7d t_w_r, t_w_l;
- t_w_r << p_w_r, q_w_r.coeffs();
- t_w_l << p_w_l, q_w_l.coeffs();
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6);
-
- // unfix LMK, perturbate state
- lmk1->unfix();
- lmk1->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_factor_relative_position_2d.cpp b/test/gtest_factor_relative_position_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e91d0fb436646ea92401d64e5bde466e5989c6d9
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d.cpp
@@ -0,0 +1,268 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_2d.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1;//1e2
+
+// Vectors
+Vector3d x_0, x_f, x_1;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm1, 1, nullptr);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dPtr fac = nullptr;
+
+void generateRandomProblemFrame()
+{
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Frame 1 position
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta;
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureFramePosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, frm0, nullptr, false, TOP_MOTION);
+}
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // landmark position
+ x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta;
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePosition2d, frame_solve_frame1_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->getP()->unfix();
+ frm1->getO()->fix();
+ frm1->getP()->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->getP()->unfix();
+ frm0->getO()->fix();
+ frm0->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->getP()->fix();
+ frm0->getO()->unfix();
+ frm0->getO()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->getP()->unfix();
+ frm1->getO()->fix();
+ lmk->fix();
+ frm1->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->getP()->fix();
+ frm1->getO()->unfix();
+ lmk->fix();
+ frm1->getO()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..87bb8f07080f83760be2fe1896904c8f64e2bd92
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -0,0 +1,239 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1;//1e2
+
+// Vectors
+Vector3d x_f, x_s;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm, 1, sensor);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // landmark position
+ x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta);
+
+ // Set states
+ frm->setState(x_f);
+ lmk->setState(x_l);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm->fix();
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->getP()->unfix();
+ frm->getO()->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->getP()->fix();
+ frm->getO()->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->getO()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb sensor P, fix the rest
+ frm->fix();
+ lmk->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb sensor O, fix the rest
+ frm->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e928a49baf2f61b7c23eb663830a7a0d63c78a47
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d.cpp
@@ -0,0 +1,280 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_3d.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1e2;
+
+// Vectors
+Vector7d x_f;
+Vector3d delta1, delta2, delta3, x_1, x_2, x_3;
+
+// Input odometry data and covariance
+Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Two frames
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm2 = problem_ptr->emplaceFrame(2, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm3 = problem_ptr->emplaceFrame(3, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmarks
+LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+// Captures
+CaptureBasePtr cap1, cap2, cap3;
+// Features
+FeatureBasePtr fea1, fea2, fea3;
+// Factors
+FactorRelativePosition3dPtr fac1, fac2, fac3;
+
+////////////////////////////////////////////////////////
+void generateRandomProblemFrame()
+{
+ // Random delta
+ delta1 = Vector3d::Random() * 1e2;
+ delta2 = Vector3d::Random() * 1e2;
+ delta3 = Vector3d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 1e2;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Frames position
+ x_1 = x_f.head<3>() + q_f * delta1;
+ x_2 = x_f.head<3>() + q_f * delta2;
+ x_3 = x_f.head<3>() + q_f * delta3;
+
+ // Set states
+ frm->setState(x_f);
+ frm1->getP()->setState(x_1);
+ frm2->getP()->setState(x_2);
+ frm3->getP()->setState(x_3);
+
+ // capture & feature & factor with delta measurementCaptureBase(const std::string& _type,
+ cap1 = CaptureBase::emplace<CaptureBase>(frm1, "CaptureFramePosition3d", 1);
+ cap2 = CaptureBase::emplace<CaptureBase>(frm2, "CaptureFramePosition3d", 2);
+ cap3 = CaptureBase::emplace<CaptureBase>(frm3, "CaptureFramePosition3d", 3);
+ fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureFramePosition3d", delta1, data_cov);
+ fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureFramePosition3d", delta2, data_cov);
+ fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureFramePosition3d", delta3, data_cov);
+ fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, frm, nullptr, false, TOP_MOTION);
+ fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, frm, nullptr, false, TOP_MOTION);
+ fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, frm, nullptr, false, TOP_MOTION);
+}
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta1 = Vector3d::Random() * 1e2;
+ delta2 = Vector3d::Random() * 1e2;
+ delta3 = Vector3d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 1e2;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Landmarks positions
+ x_1 = x_f.head<3>() + q_f * delta1;
+ x_2 = x_f.head<3>() + q_f * delta2;
+ x_3 = x_f.head<3>() + q_f * delta3;
+
+ // Set states
+ frm->setState(x_f);
+ lmk1->setState(x_1);
+ lmk2->setState(x_2);
+ lmk3->setState(x_3);
+
+ // feature & factor with delta measurement
+ cap1 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+ cap2 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+ cap3 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+ fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPosition3d", delta1, data_cov);
+ fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureLandmarkPosition3d", delta2, data_cov);
+ fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureLandmarkPosition3d", delta3, data_cov);
+ fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, lmk1, nullptr, false, TOP_LMK);
+ fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, lmk2, nullptr, false, TOP_LMK);
+ fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, lmk3, nullptr, false, TOP_LMK);
+}
+
+TEST(FactorRelativePosition3d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePosition3d, frame_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm, fix the rest
+ frm->unfix();
+ frm1->fix();
+ frm2->fix();
+ frm3->fix();
+ frm->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3d, frame_solve_frames)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm->fix();
+ frm1->unfix();
+ frm2->unfix();
+ frm3->unfix();
+ frm1->getP()->perturb(1);
+ frm2->getP()->perturb(1);
+ frm3->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("P"), x_1, 1e-3);
+ ASSERT_MATRIX_APPROX(frm2->getStateVector("P"), x_2, 1e-3);
+ ASSERT_MATRIX_APPROX(frm3->getStateVector("P"), x_3, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition3d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->unfix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ frm->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3d, landmark_solve_lmks)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm->fix();
+ lmk1->unfix();
+ lmk2->unfix();
+ lmk3->unfix();
+ lmk1->perturb(1);
+ lmk2->perturb(1);
+ lmk3->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk1->getStateVector("P"), x_1, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk2->getStateVector("P"), x_2, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk3->getStateVector("P"), x_3, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3917e5d85f021c0f950b18fa7ab4d191fb835379
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -0,0 +1,267 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1e2;
+
+// Vectors
+Vector7d x_f, x_s;
+Vector3d delta1, delta2, delta3, x_l1, x_l2, x_l3;
+
+// Input odometry data and covariance
+Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmarks
+LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureOdom3d>(frm, 1, sensor, Vector3d::Zero(), data_cov);
+// Features
+FeatureBasePtr fea1, fea2, fea3;
+// Factors
+FactorRelativePosition3dWithExtrinsicsPtr fac1, fac2, fac3;
+
+void generateRandomProblemLandmark()
+{
+ // Random deltas
+ delta1 = Vector3d::Random() * 10;
+ delta2 = Vector3d::Random() * 10;
+ delta3 = Vector3d::Random() * 10;
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 10;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Random extrinsics
+ x_s = Vector7d::Random() * 10;
+ x_s.tail<4>().normalize();
+ auto q_s = Quaterniond(x_s.tail<4>());
+
+ // Landmarks poses
+ x_l1 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta1);
+ x_l2 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta2);
+ x_l3 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta3);
+
+ // Set states
+ frm->setState(x_f);
+ lmk1->setState(x_l1);
+ lmk2->setState(x_l2);
+ lmk3->setState(x_l3);
+ sensor->setState(x_s);
+
+ // features & factors with deltas measurements
+ fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta1, data_cov);
+ fea2 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta2, data_cov);
+ fea3 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta3, data_cov);
+ fac1 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1, fea1, lmk1, nullptr, false, TOP_LMK);
+ fac2 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2, fea2, lmk2, nullptr, false, TOP_LMK);
+ fac3 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3, fea3, lmk3, nullptr, false, TOP_LMK);
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_lmks)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb landmark, fix the rest
+ frm->fix();
+ lmk1->unfix();
+ lmk2->unfix();
+ lmk3->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk1->perturb(1);
+ lmk2->perturb(1);
+ lmk3->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk1->getStateVector(), x_l1, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk2->getStateVector(), x_l2, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk3->getStateVector(), x_l3, 1e-3);
+
+ // remove features (and factors) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm->unfix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm->fix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm->fix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factory.cpp b/test/gtest_factory.cpp
index f46ee75069a4611a1fdb45be399c852b527517b0..fb269fe8784ec08d3c62edf569e98a7da83d9e03 100644
--- a/test/gtest_factory.cpp
+++ b/test/gtest_factory.cpp
@@ -44,6 +44,19 @@ TEST(TestFactory, DummyObjectFactory)
DummyObjectDerived obj_derived = DummyObjectDerived("AnotherCoolDummyObject", server);
}
+TEST(TestFactory, isCreatorRegistered)
+{
+ ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ bool object_creator_registered = FactoryDummyObject::isCreatorRegistered("DummyObjectDerived");
+
+ ASSERT_TRUE(object_creator_registered);
+
+ // FORCE LOADING
+ DummyObjectDerived obj_derived = DummyObjectDerived("AnotherCoolDummyObject", server);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 6332a69928a558cfdede9125a73fd7ab7f835338..9822acd60d37ae94d4b73ef45a30ab5207eccbd0 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -31,79 +31,21 @@
#include "core/common/wolf.h"
#include "core/state_block/factory_state_block.h"
#include "core/sensor/factory_sensor.h"
-
+#include "core/state_block/state_block_derived.h"
using namespace wolf;
-/*
-// You may use this to make some methods of Foo public
-WOLF_PTR_TYPEDEFS(FooPublic);
-class FooPublic : public Foo
-{
- // You may use this to make some methods of Foo public
-}
-
-class TestInit : public testing::Test
-{
- public:
- // You may use this to initialize stuff
- // Combine it with TEST_F(FooTest, testName) { }
- void SetUp() override
- {
- // Init all you want here
- // e.g. FooPublic foo;
- }
- void TearDown() override {} // you can delete this if unused
-};
-
-TEST_F(TestInit, testName)
-{
- // Use class TestInit
-}
-*/
-
-//TEST(FactoryStateBlock, get_getClass)
-//{
-// const auto& f = FactoryStateBlock::get();
-//
-// const std::string& s = f.getClass();
-//
-// ASSERT_EQ(s, "FactoryStateBlock");
-//}
-TEST(FactoryStateBlock, creator_default)
+TEST(FactoryStateBlock, creator_non_registered_key)
{
- auto sbp = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
- auto sbv = FactoryStateBlock::create("V", Eigen::Vector2d(4,5), false);
- auto sbw = FactoryStateBlock::create("W", Eigen::Vector1d(6), false);
-
- ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
-
- ASSERT_FALSE(sbp->hasLocalParametrization());
- ASSERT_FALSE(sbv->hasLocalParametrization());
- ASSERT_FALSE(sbw->hasLocalParametrization());
+ // non registered -> throw
+ ASSERT_THROW(auto sba = FactoryStateBlock::create("A", Eigen::Vector1d(6), false), std::runtime_error);
}
TEST(FactoryStateBlock, creator_StateBlock)
{
- auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false);
- auto sbv = FactoryStateBlock::create("StateBlock", Eigen::Vector2d(4,5), true);
- auto sbw = FactoryStateBlock::create("StateBlock", Eigen::Vector1d(6), false);
-
- ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
-
- ASSERT_FALSE(sbp->isFixed());
- ASSERT_TRUE (sbv->isFixed());
- ASSERT_FALSE(sbw->isFixed());
-
- ASSERT_FALSE(sbp->hasLocalParametrization());
- ASSERT_FALSE(sbv->hasLocalParametrization());
- ASSERT_FALSE(sbw->hasLocalParametrization());
+ ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false), std::runtime_error);
}
TEST(FactoryStateBlock, creator_Quaternion)
@@ -165,6 +107,109 @@ TEST(FactoryStateBlock, creator_O_is_wrong_size)
ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error);
}
+TEST(FactoryStateBlock, creator_Point)
+{
+ auto sba = FactoryStateBlock::create("StatePoint2d", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("StatePoint3d", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_P)
+{
+ auto sba = FactoryStateBlock::create("P", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_Vector)
+{
+ auto sba = FactoryStateBlock::create("StateVector2d", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("StateVector3d", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_V)
+{
+ auto sba = FactoryStateBlock::create("V", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("V", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_Params)
+{
+ auto sb1 = FactoryStateBlock::create("StateParams1", Eigen::Vector1d::Ones(), false);
+ auto sb2 = FactoryStateBlock::create("StateParams2", Eigen::Vector2d::Ones(), false);
+ auto sb3 = FactoryStateBlock::create("StateParams3", Eigen::Vector3d::Ones(), false);
+ auto sb4 = FactoryStateBlock::create("StateParams4", Eigen::Vector4d::Ones(), false);
+ auto sb5 = FactoryStateBlock::create("StateParams5", Eigen::Vector5d::Ones(), false);
+ auto sb6 = FactoryStateBlock::create("StateParams6", Eigen::Vector6d::Ones(), false);
+ auto sb7 = FactoryStateBlock::create("StateParams7", Eigen::Vector7d::Ones(), false);
+ auto sb8 = FactoryStateBlock::create("StateParams8", Eigen::Vector8d::Ones(), false);
+ auto sb9 = FactoryStateBlock::create("StateParams9", Eigen::Vector9d::Ones(), false);
+ auto sb10 = FactoryStateBlock::create("StateParams10", Eigen::Vector10d::Ones(), false);
+
+ ASSERT_EQ(sb1->getSize(), 1);
+ ASSERT_EQ(sb2->getSize(), 2);
+ ASSERT_EQ(sb3->getSize(), 3);
+ ASSERT_EQ(sb4->getSize(), 4);
+ ASSERT_EQ(sb5->getSize(), 5);
+ ASSERT_EQ(sb6->getSize(), 6);
+ ASSERT_EQ(sb7->getSize(), 7);
+ ASSERT_EQ(sb8->getSize(), 8);
+ ASSERT_EQ(sb9->getSize(), 9);
+ ASSERT_EQ(sb10->getSize(), 10);
+
+ ASSERT_EQ(sb1->getLocalSize(), 1);
+ ASSERT_FALSE(sb1->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::length_error);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index eb61f798d41b2aec792b864dffb39cb1cdb7d098..64dbd6d85484e2ac6ed270b470f674107d146cb6 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -33,6 +33,8 @@
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_motion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
@@ -42,7 +44,7 @@ using namespace wolf;
TEST(FrameBase, GettersAndSetters)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
// getters
ASSERT_EQ(F->id(), (unsigned int) 1);
@@ -55,7 +57,7 @@ TEST(FrameBase, GettersAndSetters)
TEST(FrameBase, StateBlocks)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
@@ -65,7 +67,7 @@ TEST(FrameBase, StateBlocks)
TEST(FrameBase, LinksBasic)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
@@ -85,8 +87,8 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
@@ -139,16 +141,16 @@ TEST(FrameBase, Frames)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
// tree is now consistent
ASSERT_TRUE(P->check(0));
@@ -396,8 +398,8 @@ TEST(FrameBase, Frames)
TEST(FrameBase, GetSetState)
{
// Create PQV_3d state blocks
- StateBlockPtr sbp = make_shared<StateBlock>(3);
- StateBlockPtr sbv = make_shared<StateBlock>(3);
+ StateBlockPtr sbp = make_shared<StatePoint3d>(Vector3d::Zero());
+ StateBlockPtr sbv = make_shared<StateVector3d>(Vector3d::Zero());
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
// Create frame
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index e60bc875310148a2fc0a127884b4fb1b3b77ab6a..2b02be288217cca57703ab98b5b9c7a40f32d505 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -34,6 +34,8 @@
#include "core/sensor/sensor_base.h"
#include "core/landmark/landmark_base.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -55,12 +57,12 @@ class HasStateBlocksTest : public testing::Test
{
problem = Problem::create("POV", 3);
- sbp0 = std::make_shared<StateBlock>(3); // size 3
+ sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo0 = std::make_shared<StateQuaternion>();
- sbv0 = std::make_shared<StateBlock>(3); // size 3
- sbp1 = std::make_shared<StateBlock>(3); // size 3
+ sbv0 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
+ sbp1 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo1 = std::make_shared<StateQuaternion>();
- sbv1 = std::make_shared<StateBlock>(3); // size 3
+ sbv1 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 5c4b2b7893837316736ab7a69a49f1b08c169b9b..f0957a5abd90436d96d6e3a4e2788ad2d5c54faa 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -31,7 +31,8 @@
#include "core/problem/problem.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "core/yaml/yaml_conversion.h"
@@ -39,6 +40,9 @@
#include <iostream>
using namespace wolf;
+std::string wolf_root = _WOLF_ROOT_DIR;
+std::string map_path = wolf_root + "/test/yaml/maps";
+
TEST(MapYaml, save_2d)
{
ProblemPtr problem = Problem::create("PO", 2);
@@ -51,19 +55,16 @@ TEST(MapYaml, save_2d)
o2 << 2;
o3 << 3;
- StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1);
- StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2);
- StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2);
- StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true);
- StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true);
+ StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1);
+ StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2);
+ StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2(0));
+ StateBlockPtr p3_sb = std::make_shared<StatePoint2d>(p3, true);
+ StateBlockPtr o3_sb = std::make_shared<StateAngle>(o3(0), true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, o3_sb);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
// Check Problem functions
std::string filename = map_path + "/map_2d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
@@ -79,89 +80,54 @@ TEST(MapYaml, load_2d)
{
ProblemPtr problem = Problem::create("PO", 2);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
- // Check Problem functions
- std::string filename = map_path + "/map_2d_problem.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->loadMap(filename);
-
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+ std::vector<std::string> maps{"map_2d_map", "map_2d_problem", "map_2d_manual"};
- for (auto lmk : problem->getMap()->getLandmarkList())
+ for (auto map : maps)
{
- if (lmk->id() == 1)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- }
- else if (lmk->id() == 2)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- }
- else if (lmk->id() == 3)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
- }
- }
-
- // empty the map
- {
- auto lmk_list = problem->getMap()->getLandmarkList();
- for (auto lmk : lmk_list)
- lmk->remove();
- }
- ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+ std::string filename = map_path + "/" + map + ".yaml";
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->loadMap(filename);
- // Check Map functions
- filename = map_path + "/map_2d_map.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->getMap()->load(filename);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk->id() == 1)
+ for (auto lmk : problem->getMap()->getLandmarkList())
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ }
}
- else if (lmk->id() == 2)
+ // empty the map
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- }
- else if (lmk->id() == 3)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
+ auto lmk_list = problem->getMap()->getLandmarkList();
+ for (auto lmk : lmk_list)
+ lmk->remove();
}
+ ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
}
}
+
TEST(MapYaml, save_3d)
{
ProblemPtr problem = Problem::create("PO", 3);
@@ -174,19 +140,16 @@ TEST(MapYaml, save_3d)
q2 << 0, 1, 0, 0;
q3 << 0, 0, 1, 0;
- auto p1_sb = std::make_shared<StateBlock>(p1);
- auto p2_sb = std::make_shared<StateBlock>(p2);
+ auto p1_sb = std::make_shared<StatePoint2d>(p1);
+ auto p2_sb = std::make_shared<StatePoint2d>(p2);
auto q2_sb = std::make_shared<StateQuaternion>(q2);
- auto p3_sb = std::make_shared<StateBlock>(p3, true);
+ auto p3_sb = std::make_shared<StatePoint2d>(p3, true);
auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, q2_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, q3_sb);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
// Check Problem functions
std::string filename = map_path + "/map_3d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
@@ -202,99 +165,60 @@ TEST(MapYaml, load_3d)
{
ProblemPtr problem = Problem::create("PO", 3);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
- // Check Problem functions
- std::string filename = map_path + "/map_3d_problem.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->loadMap(filename);
-
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk->id() == 1)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- }
- else if (lmk->id() == 2)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
- }
- else if (lmk->id() == 3)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
- }
- }
+ std::vector<std::string> maps{"map_3d_map", "map_3d_problem", "map_3d_manual"};
- // empty the map
+ for (auto map : maps)
{
- auto lmk_list = problem->getMap()->getLandmarkList();
- for (auto lmk : lmk_list)
- lmk->remove();
- }
- ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+ std::string filename = map_path + "/" + map + ".yaml";
- // Check Map functions
- filename = map_path + "/map_3d_map.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->getMap()->load(filename);
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->loadMap(filename);
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk->id() == 1)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- }
- else if (lmk->id() == 2)
+ for (auto lmk : problem->getMap()->getLandmarkList())
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
}
- else if (lmk->id() == 3)
+
+ // empty the map
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ auto lmk_list = problem->getMap()->getLandmarkList();
+ for (auto lmk : lmk_list)
+ lmk->remove();
}
+ ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
}
}
TEST(MapYaml, remove_map_files)
{
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
std::string filename = map_path + "/map_2d_problem.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
filename = map_path + "/map_2d_map.yaml";
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 522246910cc6bd3944f6924a832fbf028cb2b75c..2d852b31748b9ec2320bcf10f89f042687206c68 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -43,6 +43,10 @@
#include "core/capture/capture_diff_drive.h"
#include "core/factor/factor_diff_drive.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+#include "core/landmark/landmark_base.h"
+#include "core/math/SE3.h"
#include <iostream>
@@ -388,7 +392,7 @@ TEST(Problem, perturb)
for (int j = 0; j< 2 ; j++)
{
- auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto sb = std::make_shared<StatePoint3d>(Vector3d(1,1,1));
auto cap = CaptureBase::emplace<CaptureBase>(F, "CaptureBase", t, sensor, nullptr, nullptr, sb);
}
i++;
@@ -396,8 +400,8 @@ TEST(Problem, perturb)
for (int l = 0; l < 2; l++)
{
- auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
- auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto sb1 = std::make_shared<StatePoint2d>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateAngle>(3);
auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
if (l==0) L->fix();
}
@@ -479,7 +483,7 @@ TEST(Problem, check)
for (int j = 0; j< 2 ; j++)
{
- auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto sb = std::make_shared<StatePoint3d>(Vector3d(1,1,1));
auto cap = CaptureBase::emplace<CaptureDiffDrive>(F, t, sensor, Vector2d(1,2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
for (int k = 0; k<2; k++)
@@ -495,8 +499,8 @@ TEST(Problem, check)
for (int l = 0; l < 2; l++)
{
- auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
- auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto sb1 = std::make_shared<StatePoint2d>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateAngle>(3);
auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
if (l==0) L->fix();
}
@@ -612,10 +616,128 @@ TEST(Problem, getState)
}
+TEST(Problem, transform)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ auto S = P->getHardware()->getSensorList().front();
+
+ // Add dynamic sensor params
+ Vector3d param_sensor = Vector3d(3, 2, 1);
+ S->addStateBlock('b', std::make_shared<StateParams3>(param_sensor, false), true); // Estimated ; Dynamic
+
+ Vector6d data;
+ data << 0, 0.2, 0, 0, 0, 0; // advance on Y
+ auto C = std::make_shared<CaptureOdom3d>(0.0, S, Vector6d::Zero(), 0.01 * Matrix6d::Identity());
+
+ // Move until we get one keyframe
+ for (TimeStamp t = 0.0; t <= 3.0; t += 0.2)
+ {
+ C->setTimeStamp(t);
+ if (t > 0.0) C->setData(data); // first time data is zero!
+ C->process();
+ }
+
+ // Add dynamic sensor params to captures
+ for (auto t_f : P->getTrajectory()->getFrameMap())
+ {
+ for (auto c : t_f.second->getCapturesOf(S))
+ {
+ c->addStateBlock('b', std::make_shared<StateParams3>(S->getStateBlock('b')->getState()), P);
+ }
+ }
+
+ // Add a couple of lmks
+ LandmarkBase::emplace<LandmarkBase>(P->getMap(), //
+ "Point3d",
+ std::make_shared<StatePoint3d>(Vector3d(1, 2, 3)));
+ LandmarkBase::emplace<LandmarkBase>(P->getMap(),
+ "Pose3d",
+ std::make_shared<StatePoint3d>(Vector3d(1, 2, 3)),
+ std::make_shared<StateQuaternion>(Vector4d(0, 0, 0, 1)));
+
+ // P->print(2, 0, 1, 1); // reference
+
+ ///////////////////////////
+ // Define transformations
+
+ // translation
+ Quaterniond q_w1_w0 = Quaterniond::Identity(); // no rotation
+ VectorComposite pose_w1_w0("PO", {Vector3d(1, 0, 0), q_w1_w0.coeffs()}); // translation X: 1m
+
+ // rotation
+ Quaterniond q_w2_w1(AngleAxisd(M_PI_2, Vector3d(1, 0, 0))); // rotation X: 90deg
+ VectorComposite pose_w2_w1("PO", {Vector3d(0, 0, 0), q_w2_w1.coeffs()}); // no translation,
+
+ // rotation + translation
+ Quaterniond q_w3_w2(AngleAxisd(-M_PI_2, Vector3d(1, 0, 0))); // rotation X: -90deg
+ VectorComposite pose_w3_w2("PO", {Vector3d(1, 0, 0), q_w3_w2.coeffs()}); // translation X: 1m
+
+ // State of the problem in the initial frame "w0"
+ VectorComposite pose_w0_r0 = P->getTrajectory()->getFirstFrame()->getState();
+ VectorComposite pose_w0_r1 = P->getTrajectory()->getLastFrame()->getState();
+ Vector3d p_w0_l0 = P->getMap()->getLandmarkList().front()->getState("P").vector("P"); // Point3d
+ VectorComposite pose_w0_l1 = P->getMap()->getLandmarkList().back()->getState();
+
+ //////////////////////
+ // Apply successive changes of reference frame w0->w1->w2->w3 and compare with transformed states
+ //////////////////////
+
+ //////////////////////
+ // w0->w1: Translation
+ P->transform(pose_w1_w0);
+ // Expected values
+ VectorComposite pose_w1_r0 = SE3::compose(pose_w1_w0, pose_w0_r0);
+ VectorComposite pose_w1_r1 = SE3::compose(pose_w1_w0, pose_w0_r1);
+ Vector3d p_w1_l0 = pose_w1_w0.at('P') + q_w1_w0 * p_w0_l0;
+ VectorComposite pose_w1_l1 = SE3::compose(pose_w1_w0, pose_w0_l1);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w1_r0.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w1_r1.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w1_l0, 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w1_l1.vector("PO"), 1e-10);
+ // P->print(2, 0, 1, 1);
+
+ //////////////////////
+ // w1->w2: Rotation
+ P->transform(pose_w2_w1);
+ // Expected values
+ VectorComposite pose_w2_r0 = SE3::compose(pose_w2_w1, pose_w1_r0);
+ VectorComposite pose_w2_r1 = SE3::compose(pose_w2_w1, pose_w1_r1);
+ Vector3d p_w2_l0 = pose_w2_w1.at('P') + q_w2_w1 * p_w1_l0;
+ VectorComposite pose_w2_l1 = SE3::compose(pose_w2_w1, pose_w1_l1);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w2_r0.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w2_r1.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w2_l0, 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w2_l1.vector("PO"), 1e-10);
+ // P->print(2, 0, 1, 1);
+
+ //////////////////////
+ // w2->w3: Translation + Rotation
+ P->transform(pose_w3_w2);
+ // Expected values
+ VectorComposite pose_w3_r0 = SE3::compose(pose_w3_w2, pose_w2_r0);
+ VectorComposite pose_w3_r1 = SE3::compose(pose_w3_w2, pose_w2_r1);
+ Vector3d p_w3_l0 = pose_w3_w2.at('P') + q_w3_w2 * p_w2_l0;
+ VectorComposite pose_w3_l1 = SE3::compose(pose_w3_w2, pose_w2_l1);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w3_r0.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w3_r1.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w3_l0, 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w3_l1.vector("PO"), 1e-10);
+ // P->print(2, 0, 1, 1);
+
+ // sensor parameters should not be transformed
+ ASSERT_MATRIX_APPROX(S->getStateBlock('b')->getState(), param_sensor, 1e-10);
+
+}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index 7e1e8624ec6c9d10711875ca85fe256adcdf2ba6..1b287910cdb0ed08015ffd1831159af6325d88bd 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -127,8 +127,12 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
// pose sensor and proc (to get extrinsics in the prb)
auto intr_sp = std::make_shared<ParamsSensorPose>();
- intr_sp->std_p = 1;
- intr_sp->std_o = 1;
+ intr_sp->std_px = 1;
+ intr_sp->std_py = 1;
+ intr_sp->std_pz = 1;
+ intr_sp->std_ox = 1;
+ intr_sp->std_oy = 1;
+ intr_sp->std_oz = 1;
Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>();
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 89341815653b03ed0d189b7d91c8e1c759f9bd9f..5840118ff7c4b4ee12ed7413f2ae3e64e896ffd0 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -36,6 +36,8 @@
#include "core/capture/capture_void.h"
#include "core/problem/problem.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
// STL
#include <iterator>
@@ -104,9 +106,9 @@ TEST(ProcessorBase, KeyFrameCallback)
// Install tracker (sensor and processor)
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorTrackerDummy",
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateAngle>(0),
+ std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
proc_trk_params->time_tolerance = dt/2;
auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk, proc_trk_params);
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 0fcf734675b5b4f3f4fa5d3a46e0b6f5d5fa0cc0..9389febe08bac0e5d0e23caff425140a6eacebd2 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -104,16 +104,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
}
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
- {
- return Base::emplaceFeature(_capture_own);
- }
-
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override
- {
- return Base::emplaceFactor(_feature_motion, _capture_origin);
- }
+ // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+ // {
+ // return Base::emplaceFeature(_capture_own);
+ // }
+
+ // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ // CaptureBasePtr _capture_origin) override
+ // {
+ // return Base::emplaceFactor(_feature_motion, _capture_origin);
+ // }
ParamsProcessorDiffDrivePtr getParams()
{
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index b2bcb1990ccd266756c7f106478b79aadff3cc88..4f7500423edb371503bafc23169b12775ff3d5c5 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -20,15 +20,17 @@
//
//--------LICENSE_END--------
+#include "core/processor/processor_fixed_wing_model.h"
+
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
// STL
#include <iterator>
#include <iostream>
-#include "../include/core/processor/processor_fixed_wing_model.h"
using namespace wolf;
using namespace Eigen;
@@ -52,7 +54,7 @@ class ProcessorFixWingModelTest : public testing::Test
// Emplace sensor
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
- std::make_shared<StateBlock>(Vector3d::Zero()),
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
std::make_shared<StateQuaternion>((Vector4d() << 0,0,0,1).finished()),
nullptr,
2);
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..201e8f61a26fe70aac63d99365668ea3cc2968c0
--- /dev/null
+++ b/test/gtest_processor_landmark_external.cpp
@@ -0,0 +1,526 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/landmark/landmark_base.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/processor/processor_odom_3d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+
+#include "core/processor/processor_landmark_external.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class ProcessorLandmarkExternalTest : public testing::Test
+{
+ protected:
+ int dim;
+ bool orientation;
+ TimeStamp t;
+ double dt;
+
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor, sensor_odom;
+ ProcessorLandmarkExternalPtr processor;
+ ProcessorMotionPtr processor_motion;
+ std::vector<LandmarkBasePtr> landmarks;
+
+ // Groundtruth states
+ VectorComposite state_robot, state_sensor;
+ std::vector<VectorComposite> state_landmarks;
+
+ virtual void SetUp() {};
+ void initProblem(int _dim,
+ bool _orientation,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _add_landmarks);
+ void randomStep();
+ CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
+ void testConfiguration(int dim,
+ bool orientation,
+ double quality_th,
+ double dist_th,
+ int track_length,
+ double time_span,
+ bool add_landmarks);
+ void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
+};
+
+void ProcessorLandmarkExternalTest::initProblem(int _dim,
+ bool _orientation,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _add_landmarks)
+{
+ dim = _dim;
+ orientation = _orientation;
+ t = TimeStamp(0);
+ dt = 0.1;
+
+ problem = Problem::create("PO", dim);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // Sensors
+ if (dim == 2)
+ {
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StatePoint2d>(Vector2d::Random()),
+ std::make_shared<StateAngle>(Vector1d::Random() * M_PI),
+ nullptr,
+ 2);
+ sensor_odom = SensorBase::emplace<SensorOdom2d>(problem->getHardware(),
+ Vector3d::Zero(),
+ ParamsSensorOdom2d());
+
+ }
+ else
+ {
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StatePoint3d>(Vector3d::Random()),
+ std::make_shared<StateQuaternion>(Vector4d::Random().normalized()),
+ nullptr,
+ 3);
+ sensor_odom = SensorBase::emplace<SensorOdom3d>(problem->getHardware(),
+ (Vector7d() << 0,0,0,0,0,0,1).finished(),
+ ParamsSensorOdom3d());
+ }
+
+ // Processors
+ ParamsProcessorLandmarkExternalPtr params = std::make_shared<ParamsProcessorLandmarkExternal>();
+ params->time_tolerance = dt/2;
+ params->max_new_features = -1;
+ params->voting_active = true;
+ params->apply_loss_function = false;
+ params->use_orientation = orientation;
+ params->filter_quality_th = _quality_th;
+ params->match_dist_th = _dist_th;
+ params->min_features_for_keyframe = 1;
+ params->new_features_for_keyframe = 1000;
+ params->filter_track_length_th = _track_length_th;
+ params->time_span = _time_span;
+ processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
+
+ if (dim == 2)
+ {
+ auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
+ params_odom->state_getter = true;
+ params_odom->voting_active = false;
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom,
+ params_odom));
+ }
+ else
+ {
+ auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
+ params_odom->state_getter = true;
+ params_odom->voting_active = false;
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3d>(sensor_odom,
+ params_odom));
+ }
+
+ // Emplace frame
+ auto F0 = problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0,0,0,0,0,0,1).finished()));
+ F0->fix();
+ processor->keyFrameCallback(F0);
+ processor_motion->setOrigin(F0);
+
+ // Emplace 3 random landmarks
+ for (auto i = 0; i < 3; i++)
+ {
+ LandmarkBasePtr lmk;
+ if (dim == 2)
+ lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
+ "LandmarkExternal",
+ std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
+ (orientation ?
+ std::make_shared<StateAngle>(Vector1d::Random() * M_PI) :
+ nullptr));
+
+ else
+ lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
+ "LandmarkExternal",
+ std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
+ (orientation ?
+ std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) :
+ nullptr));
+ lmk->setId(i);
+ landmarks.push_back(lmk);
+ state_landmarks.push_back(lmk->getState());
+ }
+
+ // Store groundtruth poses
+ state_robot = F0->getState("PO");
+ state_sensor = sensor->getState("PO");
+}
+
+void ProcessorLandmarkExternalTest::randomStep()
+{
+ // compute delta
+ VectorXd delta;
+ MatrixXd delta_cov;
+ if (dim == 2)
+ {
+ delta = Vector2d::Random() * 0.1;
+ delta_cov = Matrix2d::Identity() * 0.1;
+ }
+ else
+ {
+ delta = Vector7d::Random() * 0.1;
+ delta.tail<4>().normalize();
+ delta_cov = Matrix6d::Identity() * 0.1;
+ }
+
+ // Process odometry
+ auto cap_odom = std::make_shared<CaptureMotion>("CaptureOdom", t, sensor_odom, delta, delta_cov, nullptr);
+ cap_odom->process();
+
+ // update groundtruth pose with random movement
+ state_robot = processor_motion->getState("PO");
+}
+
+CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmarks() const
+{
+ // Detections
+ auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
+ for (auto i = 0; i < landmarks.size(); i++)
+ {
+ // compute detection
+ VectorXd meas(orientation?(dim==2?3:7):(dim==2?2:3));
+ if (dim == 2)
+ {
+ meas.head(2) = Rotation2Dd(-state_sensor.at('O')(0))*(Rotation2Dd(-state_robot.at('O')(0))*(state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ if (orientation)
+ meas(2) = state_landmarks.at(i).at('O')(0) - state_robot.at('O')(0) - state_sensor.at('O')(0);
+ }
+ else
+ {
+ auto q_sensor = Quaterniond(Vector4d(state_sensor.at('O')));
+ auto q_robot = Quaterniond(Vector4d(state_robot.at('O')));
+
+ meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ if (orientation)
+ meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O')))).coeffs();
+ }
+ // cov
+ MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
+ if (orientation and dim != 2)
+ cov = MatrixXd::Identity(6, 6);
+
+ // quality
+ double quality = (double) i / (double) (landmarks.size()-1); // increasing from 0 to 1
+
+ // add detection
+ cap->addDetection(landmarks.at(i)->id(), meas, cov, quality );
+ }
+
+ return cap;
+}
+
+void ProcessorLandmarkExternalTest::testConfiguration(int dim,
+ bool orientation,
+ double quality_th,
+ double dist_th,
+ int track_length,
+ double time_span,
+ bool add_landmarks)
+{
+ initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks);
+
+ ASSERT_TRUE(problem->check());
+
+ for (auto i = 0; i<10; i++)
+ {
+ WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
+
+ // detection of landmarks
+ auto cap = computeCaptureLandmarks();
+ ASSERT_TRUE(problem->check());
+
+ // process detections
+ cap->process();
+ ASSERT_TRUE(problem->check());
+ problem->print(4,1,1,1);
+
+ // CHECKS
+ FactorBasePtrList fac_list;
+ problem->getTrajectory()->getFactorList(fac_list);
+ // should emplace KF in last?
+ bool any_active_track = quality_th <=1 and i >= track_length-1;
+ bool should_emplace_KF = t-dt > time_span and any_active_track;
+ WOLF_INFO("should emplace KF: ", should_emplace_KF, " t-dt = ", t-dt, " time_span = ", time_span, " track_length-1 = ", track_length-1);
+
+ if (should_emplace_KF)
+ {
+ // voted for keyframe
+ ASSERT_TRUE(problem->getTrajectory()->size() > 1);
+
+ // emplaced factors
+ ASSERT_FALSE(fac_list.empty());
+
+ // factors' type
+ if (should_emplace_KF)
+ for (auto fac : fac_list)
+ {
+ if (fac->getProcessor() == processor)
+ {
+ ASSERT_EQ(fac->getType(), std::string("FactorRelative") +
+ (orientation ? "Pose" : "Position") +
+ (dim == 2 ? "2d" : "3d") +
+ "WithExtrinsics");
+ }
+ }
+ // landmarks created by processor
+ if (not add_landmarks)
+ {
+ auto landmarks_map = problem->getMap()->getLandmarkList();
+ // amount of landmarks due to quality filtering of detections
+ auto n_landmarks = (quality_th <= 0 ? 3 : (quality_th <= 0.5 ? 2 : (quality_th <= 1 ? 1 : 0)));
+ ASSERT_EQ(landmarks_map.size(), n_landmarks);
+
+ // check state correctly initialized
+ for (auto lmk_map : landmarks_map)
+ {
+ ASSERT_TRUE(lmk_map->id() < landmarks.size());
+ ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id())->id());
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ }
+ }
+ }
+ else
+ {
+ // didn't vote for keyframe
+ ASSERT_FALSE(problem->getTrajectory()->size() > 1);
+ // no factors emplaced
+ ASSERT_TRUE(fac_list.empty());
+ // no landmarks created yet (if not added by the test)
+ ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
+ }
+
+ // step with random movement
+ t+=dt;
+ randomStep();
+
+ // solve
+ if (should_emplace_KF)
+ {
+ // perturb landmarks
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ lmk->perturb();
+
+ // fix frame and extrinsics
+ sensor->fix();
+ problem->getLastFrame()->fix();
+
+ // solve
+ auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ // check values
+ //assertVectorComposite(sensor->getState("PO"), state_sensor);
+ //assertVectorComposite(problem->getState("PO"), state_robot);
+ for (auto lmk_map : problem->getMap()->getLandmarkList())
+ {
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ }
+ }
+ }
+}
+
+void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
+{
+ if (v1.includesStructure("PO") and v2.includesStructure("PO"))
+ {
+ if (dim == 2)
+ {
+ ASSERT_POSE2d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
+ else
+ {
+ ASSERT_POSE3d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
+ }
+ else if (v1.includesStructure("P") and v2.includesStructure("P"))
+ {
+ ASSERT_MATRIX_APPROX(v1.vector("P"), v2.vector("P"), Constants::EPS);
+ }
+ else
+ throw std::runtime_error("wrong vector composite");
+}
+
+// / TESTS //////////////////////////////////////////
+TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks)
+{
+ testConfiguration(2, //int dim
+ false, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 6, //int track_length
+ 4.5*dt, //double time_span
+ true); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks)
+{
+ testConfiguration(2, //int dim
+ false, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 2, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality)
+{
+ testConfiguration(2, //int dim
+ false, //bool orientation
+ 0.3, //double quality_th
+ 1e6, //double dist_th
+ 6, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks)
+{
+ testConfiguration(2, //int dim
+ true, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 3, //int track_length
+ 4.5*dt, //double time_span
+ true); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks)
+{
+ testConfiguration(2, //int dim
+ true, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 1, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality)
+{
+ testConfiguration(2, //int dim
+ true, //bool orientation
+ 0.3, //double quality_th
+ 1e6, //double dist_th
+ 0, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks)
+{
+ testConfiguration(3, //int dim
+ false, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 7, //int track_length
+ 4.5*dt, //double time_span
+ true); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks)
+{
+ testConfiguration(3, //int dim
+ false, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 53, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality)
+{
+ testConfiguration(3, //int dim
+ false, //bool orientation
+ 0.3, //double quality_th
+ 1e6, //double dist_th
+ 2, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks)
+{
+ testConfiguration(3, //int dim
+ true, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 1, //int track_length
+ 4.5*dt, //double time_span
+ true); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks)
+{
+ testConfiguration(3, //int dim
+ true, //bool orientation
+ 0, //double quality_th
+ 1e6, //double dist_th
+ 4, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality)
+{
+ testConfiguration(3, //int dim
+ true, //bool orientation
+ 0.2, //double quality_th
+ 1e6, //double dist_th
+ 5, //int track_length
+ 4.5*dt, //double time_span
+ false); //bool add_landmarks
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 9623ecbaa1dd1560650c8cb9e96c044c87d18e9b..57a0d4cbdb0dc23509fb4ef9f7f691d14c36cbea 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -24,6 +24,8 @@
#include "core/problem/problem.h"
#include "core/capture/capture_base.h"
#include "core/factor/factor_relative_pose_2d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "dummy/processor_loop_closure_dummy.h"
@@ -47,8 +49,8 @@ class ProcessorLoopClosureTest : public testing::Test
// Emplace sensor
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
- std::make_shared<StateBlock>(Vector2d::Zero()),
- std::make_shared<StateBlock>(Vector1d::Zero()),
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(0),
nullptr,
2);
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 4ed3c0846017e15fe5882607ff1807fbbbbc93f5..69ed41d60e75af3346156192b145dff16c15bf31 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -25,6 +25,8 @@
#include "core/sensor/factory_sensor.h"
#include "dummy/processor_tracker_landmark_dummy.h"
#include "core/capture/capture_void.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -285,8 +287,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
Eigen::Vector1d::Ones(),
Eigen::MatrixXd::Ones(1, 1)));
LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("LandmarkBase",
- std::make_shared<StateBlock>(1),
- std::make_shared<StateBlock>(1)));
+ std::make_shared<StateParams1>(Vector1d(0)),
+ std::make_shared<StateAngle>(0)));
FactorBasePtr fac = processor->callEmplaceFactor(ftr, lmk);
ASSERT_EQ(fac->getFeature(),ftr);
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index bdc5e696b1e4a012abdee9fc93b16bd647b85172..7ec49fdee46c118d8d7907e17a43f5a2c598e661 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -813,6 +813,6 @@ int main(int argc, char **argv)
*/
testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
+ // ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index d37c700a7daf6a45862a4d8b830b8b8e67767282..ad32fad28a18655ac16993733e6fa371ef805d92 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -51,24 +51,32 @@ TEST(Pose, constructor)
TEST(Pose, getParams)
{
auto intr = std::make_shared<ParamsSensorPose>();
- intr->std_p = 2;
- intr->std_o = 3;
+ intr->std_px = 2;
+ intr->std_py = 2;
+ intr->std_pz = 2;
+ intr->std_ox = 3;
+ intr->std_oy = 3;
+ intr->std_oz = 3;
Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
auto sen = std::make_shared<SensorPose>(extr, intr);
- ASSERT_EQ(sen->getStdP(), intr->std_p);
- ASSERT_EQ(sen->getStdO(), intr->std_o);
- // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
- // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+ // ASSERT_EQ(sen->getStdP(), intr->std_p);
+ // ASSERT_EQ(sen->getStdO(), intr->std_o);
+ ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
+ ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
}
TEST(Pose, create)
{
auto intr = std::make_shared<ParamsSensorPose>();
- intr->std_p = 2;
- intr->std_o = 3;
+ intr->std_px = 2;
+ intr->std_py = 2;
+ intr->std_pz = 2;
+ intr->std_ox = 3;
+ intr->std_oy = 3;
+ intr->std_oz = 3;
Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
@@ -76,10 +84,10 @@ TEST(Pose, create)
auto sen = std::static_pointer_cast<SensorPose>(sen_base);
- ASSERT_EQ(sen->getStdP(), intr->std_p);
- ASSERT_EQ(sen->getStdO(), intr->std_o);
- // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
- // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+ // ASSERT_EQ(sen->getStdP(), intr->std_p);
+ // ASSERT_EQ(sen->getStdO(), intr->std_o);
+ ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
+ ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
}
int main(int argc, char **argv)
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index 89bf93042391ae90c0f12560922795029ff6d704..3fb6f4abadd3528878978b95764fc1359aaf9148 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -30,7 +30,7 @@
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_quaternion_absolute.h"
@@ -57,7 +57,7 @@ using namespace Eigen;
StateBlockPtr createStateBlock()
{
Vector4d state; state << 1, 0, 0, 0;
- return std::make_shared<StateBlock>(state);
+ return std::make_shared<StateParams4>(state);
}
FactorBasePtr createFactor(StateBlockPtr sb_ptr)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index e66342ba48d3ba7e2c6248e249e364540c1c9537..65056864243e2104071e4a5349961efb56d6b7aa 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -37,6 +37,8 @@
#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/solver_manager_dummy.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
#include <thread>
@@ -52,7 +54,7 @@ using namespace Eigen;
StateBlockPtr createStateBlock()
{
Vector4d state; state << 1, 0, 0, 0;
- return std::make_shared<StateBlock>(state);
+ return std::make_shared<StateParams4>(state);
}
FactorBasePtr createFactor(StateBlockPtr sb_ptr)
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index 9bce46a4856e123124e3b62aed99c4e2a09a7b01..17ad54a8ce833aecb23ac1735b2d4c7c93439d59 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -28,34 +28,34 @@
#include "core/utils/utils_gtest.h"
-
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_composite.h"
#include <iostream>
-
using namespace Eigen;
using namespace std;
using namespace wolf;
-TEST(StateBlock, block_perturb)
+TEST(StateBlock, point_perturb)
{
- Vector3d x(10,20,30);
- StateBlock sb(x);
+ Vector3d x(10, 20, 30);
+ StatePoint3d sb(x);
sb.perturb(0.5);
WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
ASSERT_NE(x.norm(), sb.getState().norm());
- ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
+ ASSERT_MATRIX_APPROX(x, sb.getState(), 0.5 * 4); // 4-sigma test...
}
TEST(StateBlock, angle_perturb)
{
- Vector1d x(1.0);
+ Vector1d x(1.0);
StateAngle sb(x(0));
sb.perturb(0.1);
@@ -63,28 +63,174 @@ TEST(StateBlock, angle_perturb)
WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
ASSERT_NE(x.norm(), sb.getState().norm());
- ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
+ ASSERT_MATRIX_APPROX(x, sb.getState(), 0.1 * 4); // 4-sigma test...
}
TEST(StateBlock, quaternion_perturb)
{
- Vector4d x(0.5,0.5,0.5,0.5);
+ Vector4d x(0.5, 0.5, 0.5, 0.5);
StateQuaternion sb(x);
sb.perturb(0.01);
WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
- ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
- ASSERT_MATRIX_APPROX(x , sb.getState() , 0.01 * 4); // 4-sigma test...
+ ASSERT_LT((sb.getState().transpose() * x).norm(), 1.0); // quaternions are not parallel ==> not equal
+ ASSERT_MATRIX_APPROX(x, sb.getState(), 0.01 * 4); // 4-sigma test...
}
-int main(int argc, char **argv)
+TEST(StatePoint3d, transformable)
+{
+ Vector3d x(1, 2, 3);
+
+ // default is transformable
+ StatePoint3d sb_tr(x);
+ ASSERT_TRUE(sb_tr.isTransformable());
+
+ // not fixed, non transformable
+ StatePoint3d sb_ntr(x, false, false);
+ ASSERT_FALSE(sb_ntr.isTransformable());
+
+ // not fixed, transformable
+ StatePoint3d sb_tr2(x, false, true);
+ ASSERT_TRUE(sb_tr2.isTransformable());
+}
+
+TEST(StatePoint3d, transform_identity)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 0, 0, 0, 0, 0, 1;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x0, 1e-15);
+}
+
+TEST(StatePoint3d, transform_translation_4x5y6z)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 4, 5, 6, 0, 0, 0, 1;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(5, 7, 9); // x0 translated (4,5,6)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint3d, transform_rotation_90x)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 0, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(1, -3, 2); // x0 rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
}
+TEST(StatePoint3d, transform_translation_1y_rotation_90x)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(1, -2, 2); // x0 translated (0,1,0) and rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint3d, non_transformable_translation_rotation)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0, false, false); // non transformable
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+ Vector3d x(1, 2, 3); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateVector3d, transform_translation_1y_rotation_90x)
+{
+ Vector3d x0(1, 2, 3);
+ StateVector3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(1, -3, 2); // x0 NOT translated (0,1,0) and rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint2d, transform_translation_1y_rotation_90)
+{
+ Vector2d x0(1, 2);
+ StatePoint2d sb(x0);
+
+ Vector3d xtrf;
+ xtrf << 0, 1, M_PI / 2;
+ VectorComposite trf(xtrf, "PO", {2, 1});
+
+ Vector2d x(-2, 2); // x0 translated (0,1) and rotated 90 deg
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateVector2d, transform_translation_1y_rotation_90)
+{
+ Vector2d x0(1, 2);
+ StateVector2d sb(x0);
+
+ Vector3d xtrf;
+ xtrf << 0, 1, M_PI / 2;
+ VectorComposite trf(xtrf, "PO", {2, 1});
+
+ Vector2d x(-2, 1); // x0 NON translated (0,1) and rotated 90 deg
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateParams4, transform_translation_1y_rotation_90x)
+{
+ Vector4d x0(1, 2, 3, 4);
+ StateParams<4> sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector4d x(1, 2, 3, 4); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 74dfe643ab18b1658a774fa782fe22e0a9284a66..5631d5fddeb26014baeb2698817481a602239a5e 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -27,6 +27,7 @@
*/
#include "core/state_block/state_composite.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/utils/utils_gtest.h"
@@ -46,11 +47,11 @@ class StateBlockCompositeInit : public testing::Test
void SetUp() override
{
- sbp = states.emplace('P', Vector3d(1,2,3));
- sbv = states.emplace('V', Vector3d(4,5,6));
+ sbp = states.emplace<StatePoint3d>('P', Vector3d(1,2,3));
+ sbv = states.emplace<StateVector3d>('V', Vector3d(4,5,6));
sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5));
- sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
+ sbx = std::make_shared<StateVector3d>(Vector3d(7,8,9));
}
};
diff --git a/test/gtest_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
index 21a4f89781b017807033f8c33d59dff716ec68ac..f558c5764668e4833274f0b3daba34e40406a176 100644
--- a/test/gtest_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.cpp
@@ -30,7 +30,7 @@
#include "core/utils/utils_gtest.h"
#include "core/yaml/yaml_conversion.h"
#include <yaml-cpp/yaml.h>
-#include <eigen3/Eigen/Dense>
+#include <Eigen/Dense>
#include <iostream>
//#include <fstream>
diff --git a/test/yaml/maps/map_2d_manual.yaml b/test/yaml/maps/map_2d_manual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4a2276ad153b4adfbae3679d42b1aba801f15bb3
--- /dev/null
+++ b/test/yaml/maps/map_2d_manual.yaml
@@ -0,0 +1,13 @@
+map_name: 2d map saved from Map::save()
+date_time: 21/09/2022 at 12:00:59
+useless_parameter: [1, 6, 3]
+landmarks:
+ -
+ id: 1
+ type: LandmarkBase
+ position: [1, 2]
+ position fixed: false
+ transformable: true
+ another_useless_param: "wtf"
+ - {id: 2, type: LandmarkBase, position: [3, 4], position fixed: false, orientation: [2], orientation fixed: false, transformable: true}
+ - {id: 3, type: LandmarkBase, position: [5, 6], position fixed: true, orientation: [3], orientation fixed: true, transformable: true}
\ No newline at end of file
diff --git a/test/yaml/maps/map_3d_manual.yaml b/test/yaml/maps/map_3d_manual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4b66ed90f51224d03b70cf484ac333da74e777f4
--- /dev/null
+++ b/test/yaml/maps/map_3d_manual.yaml
@@ -0,0 +1,25 @@
+#map_name: 3d map saved from Problem::saveMap()
+#date_time: 21/09/2022 at 12:00:59
+landmarks:
+ -
+ id: 10
+ type: LandmarkBase
+ position: [1, 2, 3]
+ position fixed: false
+ transformable: true
+ -
+ id: 11
+ type: LandmarkBase
+ position: [4, 5, 6]
+ position fixed: false
+ orientation: [0, 1, 0, 0]
+ orientation fixed: false
+ transformable: true
+ -
+ id: 12
+ type: LandmarkBase
+ position: [7, 8, 9]
+ position fixed: true
+ orientation: [0, 0, 1, 0]
+ orientation fixed: true
+ transformable: true
\ No newline at end of file
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
index c61eab58146a9a5917c1130a14e234afa08a01b6..0f7dd1954fc620a7670bc183337589ae52a25c46 100644
--- a/test/yaml/params_problem_autosetup.yaml
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -43,7 +43,7 @@ config:
dist_traveled: 999 # meters
angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
- unmeasured_perturbation_std: 0.00111
+ unmeasured_perturbation_std: 0.001
state_getter: true
state_priority: 1