diff --git a/CMakeLists.txt b/CMakeLists.txt
index 81a95ee8bec91fd4f546fd505b898692c305fcf4..2240fb4faae3283c64a90c0ae577b07c9d695486 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,28 +10,9 @@ SET(CMAKE_MACOSX_RPATH 1)
# The project name
-PROJECT(wolf)
+PROJECT(gnss)
-#string(COMPARE EQUAL "${CMAKE_BINARY_DIR}" "" result)
-#IF(result)
-# SET(CMAKE_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR})
-#ENDIF()
-#message(STATUS "Binary path : " ${CMAKE_BINARY_DIR})
-#
-
-#SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-#SET(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-#
-# We'll set the install prefix only is it's empty
-# which shouldn't be the case ...
-#string(COMPARE EQUAL "${CMAKE_INSTALL_PREFIX}" "" result)
-#IF(result)
- # This path is actually default on linux
-# SET(CMAKE_INSTALL_PREFIX /usr/local)
-#ENDIF()
-#message(STATUS "Installation path : " ${CMAKE_INSTALL_PREFIX})
-
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
@@ -80,9 +61,7 @@ if(BUILD_TESTS)
enable_testing()
endif()
-#+START_SRC --------------------------------------------------------------------------------------------------------------------------------
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
+MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
#CMAKE modules
@@ -100,7 +79,6 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# option(BUILD_EXAMPLES "Build examples" OFF)
set(BUILD_TESTS true)
-set(BUILD_EXAMPLES true)
# Does this has any other interest
# but for the examples ?
@@ -111,48 +89,18 @@ ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
#find dependencies.
-
+# ============EXAMPLE==================
FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
FIND_PACKAGE(Threads REQUIRED)
+FIND_PACKAGE(wolf REQUIRED)
+
FIND_PACKAGE(Ceres QUIET) #Ceres is not required
IF(Ceres_FOUND)
MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
ENDIF(Ceres_FOUND)
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
- FIND_PACKAGE(GLUT REQUIRED)
- FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
-IF(laser_scan_utils_FOUND)
- MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
-ENDIF(laser_scan_utils_FOUND)
-
-FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
-IF(raw_gps_utils_FOUND)
- MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision Utils
-FIND_PACKAGE(vision_utils QUIET)
-IF (vision_utils_FOUND)
- MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
- SET(PRINT_INFO_VU false)
- FIND_PACKAGE(OpenCV QUIET)
-ENDIF(vision_utils_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
-# YAML with yaml-cpp
INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
IF(YAMLCPP_FOUND)
MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
@@ -160,17 +108,6 @@ ELSEIF(YAMLCPP_FOUND)
MESSAGE("yaml-cpp Library NOT FOUND!")
ENDIF(YAMLCPP_FOUND)
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
- MESSAGE("glog Library FOUND: glog related sources will be built.")
- MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
- MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
- MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
FIND_PATH(
Suitesparse_INCLUDE_DIRS
NAMES SuiteSparse_config.h
@@ -201,612 +138,139 @@ ENDIF()
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
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 SECTION
+# ============EXAMPLE==================
INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-include_directories("include")
+INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIRS})
+include_directories(BEFORE "include")
include_directories(.)
-# include_directories("templinks")
+INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
IF(Ceres_FOUND)
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
ENDIF(Ceres_FOUND)
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-IF(laser_scan_utils_FOUND)
- INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-ENDIF(laser_scan_utils_FOUND)
-
-IF(raw_gps_utils_FOUND)
- INCLUDE_DIRECTORIES(${raw_gps_utils_INCLUDE_DIRS})
-ENDIF(raw_gps_utils_FOUND)
-
-IF(vision_utils_FOUND)
- INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(vision_utils_FOUND)
-
-# cereal
-IF(cereal_FOUND)
- INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-IF(Suitesparse_FOUND)
- INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(YAMLCPP_FOUND)
- INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
-IF(GLOG_FOUND)
-INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
#HEADERS
-
-SET(HDRS_BASE
- include/base/diff_drive_tools.h
- include/base/diff_drive_tools.hpp
- include/base/eigen_assert.h
- include/base/eigen_predicates.h
- include/base/factory.h
- include/base/frame_base.h
- include/base/hardware_base.h
- include/base/IMU_tools.h
- include/base/local_parametrization_angle.h
- include/base/local_parametrization_base.h
- include/base/local_parametrization_homogeneous.h
- include/base/local_parametrization_polyline_extreme.h
- include/base/local_parametrization_quaternion.h
- include/base/logging.h
- include/base/make_unique.h
- include/base/map_base.h
- include/base/motion_buffer.h
- include/base/node_base.h
- include/base/pinhole_tools.h
- include/base/problem.h
- include/base/rotations.h
- include/base/singleton.h
- include/base/state_angle.h
- include/base/state_block.h
- include/base/state_homogeneous_3D.h
- include/base/state_quaternion.h
- include/base/SE3.h
- include/base/time_stamp.h
- include/base/track_matrix.h
- include/base/trajectory_base.h
- include/base/wolf.h
- include/base/IMU_tools.h
- include/base/local_parametrization_polyline_extreme.h
+SET(HDRS_COMMON
+ )
+SET(HDRS_MATH
+ )
+SET(HDRS_UTILS
+ )
+SET(HDRS_STATE_BLOCK
)
SET(HDRS_CAPTURE
- include/base/capture/capture_base.h
- include/base/capture/capture_buffer.h
- include/base/capture/capture_pose.h
- include/base/capture/capture_void.h
- include/base/capture/capture_motion.h
- include/base/capture/capture_GPS_fix.h
- include/base/capture/capture_IMU.h
- include/base/capture/capture_odom_2D.h
- include/base/capture/capture_odom_3D.h
- include/base/capture/capture_GPS_fix.h
- include/base/capture/capture_IMU.h
- include/base/capture/capture_odom_2D.h
- include/base/capture/capture_odom_3D.h
- include/base/capture/capture_velocity.h
- include/base/capture/capture_wheel_joint_position.h
- include/base/capture/capture_gnss_fix.h
- include/base/capture/capture_gnss_single_diff.h
+ include/gnss/capture/capture_gnss_fix.h
+ include/gnss/capture/capture_gnss_single_diff.h
)
SET(HDRS_FACTOR
- include/base/factor/factor_AHP.h
- include/base/factor/factor_block_absolute.h
- include/base/factor/factor_container.h
- include/base/factor/factor_corner_2D.h
- include/base/factor/factor_epipolar.h
- include/base/factor/factor_IMU.h
- include/base/factor/factor_fix_bias.h
- include/base/factor/factor_GPS_2D.h
- include/base/factor/factor_GPS_pseudorange_3D.h
- include/base/factor/factor_GPS_pseudorange_2D.h
- include/base/factor/factor_odom_2D.h
- include/base/factor/factor_odom_2D_analytic.h
- include/base/factor/factor_odom_3D.h
- include/base/factor/factor_point_2D.h
- include/base/factor/factor_point_to_line_2D.h
- include/base/factor/factor_pose_2D.h
- include/base/factor/factor_pose_3D.h
- include/base/factor/factor_quaternion_absolute.h
- include/base/factor/factor_relative_2D_analytic.h
- include/base/factor/factor_autodiff_distance_3D.h
- include/base/factor/factor_block_absolute.h
- include/base/factor/factor_container.h
- include/base/factor/factor_corner_2D.h
- include/base/factor/factor_diff_drive.h
- include/base/factor/factor_epipolar.h
- include/base/factor/factor_IMU.h
- include/base/factor/factor_fix_bias.h
- include/base/factor/factor_GPS_2D.h
- include/base/factor/factor_GPS_pseudorange_3D.h
- include/base/factor/factor_GPS_pseudorange_2D.h
- include/base/factor/factor_odom_2D.h
- include/base/factor/factor_odom_2D_analytic.h
- include/base/factor/factor_odom_3D.h
- include/base/factor/factor_point_2D.h
- include/base/factor/factor_point_to_line_2D.h
- include/base/factor/factor_pose_2D.h
- include/base/factor/factor_pose_3D.h
- include/base/factor/factor_quaternion_absolute.h
- include/base/factor/factor_relative_2D_analytic.h
- include/base/factor/factor_analytic.h
- include/base/factor/factor_autodiff.h
- include/base/factor/factor_base.h
- include/base/factor/factor_gnss_fix_2D.h
- include/base/factor/factor_gnss_fix_3D.h
- include/base/factor/factor_gnss_single_diff_2D.h
- include/base/factor/factor_block_absolute.h
- include/base/factor/factor_container.h
- include/base/factor/factor_corner_2D.h
- include/base/factor/factor_epipolar.h
- include/base/factor/factor_IMU.h
- include/base/factor/factor_fix_bias.h
- include/base/factor/factor_GPS_2D.h
- include/base/factor/factor_GPS_pseudorange_3D.h
- include/base/factor/factor_GPS_pseudorange_2D.h
- include/base/factor/factor_odom_2D.h
- include/base/factor/factor_odom_2D_analytic.h
- include/base/factor/factor_odom_3D.h
- include/base/factor/factor_point_2D.h
- include/base/factor/factor_point_to_line_2D.h
- include/base/factor/factor_pose_2D.h
- include/base/factor/factor_pose_3D.h
- include/base/factor/factor_quaternion_absolute.h
- include/base/factor/factor_relative_2D_analytic.h
- include/base/factor/factor_autodiff_distance_3D.h
- include/base/factor/factor_block_absolute.h
- include/base/factor/factor_container.h
- include/base/factor/factor_corner_2D.h
- include/base/factor/factor_diff_drive.h
- include/base/factor/factor_epipolar.h
- include/base/factor/factor_IMU.h
- include/base/factor/factor_fix_bias.h
- include/base/factor/factor_GPS_2D.h
- include/base/factor/factor_GPS_pseudorange_3D.h
- include/base/factor/factor_GPS_pseudorange_2D.h
- include/base/factor/factor_odom_2D.h
- include/base/factor/factor_odom_2D_analytic.h
- include/base/factor/factor_odom_3D.h
- include/base/factor/factor_point_2D.h
- include/base/factor/factor_point_to_line_2D.h
- include/base/factor/factor_pose_2D.h
- include/base/factor/factor_pose_3D.h
- include/base/factor/factor_quaternion_absolute.h
- include/base/factor/factor_relative_2D_analytic.h
- include/base/factor/factor_analytic.h
- include/base/factor/factor_autodiff.h
- include/base/factor/factor_base.h
+ include/gnss/factor/factor_gnss_fix_2D.h
+ include/gnss/factor/factor_gnss_fix_3D.h
+ include/gnss/factor/factor_gnss_single_diff_2D.h
)
SET(HDRS_FEATURE
- include/base/feature/feature_corner_2D.h
- include/base/feature/feature_GPS_fix.h
- include/base/feature/feature_GPS_pseudorange.h
- include/base/feature/feature_IMU.h
- include/base/feature/feature_odom_2D.h
- include/base/feature/feature_polyline_2D.h
- include/base/feature/feature_corner_2D.h
- include/base/feature/feature_diff_drive.h
- include/base/feature/feature_GPS_fix.h
- include/base/feature/feature_GPS_pseudorange.h
- include/base/feature/feature_IMU.h
- include/base/feature/feature_odom_2D.h
- include/base/feature/feature_polyline_2D.h
- include/base/feature/feature_base.h
- include/base/feature/feature_match.h
- include/base/feature/feature_match_polyline_2D.h
- include/base/feature/feature_pose.h
- include/base/feature/feature_gnss_fix.h
- include/base/feature/feature_gnss_single_diff.h
+ include/gnss/feature/feature_gnss_fix.h
+ include/gnss/feature/feature_gnss_single_diff.h
)
SET(HDRS_LANDMARK
- include/base/landmark/landmark_base.h
- include/base/landmark/landmark_match.h
- include/base/landmark/landmark_match_polyline_2D.h
- include/base/landmark/landmark_corner_2D.h
- include/base/landmark/landmark_container.h
- include/base/landmark/landmark_line_2D.h
- include/base/landmark/landmark_polyline_2D.h
- include/base/landmark/landmark_corner_2D.h
- include/base/landmark/landmark_container.h
- include/base/landmark/landmark_line_2D.h
- include/base/landmark/landmark_polyline_2D.h
)
SET(HDRS_PROCESSOR
- include/base/processor/processor_capture_holder.h
- include/base/processor/processor_diff_drive.h
- include/base/processor/processor_frame_nearest_neighbor_filter.h
- include/base/processor/processor_IMU.h
- include/base/processor/processor_odom_2D.h
- include/base/processor/processor_odom_3D.h
- include/base/processor/processor_tracker_feature_dummy.h
- include/base/processor/processor_tracker_landmark.h
- include/base/processor/processor_tracker_landmark_dummy.h
- include/base/processor/processor_frame_nearest_neighbor_filter.h
- include/base/processor/processor_IMU.h
- include/base/processor/processor_odom_2D.h
- include/base/processor/processor_odom_3D.h
- include/base/processor/processor_tracker_feature_dummy.h
- include/base/processor/processor_tracker_landmark_dummy.h
- include/base/processor/processor_factory.h
- include/base/processor/processor_logging.h
- include/base/processor/processor_base.h
- include/base/processor/processor_factory.h
- include/base/processor/processor_loopclosure_base.h
- include/base/processor/processor_motion.h
- include/base/processor/processor_tracker_feature.h
- include/base/processor/processor_tracker.h
- include/base/processor/processor_gnss_fix.h
- include/base/processor/processor_gnss_single_diff.h
+ include/gnss/processor/processor_gnss_fix.h
+ include/gnss/processor/processor_gnss_fix.h
)
SET(HDRS_SENSOR
- include/base/sensor/sensor_base.h
- include/base/sensor/sensor_camera.h
- include/base/sensor/sensor_diff_drive.h
- include/base/sensor/sensor_GPS.h
- include/base/sensor/sensor_GPS_fix.h
- include/base/sensor/sensor_IMU.h
- include/base/sensor/sensor_odom_2D.h
- include/base/sensor/sensor_odom_3D.h
- include/base/sensor/sensor_camera.h
- include/base/sensor/sensor_GPS.h
- include/base/sensor/sensor_GPS_fix.h
- include/base/sensor/sensor_IMU.h
- include/base/sensor/sensor_odom_2D.h
- include/base/sensor/sensor_odom_3D.h
- include/base/sensor/sensor_factory.h
- include/base/sensor/sensor_gnss.h
+ include/gnss/sensor/sensor_gnss.h
)
SET(HDRS_SOLVER
- include/base/solver/solver_manager.h
)
-# [Add generic derived header before this line]
-
SET(HDRS_DTASSC
- include/base/track_matrix.h
- include/base/association/association_solver.h
- include/base/association/association_node.h
- include/base/association/association_tree.h
- include/base/association/association_nnls.h
- )
-
-SET(HDRS_CORE
- include/base/capture/capture_base.h
- include/base/capture/capture_buffer.h
- include/base/capture/capture_pose.h
- include/base/capture/capture_void.h
- include/base/factor/factor_analytic.h
- include/base/factor/factor_autodiff.h
- include/base/factor/factor_base.h
- include/base/processor/processor_factory.h
- include/base/feature/feature_base.h
- include/base/feature/feature_match.h
- include/base/feature/feature_pose.h
- include/base/frame_base.h
- include/base/hardware_base.h
- include/base/landmark/landmark_base.h
- include/base/local_parametrization_angle.h
- include/base/local_parametrization_base.h
- include/base/local_parametrization_homogeneous.h
- include/base/local_parametrization_quaternion.h
- include/base/processor/processor_logging.h
- include/base/map_base.h
- include/base/motion_buffer.h
- include/base/node_base.h
- include/base/problem.h
- include/base/processor/processor_base.h
- include/base/processor/processor_loopclosure_base.h
- include/base/processor/processor_motion.h
- include/base/processor/processor_tracker_feature.h
- include/base/processor/processor_tracker.h
- include/base/rotations.h
- include/base/sensor/sensor_base.h
- include/base/processor/processor_factory.h
- include/base/singleton.h
- include/base/state_angle.h
- include/base/state_block.h
- include/base/state_homogeneous_3D.h
- include/base/state_quaternion.h
- include/base/SE3.h
- include/base/time_stamp.h
- include/base/track_matrix.h
- include/base/trajectory_base.h
- include/base/wolf.h
)
#SOURCES
-SET(SRCS_CORE
- src/capture/capture_base.cpp
- src/capture/capture_pose.cpp
- src/capture/capture_void.cpp
- src/factor/factor_analytic.cpp
- src/factor/factor_base.cpp
- src/feature/feature_base.cpp
- src/feature/feature_pose.cpp
- src/frame_base.cpp
- src/hardware_base.cpp
- src/landmark/landmark_base.cpp
- src/local_parametrization_base.cpp
- src/local_parametrization_homogeneous.cpp
- src/local_parametrization_quaternion.cpp
- src/map_base.cpp
- src/motion_buffer.cpp
- src/node_base.cpp
- src/problem.cpp
- src/processor/processor_base.cpp
- src/processor/processor_loopclosure_base.cpp
- src/processor/processor_motion.cpp
- src/processor/processor_tracker.cpp
- src/sensor/sensor_base.cpp
- src/state_block.cpp
- src/time_stamp.cpp
- src/track_matrix.cpp
- src/trajectory_base.cpp
+SET(SRCS_COMMON
)
-
-SET(SRCS_BASE
- src/capture/capture_motion.cpp
- src/processor/processor_capture_holder.cpp
- # examples/test_processor_tracker_landmark.cpp
+SET(SRCS_MATH
)
-
-SET(SRCS
- src/local_parametrization_polyline_extreme.cpp
- test/processor_IMU_UnitTester.cpp
+SET(SRCS_UTILS
+ )
+SET(SRCS_STATE_BLOCK
)
SET(SRCS_CAPTURE
- src/capture/capture_GPS_fix.cpp
- src/capture/capture_IMU.cpp
- src/capture/capture_odom_2D.cpp
- src/capture/capture_odom_3D.cpp
- src/capture/capture_velocity.cpp
- src/capture/capture_wheel_joint_position.cpp
src/capture/capture_gnss_fix.cpp
)
+SET(SRCS_FACTOR
+ )
SET(SRCS_FEATURE
- src/feature/feature_corner_2D.cpp
- src/feature/feature_diff_drive.cpp
- src/feature/feature_GPS_fix.cpp
- src/feature/feature_GPS_pseudorange.cpp
- src/feature/feature_IMU.cpp
- src/feature/feature_odom_2D.cpp
- src/feature/feature_polyline_2D.cpp
)
SET(SRCS_LANDMARK
- src/landmark/landmark_corner_2D.cpp
- src/landmark/landmark_container.cpp
- src/landmark/landmark_line_2D.cpp
- src/landmark/landmark_polyline_2D.cpp
- src/landmark/landmark_match_polyline_2D.cpp
)
SET(SRCS_PROCESSOR
- src/processor/processor_frame_nearest_neighbor_filter.cpp
- src/processor/processor_diff_drive.cpp
- src/processor/processor_IMU.cpp
- src/processor/processor_odom_2D.cpp
- src/processor/processor_odom_3D.cpp
- src/processor/processor_tracker_feature.cpp
- src/processor/processor_tracker_feature_dummy.cpp
- src/processor/processor_tracker_landmark_dummy.cpp
- src/processor/processor_tracker_landmark.cpp
src/processor/processor_gnss_fix.cpp
src/processor/processor_gnss_single_diff.cpp
)
SET(SRCS_SENSOR
- src/sensor/sensor_camera.cpp
- src/sensor/sensor_diff_drive.cpp
- src/sensor/sensor_GPS.cpp
- src/sensor/sensor_GPS_fix.cpp
- src/sensor/sensor_IMU.cpp
- src/sensor/sensor_odom_2D.cpp
- src/sensor/sensor_odom_3D.cpp
src/sensor/sensor_gnss.cpp
)
SET(SRCS_DTASSC
- src/association/association_solver.cpp
- src/association/association_node.cpp
- src/association/association_tree.cpp
- src/association/association_nnls.cpp
)
SET(SRCS_SOLVER
- src/solver/solver_manager.cpp
+ )
+SET(SRCS_YAML
)
#OPTIONALS
#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
- include/base/solver_suitesparse/sparse_utils.h
- include/base/solver/solver_manager.h
- include/base/ceres_wrapper/ceres_manager.h
- #ceres_wrapper/qr_manager.h
- include/base/ceres_wrapper/cost_function_wrapper.h
- include/base/ceres_wrapper/create_numeric_diff_cost_function.h
- include/base/ceres_wrapper/local_parametrization_wrapper.h
- )
- SET(SRCS_WRAPPER
- src/solver/solver_manager.cpp
- src/ceres_wrapper/ceres_manager.cpp
- #ceres_wrapper/qr_manager.cpp
- src/ceres_wrapper/local_parametrization_wrapper.cpp
- )
-ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_laser_2D.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_polyline.h
- include/base/processor/processor_tracker_feature_corner.h
- include/base/processor/processor_tracker_landmark_corner.h
- include/base/processor/processor_tracker_feature_polyline_2D.h
- include/base/processor/polyline_2D_utils.h
- )
- SET(HDRS_SENSOR ${HDRS_SENSOR}
- include/base/sensor/sensor_laser_2D.h
- )
- SET(SRCS_CAPTURE ${SRCS_CAPTURE}
- src/capture/capture_laser_2D.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- src/processor/processor_polyline.cpp
- src/processor/processor_tracker_feature_corner.cpp
- src/processor/processor_tracker_landmark_corner.cpp
- src/processor/processor_tracker_feature_polyline_2D.cpp
- src/processor/polyline_2D_utils.cpp
- )
- SET(SRCS_SENSOR ${SRCS_SENSOR}
- src/sensor/sensor_laser_2D.cpp
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_GPS.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
- src/capture/capture_GPS.cpp
- src/processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_image.h
- )
- SET(HDRS_FEATURE ${HDRS_FEATURE}
- include/base/feature/feature_point_image.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_params_image.h
- include/base/processor/processor_tracker_feature_image.h
- include/base/processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
- include/base/landmark/landmark_point_3D.h
- include/base/landmark/landmark_AHP.h
- )
- SET(SRCS ${SRCS}
- src/capture/capture_image.cpp
- src/feature/feature_point_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
- src/landmark/landmark_point_3D.cpp
- src/landmark/landmark_AHP.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- src/processor/processor_tracker_feature_image.cpp
- src/processor/processor_tracker_landmark_image.cpp
- )
-ENDIF(vision_utils_FOUND)
-#SUBDIRECTORIES
-add_subdirectory(hello_wolf)
-IF (cereal_FOUND)
- ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
-IF (Suitesparse_FOUND)
- #DOES NOTHING?!
- #ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS ${HDRS}
- include/base/yaml/yaml_conversion.h
- )
- SET(HDRS_YAML ${HDRS_YAML}
- include/base/yaml/yaml_conversion.h
- )
-
- # sources
- SET(SRCS ${SRCS}
- src/yaml/processor_odom_3D_yaml.cpp
- src/yaml/processor_IMU_yaml.cpp
- src/yaml/sensor_camera_yaml.cpp
- src/yaml/sensor_odom_3D_yaml.cpp
- src/yaml/sensor_IMU_yaml.cpp
- )
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- src/yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- src/yaml/processor_image_yaml.cpp
- )
- ENDIF(vision_utils_FOUND)
-ENDIF(YAMLCPP_FOUND)
+# ==================EXAMPLE===============
+# IF (Ceres_FOUND)
+# SET(HDRS_WRAPPER
+# include/base/solver_suitesparse/sparse_utils.h
+# include/base/solver/solver_manager.h
+# include/base/ceres_wrapper/ceres_manager.h
+# include/base/ceres_wrapper/cost_function_wrapper.h
+# include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+# include/base/ceres_wrapper/local_parametrization_wrapper.h
+# )
+# SET(SRCS_WRAPPER
+# src/solver/solver_manager.cpp
+# src/ceres_wrapper/ceres_manager.cpp
+# src/ceres_wrapper/local_parametrization_wrapper.cpp
+# )
+# ELSE(Ceres_FOUND)
+# SET(HDRS_WRAPPER)
+# SET(SRCS_WRAPPER)
+# ENDIF(Ceres_FOUND)
# create the shared library
-ADD_LIBRARY(${PROJECT_NAME}
- SHARED
- ${SRCS_BASE}
- ${SRCS_CORE}
- ${SRCS}
+ADD_LIBRARY(${PROJECT_NAME}
+ SHARED
${SRCS_CAPTURE}
+ ${SRCS_COMMON}
+ ${SRCS_DTASSC}
${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_LANDMARK}
+ ${SRCS_MATH}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
- #${SRCS_DTASSC}
${SRCS_SOLVER}
+ ${SRCS_STATE_BLOCK}
+ ${SRCS_UTILS}
${SRCS_WRAPPER}
+ ${SRCS_YAML}
)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
#Link the created libraries
-#=============================================================
-IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
+#===============EXAMPLE=========================
+# IF (Ceres_FOUND)
+# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
+# ENDIF(Ceres_FOUND)
-IF (laser_scan_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${raw_gps_utils_LIBRARY})
-ENDIF (raw_gps_utils_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
- IF (vision_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${vision_utils_LIBRARY})
- ENDIF (vision_utils_FOUND)
-ENDIF (OPENCV_FOUND)
-
-IF (YAMLCPP_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
-ENDIF (YAMLCPP_FOUND)
+#Build tests
+#===============EXAMPLE=========================
IF (GLOG_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
ENDIF (GLOG_FOUND)
@@ -817,13 +281,6 @@ IF (GLOG_FOUND)
ENDIF(BUILD_TESTS)
ENDIF (GLOG_FOUND)
-IF(BUILD_EXAMPLES)
- #Build examples
- MESSAGE("Building examples.")
- ADD_SUBDIRECTORY(src/examples)
-ENDIF(BUILD_EXAMPLES)
-
-
#install library
#=============================================================
@@ -834,49 +291,55 @@ INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/wolf/base)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/state_block)
INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/base/association)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/association)
INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/base/capture)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_FACTOR}
- DESTINATION include/iri-algorithms/wolf/base/factor)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/base/feature)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/feature)
INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/base/sensor)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/sensor)
INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/base/processor)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/base/landmark)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/landmark)
INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/base/ceres_wrapper)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/ceres_wrapper)
INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
- DESTINATION include/iri-algorithms/wolf/base/solver_suitesparse)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/solver_suitesparse)
INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/base/solver)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/solver)
INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/base/serialization)
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/serialization)
INSTALL(FILES ${HDRS_YAML}
- DESTINATION include/iri-algorithms/wolf/base/yaml)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
- DESTINATION "lib/cmake/${PROJECT_NAME}")
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/yaml)
+INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
+ DESTINATION "lib/cmake/wolf${PROJECT_NAME}")
+FILE(WRITE laser.found "")
+INSTALL(FILES laser.found
+ DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME})
#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
+configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolf${PROJECT_NAME}Config.cmake"
+ "${CMAKE_BINARY_DIR}/wolf${PROJECT_NAME}Config.cmake" @ONLY)
+# configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
+# "${CMAKE_BINARY_DIR}/Findwolf${PROJECT_NAME}.cmake" @ONLY)
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/internal)
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
+# INSTALL(FILES "${CMAKE_BINARY_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
+# DESTINATION "lib/cmake/${PROJECT_NAME}")
+INSTALL(FILES "${CMAKE_BINARY_DIR}/wolf${PROJECT_NAME}Config.cmake" DESTINATION "lib/cmake/wolf${PROJECT_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-export(PACKAGE ${PROJECT_NAME})
+export(PACKAGE wolf_${PROJECT_NAME})
-#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
FIND_PACKAGE(Doxygen)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
@@ -918,19 +381,3 @@ ELSE(UNIX)
TARGET uninstall
)
ENDIF(UNIX)
-
-IF (UNIX)
- SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
- SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
- SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
- SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
- SET(CPACK_GENERATOR "DEB")
- SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu")
- SET(CPACK_SET_DESTDIR "ON") # Necessary because of the absolute install paths
- INCLUDE(CPack)
-ELSE(UNIX)
- ADD_CUSTOM_COMMAND(
- COMMENT "packaging only implemented in unix"
- TARGET uninstall)
-ENDIF(UNIX)
-
diff --git a/cmake_modules/Findwolf.cmake b/cmake_modules/Findwolf.cmake
index 19dee9a0bc06e908bffaa2231a55dfdc53718589..b0352427d7ac9e20d3c9dea85f7aca3547fa2d4c 100644
--- a/cmake_modules/Findwolf.cmake
+++ b/cmake_modules/Findwolf.cmake
@@ -1,8 +1,8 @@
#edit the following line to add the librarie's header files
FIND_PATH(
wolf_INCLUDE_DIRS
- # NAMES wolf.h
- PATHS /usr/local/include/iri-algorithms/wolf)
+ NAMES wolf.found
+ PATHS /usr/local/include/iri-algorithms/wolf/base)
#change INCLUDE_DIRS to its parent directory
get_filename_component(wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIRS} DIRECTORY)
MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}")
diff --git a/cmake_modules/Findwolfgnss.cmake b/cmake_modules/Findwolfgnss.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..d8283e3ead3531273ff8508f91b68b65b0dc6efa
--- /dev/null
+++ b/cmake_modules/Findwolfgnss.cmake
@@ -0,0 +1,36 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(
+ example_INCLUDE_DIRS
+ # NAMES wolf.found
+ PATHS /usr/local/include/iri-algorithms/wolf/example)
+#change INCLUDE_DIRS to its parent directory
+# get_filename_component(example_INCLUDE_DIRS ${example_INCLUDE_DIRS} DIRECTORY)
+IF(example_INCLUDE_DIRS)
+ MESSAGE("Found example include dirs: ${example_INCLUDE_DIRS}")
+ELSE
+ MESSAGE("Couldn't find example include dirs")
+ENDIF
+
+FIND_LIBRARY(
+ example_LIBRARY
+ NAMES libexample.so
+ PATHS /usr/lib /usr/local/lib /usr/local/lib/iri-algorithms)
+IF(example_LIBRARY)
+ MESSAGE("Found example lib: ${example_LIBRARY}")
+ELSE
+ MESSAGE("Couldn't find example lib")
+ENDIF
+IF (example_INCLUDE_DIRS AND example_LIBRARY)
+ SET(example_FOUND TRUE)
+ENDIF (example_INCLUDE_DIRS AND example_LIBRARY)
+
+IF (example_FOUND)
+ IF (NOT example_FIND_QUIETLY)
+ MESSAGE(STATUS "Found example: ${example_LIBRARY}")
+ ENDIF (NOT example_FIND_QUIETLY)
+ELSE (example_FOUND)
+ IF (wolf_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find example")
+ ENDIF (wolf_FIND_REQUIRED)
+ENDIF (example_FOUND)
+
diff --git a/cmake_modules/wolfgnssConfig.cmake b/cmake_modules/wolfgnssConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..82bb7cf0a5523302eb3d6c7fd5064e382a6c59f5
--- /dev/null
+++ b/cmake_modules/wolfgnssConfig.cmake
@@ -0,0 +1,222 @@
+# This file was copied and adapted from the ceres_solver project
+# http://ceres-solver.org/
+
+# wolf - Windowed Localization Frames
+# Copyright 2016
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Authors:
+#
+
+# Config file for wolf - Find wolf & dependencies.
+#
+# This file is used by CMake when find_package(wolf) is invoked and either
+# the directory containing this file either is present in CMAKE_MODULE_PATH
+# (if wolf was installed), or exists in the local CMake package registry if
+# the wolf build directory was exported.
+#
+# This module defines the following variables:
+#
+# wolf_FOUND / wolf_FOUND: True if wolf has been successfully
+# found. Both variables are set as although
+# FindPackage() only references wolf_FOUND
+# in Config mode, given the conventions for
+# <package>_FOUND when FindPackage() is
+# called in Module mode, users could
+# reasonably expect to use wolf_FOUND
+# instead.
+#
+# wolf_VERSION: Version of wolf found.
+#
+# wolf_INCLUDE_DIRS: Include directories for wolf and the
+# dependencies which appear in the wolf public
+# API and are thus required to use wolf.
+#
+# wolf_LIBRARIES: Libraries for wolf and all
+# dependencies against which wolf was
+# compiled. This will not include any optional
+# dependencies that were disabled when wolf was
+# compiled.
+#
+# The following variables are also defined for legacy compatibility
+# only. Any new code should not use them as they do not conform to
+# the standard CMake FindPackage naming conventions.
+#
+# wolf_INCLUDES = ${wolf_INCLUDE_DIRS}.
+
+# Called if we failed to find Ceres or any of its required dependencies,
+# unsets all public (designed to be used externally) variables and reports
+# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
+macro(wolf_report_not_found REASON_MSG)
+ # FindPackage() only references Ceres_FOUND, and requires it to be
+ # explicitly set FALSE to denote not found (not merely undefined).
+ set(wolf_FOUND FALSE)
+ set(wolf_FOUND FALSE)
+ unset(wolf_INCLUDE_DIRS)
+ unset(wolf_LIBRARIES)
+
+ # Reset the CMake module path to its state when this script was called.
+ set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+ # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+ # FindPackage() use the camelcase library name, not uppercase.
+ if (wolf_FIND_QUIETLY)
+ message(STATUS "Failed to find wolf - " ${REASON_MSG} ${ARGN})
+ else (wolf_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
+ else()
+ # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+ # that prevents generation, but continues configuration.
+ message(SEND_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
+ endif ()
+ return()
+endmacro(wolf_report_not_found)
+
+# Record the state of the CMake module path when this script was
+# called so that we can ensure that we leave it in the same state on
+# exit as it was on entry, but modify it locally.
+set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
+
+# Get the (current, i.e. installed) directory containing this file.
+get_filename_component(wolf_CURRENT_CONFIG_DIR
+ "${CMAKE_CURRENT_LIST_FILE}" PATH)
+
+# Reset CMake module path to the installation directory of this
+# script, thus we will use the FindPackage() scripts shipped with
+# wolf to find wolf' dependencies, even if the user has equivalently
+# named FindPackage() scripts in their project.
+set(CMAKE_MODULE_PATH ${wolf_CURRENT_CONFIG_DIR})
+
+# Build the absolute root install directory as a relative path
+# (determined when wolf was configured & built) from the current
+# install directory for this this file. This allows for the install
+# tree to be relocated, after wolf was built, outside of CMake.
+get_filename_component(CURRENT_ROOT_INSTALL_DIR
+ ${wolf_CURRENT_CONFIG_DIR}/../../../
+ ABSOLUTE)
+if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
+ wolf_report_not_found(
+ "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
+ "determined from relative path from wolfConfig.cmake install location: "
+ "${wolf_CURRENT_CONFIG_DIR}, does not exist. Either the install "
+ "directory was deleted, or the install tree was only partially relocated "
+ "outside of CMake after wolf was built.")
+endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
+
+# Set the include directories for wolf (itself).
+set(wolf_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include/iri-algorithms")
+
+# if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
+if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
+ wolf_report_not_found(
+ "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
+ "determined from relative path from wolfConfig.cmake install location: "
+ "${wolf_CURRENT_CONFIG_DIR}, does not contain wolf headers. "
+ "Either the install directory was deleted, or the install tree was only "
+ "partially relocated outside of CMake after wolf was built.")
+# endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
+endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
+list(APPEND wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIR}/wolf)
+
+# Set the version.
+set(wolf_VERSION 0.0.1)
+
+# Eigen.
+
+# Flag set during configuration and build of wolf.
+set(wolf_EIGEN_VERSION @EIGEN_VERSION@)
+# Append the locations of Eigen when wolf was built to the search path hints.
+list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3)
+# Search quietly to control the timing of the error message if not found. The
+# search should be for an exact match, but for usability reasons do a soft
+# match and reject with an explanation below.
+
+find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET)
+
+# Flag set with currently found Eigen version.
+set(EIGEN_VERSION @EIGEN_VERSION@)
+if (EIGEN3_FOUND)
+ if (NOT EIGEN_VERSION VERSION_EQUAL wolf_EIGEN_VERSION)
+ # CMake's VERSION check in FIND_PACKAGE() will accept any version >= the
+ # specified version. However, only version = is supported. Improve
+ # usability by explaining why we don't accept non-exact version matching.
+ wolf_report_not_found("Found Eigen dependency, but the version of Eigen "
+ "found (${EIGEN_VERSION}) does not exactly match the version of Eigen "
+ "wolf was compiled with (${wolf_EIGEN_VERSION}). This can cause subtle "
+ "bugs by triggering violations of the One Definition Rule. See the "
+ "Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule "
+ "for more details")
+ endif ()
+ message(STATUS "Found required wolf dependency: "
+ "Eigen version ${wolf_EIGEN_VERSION} in ${EIGEN3_INCLUDE_DIR}")
+else (EIGEN3_FOUND)
+ wolf_report_not_found("Missing required wolf "
+ "dependency: Eigen version ${wolf_EIGEN_VERSION}, please set "
+ "EIGEN3_INCLUDE_DIR.")
+endif (EIGEN3_FOUND)
+list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+
+# Import exported wolf targets, if they have not already been imported.
+if (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
+ include(${wolf_CURRENT_CONFIG_DIR}/wolfTargets.cmake)
+endif (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
+# Set the expected XX_LIBRARIES variable for FindPackage().
+set(wolf_LIBRARIES wolf)
+
+# Set legacy library variable for backwards compatibility.
+set(wolf_LIBRARY ${wolf_LIBRARIES})
+
+# Make user aware of any compile flags that will be added to their targets
+# which use wolf (i.e. flags exported in the wolf target). Only CMake
+# versions >= 2.8.12 support target_compile_options().
+if (TARGET ${wolf_LIBRARIES} AND
+ NOT CMAKE_VERSION VERSION_LESS "2.8.12")
+ get_target_property(wolf_INTERFACE_COMPILE_OPTIONS
+ ${wolf_LIBRARIES} INTERFACE_COMPILE_OPTIONS)
+
+ set(wolf_LOCATION "${CURRENT_ROOT_INSTALL_DIR}")
+
+ # Check for -std=c++11 flags.
+ if (wolf_INTERFACE_COMPILE_OPTIONS MATCHES ".*std=c\\+\\+11.*")
+ message(STATUS "wolf version ${wolf_VERSION} detected here: "
+ "${wolf_LOCATION} was built with C++11. wolf target will add "
+ "C++11 flags to compile options for targets using it.")
+ endif()
+endif()
+
+# Reset CMake module path to its state when this script was called.
+set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+# As we use wolf_REPORT_NOT_FOUND() to abort, if we reach this point we have
+# found wolf and all required dependencies.
+message(STATUS "Found wolf version: ${wolf_VERSION} installed in: ${CURRENT_ROOT_INSTALL_DIR}")
+
+# Set wolf_FOUND to be equivalent to wolf_FOUND, which is set to
+# TRUE by FindPackage() if this file is found and run, and after which
+# wolf_FOUND is not (explicitly, i.e. undefined does not count) set
+# to FALSE.
+set(wolf_FOUND TRUE)
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 7eabd88301bad8fdb4e5297e259c410353241f32..3708f51d3aa1141756dc54c27f76039671bfa42e 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -12,7 +12,7 @@
* \author: jsola
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_2D.h"
diff --git a/include/base/IMU_tools.h b/include/base/IMU_tools.h
deleted file mode 100644
index eecad244c7a57ee2868bcf464e951a0a5108160d..0000000000000000000000000000000000000000
--- a/include/base/IMU_tools.h
+++ /dev/null
@@ -1,610 +0,0 @@
-/*
- * imu_tools.h
- *
- * Created on: Jul 29, 2017
- * Author: jsola
- */
-
-#ifndef IMU_TOOLS_H_
-#define IMU_TOOLS_H_
-
-#include "base/wolf.h"
-#include "base/rotations.h"
-
-/*
- * Most functions in this file are explained in the document:
- *
- * Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC
- *
- * They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
- *
- * The Delta is defined as
- * Delta = [Dp, Dq, Dv]
- * with
- * Dp : position delta
- * Dq : quaternion delta
- * Dv : velocity delta
- *
- * They are listed below:
- *
- * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], Dv = [0,0,0]
- * - inverse: so that D (+) D.inv = I
- * - compose: Dc = D1 (+) D2
- * - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
- * - composeOverState: x2 = x1 (+) D
- * - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
- * - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
- * - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 (+) exp_IMU(d)
- * - diff: d = log_IMU( D2 (-) D1 )
- * - body2delta: construct a delta from body magnitudes of linAcc and angVel
- */
-
-namespace wolf
-{
-namespace imu {
-using namespace Eigen;
-
-template<typename D1, typename D2, typename D3>
-inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v)
-{
- p = MatrixBase<D1>::Zero(3,1);
- q = QuaternionBase<D2>::Identity();
- v = MatrixBase<D3>::Zero(3,1);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q, MatrixBase<D3>& v)
-{
- typedef typename D1::Scalar T1;
- typedef typename D2::Scalar T2;
- typedef typename D3::Scalar T3;
- p << T1(0), T1(0), T1(0);
- q << T2(0), T2(0), T2(0), T2(1);
- v << T3(0), T3(0), T3(0);
-}
-
-template<typename T = Scalar>
-inline Matrix<T, 10, 1> identity()
-{
- Matrix<T, 10, 1> ret;
- ret<< T(0), T(0), T(0),
- T(0), T(0), T(0), T(1),
- T(0), T(0), T(0);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T>
-inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq, const MatrixBase<D3>& dv,
- const T dt,
- MatrixBase<D4>& idp, QuaternionBase<D5>& idq, MatrixBase<D6>& idv )
-{
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(dv);
- MatrixSizeCheck<3, 1>::check(idp);
- MatrixSizeCheck<3, 1>::check(idv);
-
- idp = - ( dq.conjugate() * (dp - dv * typename D3::Scalar(dt) ) );
- idq = dq.conjugate();
- idv = - ( dq.conjugate() * dv );
-}
-
-template<typename D1, typename D2, class T>
-inline void inverse(const MatrixBase<D1>& d,
- T dt,
- MatrixBase<D2>& id)
-{
- MatrixSizeCheck<10, 1>::check(d);
- MatrixSizeCheck<10, 1>::check(id);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 7 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
- Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idv ( & id( 7 ) );
-
- inverse(dp, dq, dv, dt, idp, idq, idv);
-}
-
-template<typename D, class T>
-inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
- T dt)
-{
- Matrix<typename D::Scalar, 10, 1> id;
- inverse(d, dt, id);
- return id;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- const T dt,
- MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q, MatrixBase<D9>& sum_v )
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dv1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dv2);
- MatrixSizeCheck<3, 1>::check(sum_p);
- MatrixSizeCheck<3, 1>::check(sum_v);
-
- sum_p = dp1 + dv1*dt + dq1*dp2;
- sum_v = dv1 + dq1*dv2;
- sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& sum)
-{
- MatrixSizeCheck<10, 1>::check(d1);
- MatrixSizeCheck<10, 1>::check(d2);
- MatrixSizeCheck<10, 1>::check(sum);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1( 7 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2( 7 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
- Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( & sum( 7 ) );
-
- compose(dp1, dq1, dv1, dp2, dq2, dv2, dt, sum_p, sum_q, sum_v);
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- compose(d1, d2, dt, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, class T>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& sum,
- MatrixBase<D4>& J_sum_d1,
- MatrixBase<D5>& J_sum_d2)
-{
- MatrixSizeCheck<10, 1>::check(d1);
- MatrixSizeCheck<10, 1>::check(d2);
- MatrixSizeCheck<10, 1>::check(sum);
- MatrixSizeCheck< 9, 9>::check(J_sum_d1);
- MatrixSizeCheck< 9, 9>::check(J_sum_d2);
-
- // Some useful temporaries
- Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR
- Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
-
- // Jac wrt first delta
- J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
- J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
- J_sum_d1.block(0,6,3,3) = Matrix3s::Identity() * dt; // dDp'/dDv = I*dt
- J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
- J_sum_d1.block(6,3,3,3).noalias() = - dR1 * skew(d2.tail(3)) ; // dDv'/dDo
-
- // Jac wrt second delta
- J_sum_d2.setIdentity(); //
- J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
- J_sum_d2.block(6,6,3,3) = dR1; // dDv'/ddv
- // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
-
- // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
- compose(d1, d2, dt, sum);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- const T dt,
- MatrixBase<D7>& diff_p, QuaternionBase<D8>& diff_q, MatrixBase<D9>& diff_v )
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dv1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dv2);
- MatrixSizeCheck<3, 1>::check(diff_p);
- MatrixSizeCheck<3, 1>::check(diff_v);
-
- diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
- diff_v = dq1.conjugate() * ( dv2 - dv1 );
- diff_q = dq1.conjugate() * dq2;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& d2_minus_d1)
-{
- MatrixSizeCheck<10, 1>::check(d1);
- MatrixSizeCheck<10, 1>::check(d2);
- MatrixSizeCheck<10, 1>::check(d2_minus_d1);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & d2_minus_d1(0) );
- Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & d2_minus_d1(7) );
-
- between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v);
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> diff;
- between(d1, d2, dt, diff);
- return diff;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void composeOverState(const MatrixBase<D1>& x,
- const MatrixBase<D2>& d,
- T dt,
- MatrixBase<D3>& x_plus_d)
-{
- MatrixSizeCheck<10, 1>::check(x);
- MatrixSizeCheck<10, 1>::check(d);
- MatrixSizeCheck<10, 1>::check(x_plus_d);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & x( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > q ( & x( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > v ( & x( 7 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq ( & d( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv ( & d( 7 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > p_plus_d ( & x_plus_d( 0 ) );
- Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > v_plus_d ( & x_plus_d( 7 ) );
-
- p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
- v_plus_d = v + gravity()*dt + q*dv;
- q_plus_d = q*dq; // dq here to avoid possible aliasing between x and x_plus_d
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> composeOverState(const MatrixBase<D1>& x,
- const MatrixBase<D2>& d,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- composeOverState(x, d, dt, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-inline void betweenStates(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, const MatrixBase<D3>& v1,
- const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, const MatrixBase<D6>& v2,
- const T dt,
- MatrixBase<D7>& dp, QuaternionBase<D8>& dq, MatrixBase<D9>& dv )
-{
- MatrixSizeCheck<3, 1>::check(p1);
- MatrixSizeCheck<3, 1>::check(v1);
- MatrixSizeCheck<3, 1>::check(p2);
- MatrixSizeCheck<3, 1>::check(v2);
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(dv);
-
- dp = q1.conjugate() * ( p2 - p1 - v1*dt - (T)0.5*gravity().cast<T>()*(T)dt*(T)dt );
- dv = q1.conjugate() * ( v2 - v1 - gravity().cast<T>()*(T)dt );
- dq = q1.conjugate() * q2;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void betweenStates(const MatrixBase<D1>& x1,
- const MatrixBase<D2>& x2,
- T dt,
- MatrixBase<D3>& x2_minus_x1)
-{
- MatrixSizeCheck<10, 1>::check(x1);
- MatrixSizeCheck<10, 1>::check(x2);
- MatrixSizeCheck<10, 1>::check(x2_minus_x1);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & x1(0) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > v1 ( & x1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & x2(0) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > v2 ( & x2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & x2_minus_x1(0) );
- Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & x2_minus_x1(7) );
-
- betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv);
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1,
- const MatrixBase<D2>& x2,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- betweenStates(x1, x2, dt, ret);
- return ret;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in)
-{
- MatrixSizeCheck<10, 1>::check(delta_in);
-
- Matrix<typename Derived::Scalar, 9, 1> ret;
-
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
- Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & delta_in(7) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dv_ret ( & ret(6) );
-
- dp_ret = dp_in;
- dv_ret = dv_in;
- do_ret = log_q(dq_in);
-
- return ret;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in)
-{
- MatrixSizeCheck<9, 1>::check(d_in);
-
- Matrix<typename Derived::Scalar, 10, 1> ret;
-
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & d_in(6) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
- Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dv ( & ret(7) );
-
- dp = dp_in;
- dv = dv_in;
- dq = exp_q(do_in);
-
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2,
- MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v )
-{
- plus_p = dp1 + dp2;
- plus_v = dv1 + dv2;
- plus_q = dq1 * exp_q(do2);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d_pert)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(6) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) );
- Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dv_p ( & d_pert(7) );
-
- plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- plus(d1, d2, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v )
-{
- diff_p = dp2 - dp1;
- diff_v = dv2 - dv1;
- diff_o = log_q(dq1.conjugate() * dq2);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v ,
- MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2)
-{
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
-
- J_do_dq1 = - jac_SO3_left_inv(diff_o);
- J_do_dq2 = jac_SO3_right_inv(diff_o);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& err)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & err(6) );
-
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& dif,
- MatrixBase<D4>& J_diff_d1,
- MatrixBase<D5>& J_diff_d2)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
-
- Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
-
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
-
- /* d = diff(d1, d2) is
- * dp = dp2 - dp1
- * do = Log(dq1.conj * dq2)
- * dv = dv2 - dv1
- *
- * With trivial Jacobians for dp and dv, and:
- * J_do_dq1 = - J_l_inv(theta)
- * J_do_dq2 = J_r_inv(theta)
- */
-
- J_diff_d1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity
- J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
-
- J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
- J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 9, 1> ret;
- diff(d1, d2, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void body2delta(const MatrixBase<D1>& a,
- const MatrixBase<D2>& w,
- const typename D1::Scalar& dt,
- MatrixBase<D3>& dp,
- QuaternionBase<D4>& dq,
- MatrixBase<D5>& dv)
-{
- MatrixSizeCheck<3,1>::check(a);
- MatrixSizeCheck<3,1>::check(w);
- MatrixSizeCheck<3,1>::check(dp);
- MatrixSizeCheck<3,1>::check(dv);
-
- dp = 0.5 * a * dt * dt;
- dv = a * dt;
- dq = exp_q(w * dt);
-}
-
-template<typename D1>
-inline Matrix<typename D1::Scalar, 10, 1> body2delta(const MatrixBase<D1>& body,
- const typename D1::Scalar& dt)
-{
- MatrixSizeCheck<6,1>::check(body);
-
- typedef typename D1::Scalar T;
-
- Matrix<T, 10, 1> delta;
-
- Map< Matrix<T, 3, 1>> dp ( & delta(0) );
- Map< Quaternion<T>> dq ( & delta(3) );
- Map< Matrix<T, 3, 1>> dv ( & delta(7) );
-
- body2delta(body.block(0,0,3,1), body.block(3,0,3,1), dt, dp, dq, dv);
-
- return delta;
-}
-
-template<typename D1, typename D2, typename D3>
-inline void body2delta(const MatrixBase<D1>& body,
- const typename D1::Scalar& dt,
- MatrixBase<D2>& delta,
- MatrixBase<D3>& jac_body)
-{
- MatrixSizeCheck<6,1>::check(body);
- MatrixSizeCheck<9,6>::check(jac_body);
-
- typedef typename D1::Scalar T;
-
- delta = body2delta(body, dt);
-
- Matrix<T, 3, 1> w = body.block(3,0,3,1);
-
- jac_body.setZero();
- jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
- jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
- jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
-void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const QuaternionBase<D3>& q, const MatrixBase<D4>& a_b, const MatrixBase<D5>& w_b, MatrixBase<D6>& a_m, MatrixBase<D7>& w_m)
-{
- MatrixSizeCheck<3,1>::check(a);
- MatrixSizeCheck<3,1>::check(w);
- MatrixSizeCheck<3,1>::check(a_b);
- MatrixSizeCheck<3,1>::check(w_b);
- MatrixSizeCheck<3,1>::check(a_m);
- MatrixSizeCheck<3,1>::check(w_m);
-
- // Note: data = (a_m , w_m)
- a_m = a + a_b - q.conjugate()*gravity();
- w_m = w + w_b;
-}
-
-/* Create IMU data from body motion
- * Input:
- * motion : [ax, ay, az, wx, wy, wz] the motion in body frame
- * q : the current orientation wrt horizontal
- * bias : the bias of the IMU
- * Output:
- * return : the data vector as created by the IMU (with motion, gravity, and bias)
- */
-template<typename D1, typename D2, typename D3>
-Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias)
-{
- Matrix<typename D1::Scalar, 6, 1> data;
- Map<Matrix<typename D1::Scalar, 3, 1>> a_m (data.data() );
- Map<Matrix<typename D1::Scalar, 3, 1>> w_m (data.data() + 3);
-
- motion2data(motion.block(0,0,3,1),
- motion.block(3,0,3,1),
- q,
- bias.block(0,0,3,1),
- bias.block(3,0,3,1),
- a_m,
- w_m );
-
- return data;
-}
-
-} // namespace imu
-} // namespace wolf
-
-#endif /* IMU_TOOLS_H_ */
diff --git a/include/base/SE3.h b/include/base/SE3.h
deleted file mode 100644
index cea35460771f0bf91d4f56520dd823ad3a1d505d..0000000000000000000000000000000000000000
--- a/include/base/SE3.h
+++ /dev/null
@@ -1,479 +0,0 @@
-/*
- * SE3.h
- *
- * Created on: Mar 15, 2018
- * Author: jsola
- */
-
-#ifndef SE3_H_
-#define SE3_H_
-
-#include "base/wolf.h"
-#include "base/rotations.h"
-
-/*
- * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
- *
- * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form,
- * Delta = [Dp, Dq]
- * with
- * Dp : position delta
- * Dq : quaternion delta
- *
- * The functions are listed below:
- *
- * - compose: Dc = D1 (+) D2
- * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
- * - inverse: so that D (+) D.inv = D.inv (+) D = I
- * - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
- * - log_SE3: go from Delta manifold to tangent space (equivalent to log() in rotations)
- * - exp_SE3: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 * exp_SE3(d)
- * - minus: d = log_SE3( D1.inv() * D2 )
- * - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
- */
-
-namespace wolf
-{
-namespace three_D {
-using namespace Eigen;
-
-template<typename D1, typename D2>
-inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
-{
- MatrixSizeCheck<3, 1>::check(p);
- p = MatrixBase<D1>::Zero(3,1);
- q = QuaternionBase<D2>::Identity();
-}
-
-template<typename D1, typename D2>
-inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
-{
- MatrixSizeCheck<3, 1>::check(p);
- MatrixSizeCheck<4, 1>::check(q);
- typedef typename D1::Scalar T1;
- typedef typename D2::Scalar T2;
- p << T1(0), T1(0), T1(0);
- q << T2(0), T2(0), T2(0), T2(1);
-}
-
-template<typename T = Scalar>
-inline Matrix<T, 7, 1> identity()
-{
- Matrix<T, 7, 1> ret;
- ret<< T(0), T(0), T(0),
- T(0), T(0), T(0), T(1);
- return ret;
-}
-
-template<typename D1, typename D2, typename D4, typename D5>
-inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
- MatrixBase<D4>& idp, QuaternionBase<D5>& idq)
-{
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(idp);
-
- idp = - dq.conjugate() * dp ;
- idq = dq.conjugate() ;
-}
-
-template<typename D1, typename D2>
-inline void inverse(const MatrixBase<D1>& d,
- MatrixBase<D2>& id)
-{
- MatrixSizeCheck<7, 1>::check(d);
- MatrixSizeCheck<7, 1>::check(id);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
- Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
-
- inverse(dp, dq, idp, idq);
-}
-
-template<typename D>
-inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
-{
- Matrix<typename D::Scalar, 7, 1> id;
- inverse(d, id);
- return id;
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(sum_p);
-
- sum_p = dp1 + dq1*dp2;
- sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
-}
-
-template<typename D1, typename D2, typename D3>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& sum)
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 1>::check(sum);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
- Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
-
- compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2 )
-{
- Matrix<typename D1::Scalar, 7, 1> ret;
- compose(d1, d2, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& sum,
- MatrixBase<D4>& J_sum_d1,
- MatrixBase<D5>& J_sum_d2)
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 1>::check(sum);
- MatrixSizeCheck< 6, 6>::check(J_sum_d1);
- MatrixSizeCheck< 6, 6>::check(J_sum_d2);
-
- // Some useful temporaries
- Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR
- Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
-
- // Jac wrt first delta
- J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
- J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
- J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
-
- // Jac wrt second delta
- J_sum_d2.setIdentity(); //
- J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
- // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
-
- // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
- compose(d1, d2, sum);
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12)
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dp12);
-
- dp12 = dq1.conjugate() * ( dp2 - dp1 );
- dq12 = dq1.conjugate() * dq2;
-}
-
-template<typename D1, typename D2, typename D3>
-inline void between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d2_minus_d1)
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 1>::check(d2_minus_d1);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) );
- Map<Quaternion<typename D3::Scalar> > dq12 ( & d2_minus_d1(3) );
-
- between(dp1, dq1, dp2, dq2, dp12, dq12);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2 )
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- Matrix<typename D1::Scalar, 7, 1> d12;
- between(d1, d2, d12);
- return d12;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
-{
- MatrixSizeCheck<7, 1>::check(delta_in);
-
- Matrix<typename Derived::Scalar, 6, 1> ret;
-
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
- Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
-
- Matrix<typename Derived::Scalar, 3, 3> V_inv;
-
- do_ret = log_q(dq_in);
- V_inv = jac_SO3_left_inv(do_ret);
- dp_ret = V_inv * dp_in;
-
- return ret;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
-{
- MatrixSizeCheck<6, 1>::check(d_in);
-
- Matrix<typename Derived::Scalar, 7, 1> ret;
-
- Matrix<typename Derived::Scalar, 3, 3> V;
-
- V = jac_SO3_left(d_in.template tail<3>());
-
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
- Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
-
- dp = V * dp_in;
- dq = exp_q(do_in);
-
- return ret;
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
- MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(plus_p);
- plus_p = dp1 + dp2;
- plus_q = dq1 * exp_q(do2);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d_plus)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) );
- Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(3) );
-
- plus(dp1, dq1, dp2, do2, dp_p, dq_p);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 7, 1> d_plus;
- plus(d1, d2, d_plus);
- return d_plus;
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
-{
- diff_p = dp2 - dp1;
- diff_o = log_q(dq1.conjugate() * dq2);
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
- MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
-{
- minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
-
- J_do_dq1 = - jac_SO3_left_inv(diff_o);
- J_do_dq2 = jac_SO3_right_inv(diff_o);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void minus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& err)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
-
- minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void minus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& dif,
- MatrixBase<D4>& J_diff_d1,
- MatrixBase<D5>& J_diff_d2)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
-
- Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
-
- minus(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
-
- /* d = minus(d1, d2) is
- * dp = dp2 - dp1
- * do = Log(dq1.conj * dq2)
- * dv = dv2 - dv1
- *
- * With trivial Jacobians for dp and dv, and:
- * J_do_dq1 = - J_l_inv(theta)
- * J_do_dq2 = J_r_inv(theta)
- */
-
- J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity
- J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
-
- J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity
- J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 6, 1> ret;
- minus(d1, d2, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3>
-inline void interpolate(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- const typename D1::Scalar& t,
- MatrixBase<D3>& ret)
-{
- Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
-
- Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
-
- ret = compose(d1, exp_SE3(tau));
-}
-
-template<typename D1, typename D2>
-inline void toSE3(const MatrixBase<D1>& pose,
- MatrixBase<D2>& SE3)
-{
- MatrixSizeCheck<4,4>::check(SE3);
-
- typedef typename D1::Scalar T;
-
- SE3.template block<3,1>(0,3) = pose.template head<3>();
- SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>());
- SE3.template block<1,3>(3,0).setZero();
- SE3(3,3) = (T)1.0;
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w)
-{
- typedef typename D1::Scalar T;
-
- Matrix<T, 3, 3> V = skew(v);
- Matrix<T, 3, 3> W = skew(w);
- Matrix<T, 3, 3> VW = V * W;
- Matrix<T, 3, 3> WV = VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
- Matrix<T, 3, 3> WVW = WV * W;
- Matrix<T, 3, 3> VWW = VW * W;
-
- T th_2 = w.squaredNorm();
-
- T A(T(0.5)), B, C, D;
-
- // Small angle approximation
- if (th_2 <= T(1e-8))
- {
- B = Scalar(1./6.) + Scalar(1./120.) * th_2;
- C = -Scalar(1./24.) + Scalar(1./720.) * th_2;
- D = -Scalar(1./60.);
- }
- else
- {
- T th = sqrt(th_2);
- T th_3 = th_2*th;
- T th_4 = th_2*th_2;
- T th_5 = th_3*th_2;
- T sin_th = sin(th);
- T cos_th = cos(th);
- B = (th-sin_th)/th_3;
- C = (T(1.0) - th_2/T(2.0) - cos_th)/th_4;
- D = (th - sin_th - th_3/T(6.0))/th_5;
- }
- Matrix<T, 3, 3> Q;
- Q.noalias() =
- + A * V
- + B * (WV + VW + WVW)
- - C * (VWW - VWW.transpose() - Scalar(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
- - D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
-
- return Q;
-}
-
-template<typename D1>
-inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent)
-{
- typedef typename D1::Scalar T;
- Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
- Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
-
- Matrix<T, 3, 3> Jl(jac_SO3_left(w));
- Matrix<T, 3, 3> Q = Q_helper(v,w);
-
- Matrix<T, 6, 6> Jl_SE3;
- Jl_SE3.topLeftCorner(3,3) = Jl;
- Jl_SE3.bottomRightCorner(3,3) = Jl;
- Jl_SE3.topRightCorner(3,3) = Q;
- Jl_SE3.bottomLeftCorner(3,3) .setZero();
-}
-
-template<typename D1>
-inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent)
-{
- return jac_SE3_left(-tangent);
-}
-
-} // namespace three_d
-} // namespace wolf
-
-
-#endif /* SE3_H_ */
diff --git a/include/base/association/association_nnls.h b/include/base/association/association_nnls.h
deleted file mode 100644
index 6563a1f66a513f6359200813e095e446e91327fc..0000000000000000000000000000000000000000
--- a/include/base/association/association_nnls.h
+++ /dev/null
@@ -1,83 +0,0 @@
-
-#ifndef association_nnls_H
-#define association_nnls_H
-
-//std
-#include <iostream>
-#include <vector>
-
-//pipol tracker
-#include "base/association/association_solver.h"
-
-namespace wolf
-{
-
-//consts
-const double MAX_DIST_DEFAULT = 0.5; //units (meters in pt case)
-
-/** \brief Nearest neighbour linear search
- *
- * Nearest neighbour linear search to solve data association problems, given a table of distances
- *
-*/
-class AssociationNNLS : public AssociationSolver
-{
- protected:
- double max_dist_; //maximum distance to allow association
- std::vector<bool> i_mask_; // mask already allocated detections (rows)
- std::vector<bool> j_mask_; // mask already allocated targets (columns)
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- */
- AssociationNNLS();
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationNNLS();
-
- /** \brief Sets max_dist_
- *
- * Sets max_dist_
- *
- **/
- void setMaxDist(const double _max_dist);
-
- /** \brief Resets problem
- *
- * Resets problem
- *
- */
- void reset();
-
- /** \brief Resizes the problem
- *
- * Resizes the problem
- *
- */
- void resize(const unsigned int _n_det, const unsigned int _n_tar);
-
- /** \brief Solves the problem
- *
- * Solves the association problem following nearest neighbor linear search.
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- * Assumes i/j_mask_ vector class members and scores_ matrix are correctly sized, by a previous call to resize()
- *
- **/
- //void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<unsigned int> & _unassoc);
- void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask);
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/association/association_node.h b/include/base/association/association_node.h
deleted file mode 100644
index c652ba6c48878fb43801cfc546733f7abc0790bb..0000000000000000000000000000000000000000
--- a/include/base/association/association_node.h
+++ /dev/null
@@ -1,177 +0,0 @@
-
-#ifndef association_node_H
-#define association_node_H
-
-//std
-#include <iostream>
-#include <vector>
-#include <list>
-#include <algorithm> //find()
-
-//pipol tracker
-#include "base/association/matrix.h"
-
-//constants
-const double PROB_ZERO_ = 1e-3;
-
-/** \brief A node in the association decision tree
- *
- * A node in the association decision tree. A node associates a pair between a detection index and a target index, which is
- * usually diferent from detection Id and target Id. Therefore, Id to index mapping has to be implemented outside of this class.
- *
-*/
-class AssociationNode
-{
- protected:
- bool is_root_;///<true if the node is root
- unsigned int det_idx_; ///< detection node index
- unsigned int tar_idx_; ///< target node index
- double node_prob_; ///< Node Probability. Normalized->Conditional Probability that detection associates to target. Non-normalizeNodeProbs->Product of likelihoods.
- double tree_prob_; ///< Tree Probability. Joint Probability from the root node up to this (product of node probabilities)
- AssociationNode * up_node_ptr_; ///< Pointer to up node
- std::list<AssociationNode> node_list_; ///< List of nodes below of this in the association tree
-
-
- public:
- /** \brief Constructor
- *
- * Constructor with arguments _det_idx and _tar_idx which indicates association of detection and target,
- * with the probability _prob;
- *
- */
- AssociationNode(const unsigned int _det_idx, const unsigned int _tar_idx, const double _prob, AssociationNode * _un_ptr, bool _is_root = false);
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationNode();
-
- /** \brief True if this is the root node
- *
- * True if this is the root node, which is equivalent to check if up_node_ptr_ == NULL
- *
- **/
- bool isRoot() const;
-
- /** \brief True if this is node is terminus (no more nodes below)
- *
- * True if this is node is terminus (no more nodes below), which is equivalent to check node_list_.empty()
- *
- **/
- bool isTerminus() const;
-
- /** \brief Returns det_idx_
- *
- * Returns det_idx_
- *
- **/
- unsigned int getDetectionIndex() const;
-
- /** \brief Returns tar_idx_
- *
- * Returns tar_idx_
- *
- **/
- unsigned int getTargetIndex() const;
-
- /** \brief Returns node_prob_
- *
- * Returns node_prob_
- *
- **/
- double getNodeProb() const;
-
- /** \brief Sets node_prob_
- *
- * Sets node_prob_
- *
- **/
- void setNodeProb(double _np);
-
- /** \brief Returns tree_prob_
- *
- * Returns tree_prob_
- *
- **/
- double getTreeProb() const;
-
- /** \brief Returns a copy of up_node_ptr_
- *
- * Returns a copy of up_node_ptr_
- *
- **/
- AssociationNode * upNode() const;
-
- /** \brief Computes node probability
- *
- * Computes probability that detection_i associates to target_j, given the scores table _stab.
- * \param _nd TODO document this
- * \param _nt TODO document this
- * \param _di detection index (not id)
- * \param _tj target index (not id)
- * \param _stab score table
- * Returns the probability.
- * Nodes require computing other ij probs to decide if they continue growing or not at function growTree().
- *
- **/
- //double computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const std::vector< std::vector<double> > & _stab) const;
- double computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const Matrixx<double> & _stab) const;
-
- /** \brief Normalizes node probabilities recursively
- *
- * Normalizes node probabilities recursively.
- * All node probs of node_list_ should sum 1
- *
- **/
- void normalizeNodeProbs();
-
- /** \brief Computes tree probabilities recursively
- *
- * Computes tree probabilities recursively, while setting tree_prob_ data member
- * Updates the terminus node list, passed as second parameter, with all nodes that are terminus
- * \param _up_prob probability of upper node
- * \param _tn_list: List of terminus nodes. Filled with terminus nodes, while recurisve computing tree
- *
- **/
- void computeTreeProb(const double & _up_prob, std::list<AssociationNode*> & _tn_list);
-
- /** \brief Grows tree recursively
- *
- * Grows tree recursively according the association probability table provided
- * \param _nd
- * \param _nt
- * \param _det_i: detection index
- * \param _stab: table of association probabilities between detections and targets
- * \param _excluded: vector of target index for which the tree should not continue growing
- *
- **/
- //void growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const std::vector< std::vector<double> > & _stab, std::vector<unsigned int> & _excluded);
- void growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const Matrixx<double> & _stab, std::vector<unsigned int> & _excluded);
-
- /** \brief Destroys tree
- *
- * Recursively destroys tree
- *
- **/
- void destroyTree();
-
- /** \brief Prints node info
- *
- * Prints node info
- *
- **/
- void printNode() const;
-
- /** \brief Prints the tree, by printing node info recursively
- *
- * Prints the tree, by printing node info recursively
- * \param _ntabs Number of tabulators before printing. Useful for recursively print a whole tree
- *
- * TODO: This function should be const, but we run recursively with an iterator over the node_list_ and compiler
- * claims saying it can't return a const iterator.
- **/
- void printTree(const unsigned int _ntabs = 0);
-};
-#endif
diff --git a/include/base/association/association_solver.h b/include/base/association/association_solver.h
deleted file mode 100644
index 6ac986e073d7419514e174301426ad83b9e7b4d5..0000000000000000000000000000000000000000
--- a/include/base/association/association_solver.h
+++ /dev/null
@@ -1,108 +0,0 @@
-
-#ifndef association_solver_H
-#define association_solver_H
-
-//std
-#include <iostream>
-#include <vector>
-
-//matrix class
-#include "base/association/matrix.h"
-
-namespace wolf
-{
-
-/** \brief A pure virtual solver for the association problem
- *
- * A pure virtual solver for the association problem
- *
-*/
-class AssociationSolver
-{
- protected:
- unsigned int nd_; //num detections
- unsigned int nt_; //num targets, without counting the void target
-// std::vector< std::vector<double> > scores_;//scores table. Size is (nd_) x (nt_+1), to account for the void target
- Matrixx<double> scores_;//scores table. Size is (nd_) x (nt_+1), to account for the void target
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- */
- AssociationSolver();
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationSolver();
-
- /** \brief Returns num of detections nd_
- *
- * Returns num of detections nd_
- *
- **/
- unsigned int numDetections();
-
- /** \brief Returns num of actual targets nt_
- *
- * Returns num of actual targets nt_
- *
- **/
- unsigned int numTargets();
-
- /** \brief Sets values to scores_ table
- *
- * Sets value to score table, at cell ij, corresponding to detection_i and target_j
- *
- **/
- void setScore(const unsigned int _det_i, const unsigned int _tar_j, const double _m_ij);
-
- /** \brief Gets a score
- *
- * Gets score of cell ij, corresponding to detection_i and target_j
- *
- **/
- double getScore(const unsigned int _det_i, const unsigned int _tar_j);
-
- /** \brief Prints the score table
- *
- * Prints the score table
- *
- */
- void printScoreTable() const;
-
- /** \brief Resets the problem
- *
- * Deletes and clears the problem
- *
- */
- virtual void reset() = 0;
-
- /** \brief Resizes the problem
- *
- * Resizes the problem:
- * \param _n_det num of detections
- * \param _n_tar num of targets
- *
- */
- virtual void resize(const unsigned int _n_det, const unsigned int _n_tar) = 0;
-
- /** \brief Solves and sets decision pairs
- *
- * Solves and sets decision pairs
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- **/
- //virtual void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<unsigned int> & _unassoc) = 0;
- virtual void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask) = 0;
-
-};
-
-} //namespace wolf
-#endif
diff --git a/include/base/association/association_tree.h b/include/base/association/association_tree.h
deleted file mode 100644
index b6566a60459986d19434492b1f5603b78fd40fe9..0000000000000000000000000000000000000000
--- a/include/base/association/association_tree.h
+++ /dev/null
@@ -1,127 +0,0 @@
-
-#ifndef association_tree_H
-#define association_tree_H
-
-//std
-// #include <list>
-// #include <vector>
-//#include <pair>
-//#include <memory>
-
-//pipol tracker
-#include "base/association/matrix.h"
-#include "base/association/association_solver.h"
-#include "base/association/association_node.h"
-#include <map>
-
-namespace wolf
-{
-
-/** \brief The whole decision tree
- */
-class AssociationTree : public AssociationSolver
-{
- protected:
- AssociationNode root_;
- std::list<AssociationNode*> terminus_node_list_;
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- */
- AssociationTree();
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationTree();
-
- /** \brief Reset
- *
- * Deletes all nodes and clears scores and association list
- *
- */
- void reset();
-
- /** \brief Resizes the problem
- *
- * Resizes the problem:
- * \param _n_det num of detections
- * \param _n_tar num of targets
- * Resizes the scores_ matrix which will allocate _n_det rows and _n_tar+1 columns to take into account void target
- *
- */
- void resize(const unsigned int _n_det, const unsigned int _n_tar);
-
- /** \brief Build tree from scores
- *
- * Build tree from scores
- *
- */
- void growTree();
-
- /** \brief Computes tree probabilities
- *
- * Computes tree probabilities
- *
- */
- void computeTree();
-
- /** \brief Normalizes node probabilities
- *
- * Normalizes node probabilities
- *
- */
- void normalizeTree();
-
- /** \brief choose best terminus node
- *
- * Choose best terminus node based on the best tree probability
- * \param _best_node a reference to an iterator to a list of pointers, where returned result is placed.
- * At output, _best_node points the bets node in the terminus_node_list_
- *
- **/
- void chooseBestTerminus(std::list<AssociationNode*>::iterator & _best_node);
-
- /** \brief Gets tree decision
- *
- * Decides best hypothesis according tree computation made by computeTree()
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- **/
- void solve(std::map<unsigned int, unsigned int> & _pairs, std::vector<bool> & _associated_mask);
-
- /** \brief Gets tree decision
- *
- * Decides best hypothesis according tree computation made by computeTree()
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- **/
- void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask);
-
- /** \brief Prints the tree
- *
- * Prints the tree
- *
- * TODO: this function should be const. See comments on printTree() at association_node.h
- */
- void printTree();
-
- /** \brief Prints terminus_node_list_
- *
- * Prints terminus_node_list_
- *
- */
- void printTerminusNodes();
-};
-
-} // namespace wolf
-#endif
diff --git a/include/base/association/matrix.h b/include/base/association/matrix.h
deleted file mode 100644
index c76bba51e4de2db56a380e48e7002a16bba78bcc..0000000000000000000000000000000000000000
--- a/include/base/association/matrix.h
+++ /dev/null
@@ -1,82 +0,0 @@
-
-#ifndef matrix_H
-#define matrix_H
-
-//std
-#include <iostream>
-#include <vector>
-#include <assert.h> //assert
-
-template <typename T>
-class Matrixx
-{
- protected:
- unsigned int rows_, cols_;
- std::vector<T> inner_;
-
- public:
- Matrixx() :
- rows_(0),
- cols_(0)
- {
- //
- }
-
- Matrixx(unsigned int _rows, unsigned int _cols) :
- rows_ (_rows),
- cols_ (_cols),
- inner_(rows_*cols_)
- {
- //
- }
-
- ~Matrixx()
- {
- //
- }
-
- void clear()
- {
- inner_.clear();
- }
-
- void resize(unsigned int _rows, unsigned int _cols)
- {
- rows_ = _rows;
- cols_ = _cols;
- inner_.resize (rows_*cols_);
- //std::cout << "Resizing matrix to " << rows_ << " x " << cols_ << std::endl;
- }
-
- unsigned int size() const
- {
- return rows_*cols_;
- }
-
- T& operator()(unsigned int _i, unsigned int _j)
- {
- assert( (_i < rows_) && (_j < cols_) && "Matrix::operator(): Wrong matrix indexes. Program abort.");
- return inner_[cols_*_i + _j];
- }
-
- const T& operator()(unsigned int _i, unsigned int _j) const
- {
- assert( (_i < rows_) && (_j < cols_) && "Matrix::operator(): Wrong matrix indexes. Program abort.");
- return inner_[cols_*_i + _j];
- }
-
- void print() const
- {
- for(unsigned int ii=0; ii<rows_; ii++)
- {
- for(unsigned int jj=0; jj<cols_; jj++)
- {
- std::cout << inner_[cols_*ii + jj] << " ";
- }
- std::cout << std::endl;
- }
- std::cout << std::endl;
- }
-
-};
-#endif
diff --git a/include/base/capture/capture_GPS.h b/include/base/capture/capture_GPS.h
deleted file mode 100644
index 5f5088bd6dbf5e9674bcd12c3c13caa6a213c8e5..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_GPS.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef CAPTURE_GPS_H_
-#define CAPTURE_GPS_H_
-
-// Wolf includes
-#include "raw_gps_utils/satellites_obs.h"
-#include "base/capture/capture_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureGPS);
-
-class CaptureGPS : public CaptureBase
-{
- protected:
- rawgpsutils::SatellitesObs obs_;
-
- public:
- CaptureGPS(const TimeStamp &_ts, SensorBasePtr _sensor_ptr, rawgpsutils::SatellitesObs &_obs);
- virtual ~CaptureGPS();
-
- rawgpsutils::SatellitesObs &getData();
-};
-
-} // namespace wolf
-
-#endif //CAPTURE_GPS_H_
diff --git a/include/base/capture/capture_GPS_fix.h b/include/base/capture/capture_GPS_fix.h
deleted file mode 100644
index 8cb4afbf4cc340a09693ecf2e426cf747df5b058..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_GPS_fix.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef CAPTURE_GPS_FIX_H_
-#define CAPTURE_GPS_FIX_H_
-
-//Wolf includes
-#include "base/feature/feature_GPS_fix.h"
-#include "base/capture/capture_base.h"
-
-//std includes
-//
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureGPSFix);
-
-//class CaptureGPSFix
-class CaptureGPSFix : public CaptureBase
-{
- protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
-
- public:
- CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data);
- CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
- virtual ~CaptureGPSFix();
- virtual void process();
-};
-
-} //namespace wolf
-#endif
diff --git a/include/base/capture/capture_IMU.h b/include/base/capture/capture_IMU.h
deleted file mode 100644
index 6e2de9d9d91eee27afb6b46940f2c5558ee0fddb..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_IMU.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef CAPTURE_IMU_H
-#define CAPTURE_IMU_H
-
-//Wolf includes
-#include "base/IMU_tools.h"
-#include "base/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureIMU);
-
-class CaptureIMU : public CaptureMotion
-{
- public:
-
- CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
- const Vector6s& _bias,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- virtual ~CaptureIMU();
-
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
-
-};
-
-inline Eigen::VectorXs CaptureIMU::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
-{
- return imu::plus(_delta, _delta_error);
-}
-
-} // namespace wolf
-
-#endif // CAPTURE_IMU_H
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
deleted file mode 100644
index 0082d9d314fa28cf9f26a64b0cb455ab07102244..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_base.h
+++ /dev/null
@@ -1,231 +0,0 @@
-#ifndef CAPTURE_BASE_H_
-#define CAPTURE_BASE_H_
-
-// Forward declarations for node templates
-namespace wolf{
-class FrameBase;
-class FeatureBase;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
-
-//std includes
-
-namespace wolf{
-
-//class CaptureBase
-class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
-{
- private:
- FrameBaseWPtr frame_ptr_;
- FeatureBasePtrList feature_list_;
- FactorBasePtrList constrained_by_list_;
- SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
- // Deal with sensors with dynamic extrinsics (check dynamic_extrinsic_ in SensorBase)
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
- SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
-
- static unsigned int capture_id_count_;
-
- protected:
- unsigned int capture_id_;
- TimeStamp time_stamp_; ///< Time stamp
-
- public:
-
- CaptureBase(const std::string& _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr = nullptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- virtual ~CaptureBase();
- virtual void remove();
-
- // Type
- virtual bool isMotion() const { return false; }
-
- bool process();
-
- unsigned int id();
- TimeStamp getTimeStamp() const;
- void setTimeStamp(const TimeStamp& _ts);
- void setTimeStampToNow();
-
- FrameBasePtr getFrame() const;
- void setFrame(const FrameBasePtr _frm_ptr);
- void unlinkFromFrame(){frame_ptr_.reset();}
-
- virtual void setProblem(ProblemPtr _problem) final;
-
- FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
- FeatureBasePtrList& getFeatureList();
- void addFeatureList(FeatureBasePtrList& _new_ft_list);
-
- void getFactorList(FactorBasePtrList& _fac_list);
-
- SensorBasePtr getSensor() const;
- virtual void setSensor(const SensorBasePtr sensor_ptr);
-
- // constrained by
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
-
- // State blocks
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
- StateBlockPtr getStateBlock(unsigned int _i) const;
- void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
-
- StateBlockPtr getSensorP() const;
- StateBlockPtr getSensorO() const;
- StateBlockPtr getSensorIntrinsic() const;
- void removeStateBlocks();
- virtual void registerNewStateBlocks();
-
- void fix();
- void unfix();
- void fixExtrinsics();
- void unfixExtrinsics();
- void fixIntrinsics();
- void unfixIntrinsics();
-
- bool hasCalibration() {return calib_size_ > 0;}
- SizeEigen getCalibSize() const;
- virtual Eigen::VectorXs getCalibration() const;
- void setCalibration(const Eigen::VectorXs& _calib);
-
- protected:
- SizeEigen computeCalibSize() const;
-
- private:
- void updateCalibSize();
-};
-
-}
-
-#include "base/sensor/sensor_base.h"
-#include "base/frame_base.h"
-#include "base/feature/feature_base.h"
-#include "base/state_block.h"
-
-namespace wolf{
-
-inline SizeEigen CaptureBase::getCalibSize() const
-{
- return calib_size_;
-}
-
-inline void CaptureBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto ft : feature_list_)
- ft->setProblem(_problem);
-}
-
-inline void CaptureBase::updateCalibSize()
-{
- calib_size_ = computeCalibSize();
-}
-
-inline const std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
-{
- state_block_vec_[_i] = _sb_ptr;
-}
-
-inline StateBlockPtr CaptureBase::getSensorP() const
-{
- return getStateBlock(0);
-}
-
-inline StateBlockPtr CaptureBase::getSensorO() const
-{
- return getStateBlock(1);
-}
-
-inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
-{
- return getStateBlock(2);
-}
-
-inline unsigned int CaptureBase::id()
-{
- return capture_id_;
-}
-
-inline FrameBasePtr CaptureBase::getFrame() const
-{
- return frame_ptr_.lock();
-}
-
-inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
-{
- frame_ptr_ = _frm_ptr;
-}
-
-inline FeatureBasePtrList& CaptureBase::getFeatureList()
-{
- return feature_list_;
-}
-
-inline unsigned int CaptureBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
-inline FactorBasePtrList& CaptureBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
-
-inline TimeStamp CaptureBase::getTimeStamp() const
-{
- return time_stamp_;
-}
-
-inline SensorBasePtr CaptureBase::getSensor() const
-{
- return sensor_ptr_.lock();
-}
-
-inline void CaptureBase::setSensor(const SensorBasePtr sensor_ptr)
-{
- sensor_ptr_ = sensor_ptr;
-}
-
-inline void CaptureBase::setTimeStamp(const TimeStamp& _ts)
-{
- time_stamp_ = _ts;
-}
-
-inline void CaptureBase::setTimeStampToNow()
-{
- time_stamp_.setToNow();
-}
-
-inline bool CaptureBase::process()
-{
- assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
-
- return getSensor()->process(shared_from_this());
-}
-
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/capture/capture_buffer.h b/include/base/capture/capture_buffer.h
deleted file mode 100644
index 434716dcfdf596b09d037a62f136428bc9d3e893..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_buffer.h
+++ /dev/null
@@ -1,172 +0,0 @@
-/**
- * \file capture_buffer.h
- *
- * Created on: Jul 12, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_CAPTURE_BUFFER_H_
-#define _WOLF_CAPTURE_BUFFER_H_
-
-#include "base/wolf.h"
-#include "base/time_stamp.h"
-
-#include <list>
-#include <algorithm>
-
-namespace wolf {
-
-//using namespace Eigen;
-
-enum class CaptureBufferPolicy : std::size_t
-{
- TIME = 0,
- NUM_CAPTURES,
- ALL
-};
-
-/** \brief class for capture buffers.
- *
- * It consists of a buffer of pre-integrated motions (aka. delta-integrals) that is being filled
- * by the motion processors (deriving from ProcessorMotion).
- * Each delta-integral is accompanied by a time stamp, a Jacobian and a covariances matrix.
- *
- * This buffer contains the time stamp and delta-integrals:
- * - since the last key-Frame
- * - until the frame of this capture.
- *
- * The buffer can be queried for motion-integrals and delta-integrals corresponding to a provided time stamp,
- * with the following rules:
- * - If the query time stamp is later than the last one in the buffer,
- * the last motion-integral or delta-integral is returned.
- * - If the query time stamp is earlier than the beginning of the buffer,
- * the earliest Motion or Delta is returned.
- * - If the query time stamp matches one time stamp in the buffer exactly,
- * the returned motion-integral or delta-integral is the one of the queried time stamp.
- * - If the query time stamp does not match any time stamp in the buffer,
- * the returned motion-integral or delta-integral is the one immediately before the query time stamp.
- */
-
-//template <CaptureBufferPolicy policy>
-class CaptureBuffer
-{
-public:
-
- CaptureBuffer(const Scalar _buffer_dt, const int _max_capture = -1);
- ~CaptureBuffer() = default;
-
- void push_back(const CaptureBasePtr& capture);
-
-// std::list<CaptureBasePtr>& get();
-// const std::list<CaptureBasePtr>& get() const;
-
- const CaptureBasePtr& getCapture(const TimeStamp& _ts) const;
- void getCapture(const TimeStamp& _ts, CaptureBasePtr& _motion) const;
-
- void remove(const CaptureBasePtr& capture);
-
- void clear();
-// void print();
-
- const TimeStamp earliest() const;
- const TimeStamp latest() const;
-
-//private:
-
- int max_capture_;
- Scalar buffer_dt_;
-
- std::list<CaptureBasePtr> container_;
-};
-
-CaptureBuffer::CaptureBuffer(const Scalar _buffer_dt, const int _max_capture) :
- max_capture_(_max_capture), buffer_dt_(_buffer_dt)
-{
- //
-}
-
-void CaptureBuffer::push_back(const CaptureBasePtr& capture)
-{
- container_.push_back(capture);
-
- WOLF_DEBUG("Buffer dt : ", container_.back()->getTimeStamp() -
- container_.front()->getTimeStamp(), " vs ", buffer_dt_);
-
- while (container_.back()->getTimeStamp() -
- container_.front()->getTimeStamp() > buffer_dt_)
- {
- container_.pop_front();
- }
-}
-
-const CaptureBasePtr& CaptureBuffer::getCapture(const TimeStamp& _ts) const
-{
- //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(container_.rbegin(), container_.rend(),
- [&](const CaptureBasePtr& c)
- {
- return c->getTimeStamp() <= _ts;
- });
-
- if (previous == container_.rend())
- // The time stamp is older than the buffer's oldest data.
- // We could do something here, and throw an error or something, but by now we'll return the first valid data
- previous--;
-
- return *previous;
-}
-
-void CaptureBuffer::getCapture(const TimeStamp& _ts, CaptureBasePtr& _capture) const
-{
-// //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
-// auto previous = std::find_if(container_.rbegin(), container_.rend(),
-// [&](const Motion& m)
-// {
-// return c->getTimeStamp() <= _ts;
-// });
-
-// if (previous == container_.rend())
-// // The time stamp is older than the buffer's oldest data.
-// // We could do something here, but by now we'll return the last valid data
-// previous--;
-
-// _capture = *previous;
-
- _capture = getCapture(_ts);
-}
-
-//inline std::list<CaptureBasePtr>& CaptureBuffer::get()
-//{
-// return container_;
-//}
-
-//inline const std::list<CaptureBasePtr>& CaptureBuffer::get() const
-//{
-// return container_;
-//}
-
-inline void CaptureBuffer::remove(const CaptureBasePtr& capture)
-{
- container_.remove(capture);
-}
-
-inline void CaptureBuffer::clear()
-{
- return container_.clear();
-}
-
-inline const TimeStamp CaptureBuffer::earliest() const
-{
- return (!container_.empty())? container_.front()->getTimeStamp() :
- InvalidStamp;
-}
-
-inline const TimeStamp CaptureBuffer::latest() const
-{
- return (!container_.empty())? container_.back()->getTimeStamp() :
- InvalidStamp;
-}
-
-} // namespace wolf
-
-#endif /* _WOLF_CAPTURE_BUFFER_H_ */
diff --git a/include/base/capture/capture_image.h b/include/base/capture/capture_image.h
deleted file mode 100644
index 4e9771cf08808342e9339dfe9c3f108e49aae542..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_image.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef CAPTURE_IMAGE_H
-#define CAPTURE_IMAGE_H
-
-//Wolf includes
-#include "base/capture/capture_base.h"
-#include "base/feature/feature_point_image.h"
-#include "base/sensor/sensor_camera.h"
-
-// Vision Utils includes
-#include "vision_utils/vision_utils.h"
-#include "vision_utils/common_class/frame.h"
-
-namespace wolf {
-
-// Set ClassPtr, ClassConstPtr and ClassWPtr typedefs;
-WOLF_PTR_TYPEDEFS(CaptureImage);
-
-/**
- * \brief class CaptureImage
- *
- * This class stores a cv::Mat image, with keypoints and descriptors defined in the OpenCV format.
- * This encapsulation allows this Capture to be used in OpenCV with ease.
- */
-class CaptureImage : public CaptureBase
-{
- protected:
- vision_utils::Frame frame_;
-
- public:
- vision_utils::FeatureIdxGridPtr grid_features_;
- KeyPointVector keypoints_;
- cv::Mat descriptors_;
- DMatchVector matches_from_precedent_;
- std::vector<Scalar> matches_normalized_scores_;
- std::map<int, int> map_index_to_next_;
-
- public:
- CaptureImage(const TimeStamp& _ts, SensorCameraPtr _camera_ptr, cv::Mat _data_cv);
- virtual ~CaptureImage();
-
- const cv::Mat& getImage() const;
- void setDescriptors(const cv::Mat &_descriptors);
- void setKeypoints(const std::vector<cv::KeyPoint>& _keypoints);
- cv::Mat& getDescriptors();
- std::vector<cv::KeyPoint>& getKeypoints();
-};
-
-} // namespace wolf
-
-#endif // CAPTURE_IMAGE_H
diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h
deleted file mode 100644
index 96a434547b5e802376472f472be0aeb0e27af940..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_laser_2D.h
+++ /dev/null
@@ -1,52 +0,0 @@
-
-#ifndef CAPTURE_LASER_2D_H_
-#define CAPTURE_LASER_2D_H_
-
-//wolf forward declarations
-namespace wolf{
-class SensorLaser2D;
-}
-
-//wolf includes
-#include "base/capture/capture_base.h"
-#include "base/sensor/sensor_laser_2D.h"
-
-//laserscanutils includes
-#include "laser_scan_utils/laser_scan.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureLaser2D);
-
-class CaptureLaser2D : public CaptureBase
-{
- public:
- /** \brief Constructor with ranges
- **/
- CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges);
- virtual ~CaptureLaser2D() = default;
-
- laserscanutils::LaserScan& getScan();
-
- void setSensor(const SensorBasePtr sensor_ptr);
-
- private:
- SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
- laserscanutils::LaserScan scan_;
-
-};
-
-inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
-{
- return scan_;
-}
-
-inline void CaptureLaser2D::setSensor(const SensorBasePtr sensor_ptr)
-{
- CaptureBase::setSensor(sensor_ptr);
- laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr);
-}
-
-} // namespace wolf
-
-#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
deleted file mode 100644
index ed71171e99ef6d221777f1ab89a7e0ba4a8dc0d7..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_motion.h
+++ /dev/null
@@ -1,201 +0,0 @@
-/**
- * \file capture_motion2.h
- *
- * Created on: Mar 16, 2016
- * \author: jsola
- */
-
-#ifndef SRC_CAPTURE_MOTION_H_
-#define SRC_CAPTURE_MOTION_H_
-
-// Wolf includes
-#include "base/capture/capture_base.h"
-#include "base/motion_buffer.h"
-
-// STL includes
-#include <list>
-#include <algorithm>
-#include <iterator>
-#include <utility>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureMotion);
-
-
-/** \brief Base class for motion Captures.
- *
- * This class implements Captures for sensors integrating motion.
- *
- * The raw data of this type of captures is required to be in the form of a vector --> see attribute data_.
- *
- * It contains a MotionBuffer buffer of Motion pre-integrated motions that is being filled
- * by the motion processors (deriving from ProcessorMotion) --> See Motion, MotionBuffer, and ProcessorMotion.
- *
- * This buffer contains the integrated motion:
- * - since the last key-Frame
- * - until the frame of this capture.
- *
- * Once a keyframe is generated, this buffer is frozen and kept in the Capture for eventual later uses.
- * It is then used to compute the factor that links the Frame of this capture to the previous key-frame in the Trajectory.
- */
-
-class CaptureMotion : public CaptureBase
-{
-
- // public interface:
-
- public:
- CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr);
-
- CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- virtual ~CaptureMotion();
-
- // Type
- virtual bool isMotion() const override { return true; }
-
- // Data
- const Eigen::VectorXs& getData() const;
- const Eigen::MatrixXs& getDataCovariance() const;
- void setData(const Eigen::VectorXs& _data);
- void setDataCovariance(const Eigen::MatrixXs& _data_cov);
-
- // Buffer
- MotionBuffer& getBuffer();
- const MotionBuffer& getBuffer() const;
-
- // Buffer's initial conditions for pre-integration
- VectorXs getCalibrationPreint() const;
- void setCalibrationPreint(const VectorXs& _calib_preint);
- MatrixXs getJacobianCalib();
- MatrixXs getJacobianCalib(const TimeStamp& _ts);
-
- // Get delta preintegrated, and corrected for changes on calibration params
- VectorXs getDeltaCorrected(const VectorXs& _calib_current);
- VectorXs getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts);
- VectorXs getDeltaPreint();
- VectorXs getDeltaPreint(const TimeStamp& _ts);
- MatrixXs getDeltaPreintCov();
- MatrixXs getDeltaPreintCov(const TimeStamp& _ts);
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error);
-
- // Origin frame
- FrameBasePtr getOriginFrame();
- void setOriginFrame(FrameBasePtr _frame_ptr);
-
- // member data:
- private:
- Eigen::VectorXs data_; ///< Motion data in form of vector mandatory
- Eigen::MatrixXs data_cov_; ///< Motion data covariance
- MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
- FrameBaseWPtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
-};
-
-inline const Eigen::VectorXs& CaptureMotion::getData() const
-{
- return data_;
-}
-
-inline const Eigen::MatrixXs& CaptureMotion::getDataCovariance() const
-{
- return data_cov_;
-}
-
-inline void CaptureMotion::setData(const Eigen::VectorXs& _data)
-{
- assert(_data.size() == data_.size() && "Wrong size of data vector!");
- data_ = _data;
-}
-
-inline void CaptureMotion::setDataCovariance(const Eigen::MatrixXs& _data_cov)
-{
- assert(_data_cov.rows() == data_cov_.rows() && "Wrong number of rows of data vector!");
- assert(_data_cov.cols() == data_cov_.cols() && "Wrong number of cols of data vector!");
- data_cov_ = _data_cov;
-}
-
-inline const MotionBuffer& CaptureMotion::getBuffer() const
-{
- return buffer_;
-}
-
-inline MotionBuffer& CaptureMotion::getBuffer()
-{
- return buffer_;
-}
-
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib()
-{
- return getBuffer().get().back().jacobian_calib_;
-}
-
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts)
-{
- return getBuffer().getMotion(_ts).jacobian_calib_;
-}
-
-inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
-{
- WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!")
- return _delta + _delta_error;
-}
-
-inline FrameBasePtr CaptureMotion::getOriginFrame()
-{
- return origin_frame_ptr_.lock();
-}
-
-inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
-{
- origin_frame_ptr_ = _frame_ptr;
-}
-
-inline VectorXs CaptureMotion::getCalibrationPreint() const
-{
- return getBuffer().getCalibrationPreint();
-}
-
-inline void CaptureMotion::setCalibrationPreint(const VectorXs& _calib_preint)
-{
- getBuffer().setCalibrationPreint(_calib_preint);
-}
-
-inline VectorXs CaptureMotion::getDeltaPreint()
-{
- return getBuffer().get().back().delta_integr_;
-}
-
-inline VectorXs CaptureMotion::getDeltaPreint(const TimeStamp& _ts)
-{
- return getBuffer().getMotion(_ts).delta_integr_;
-}
-
-inline MatrixXs CaptureMotion::getDeltaPreintCov()
-{
- return getBuffer().get().back().delta_integr_cov_;
-}
-
-inline MatrixXs CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts)
-{
- return getBuffer().getMotion(_ts).delta_integr_cov_;
-}
-
-} // namespace wolf
-
-#endif /* SRC_CAPTURE_MOTION_H_ */
diff --git a/include/base/capture/capture_odom_2D.h b/include/base/capture/capture_odom_2D.h
deleted file mode 100644
index eafb6ead7bbe4b12f07234dc896f8fe7729b07bf..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_odom_2D.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * capture_odom_2D.h
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
-
-#ifndef CAPTURE_ODOM_2D_H_
-#define CAPTURE_ODOM_2D_H_
-
-#include "base/capture/capture_motion.h"
-
-#include "base/rotations.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(CaptureOdom2D);
-
-class CaptureOdom2D : public CaptureMotion
-{
- public:
- CaptureOdom2D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- CaptureOdom2D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- virtual ~CaptureOdom2D();
-
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
-
-};
-
-inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
-{
- Vector3s delta = _delta + _delta_error;
- delta(2) = pi2pi(delta(2));
- return delta;
-}
-
-} /* namespace wolf */
-
-#endif /* CAPTURE_ODOM_2D_H_ */
diff --git a/include/base/capture/capture_odom_3D.h b/include/base/capture/capture_odom_3D.h
deleted file mode 100644
index d1f29508bb60fa7bae5c620b7b010f368e00f100..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_odom_3D.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * capture_odom_3D.h
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
-
-#ifndef CAPTURE_ODOM_3D_H_
-#define CAPTURE_ODOM_3D_H_
-
-#include "base/capture/capture_motion.h"
-
-#include "base/rotations.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(CaptureOdom3D);
-
-class CaptureOdom3D : public CaptureMotion
-{
- public:
- CaptureOdom3D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- CaptureOdom3D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- virtual ~CaptureOdom3D();
-
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
-
-};
-
-} /* namespace wolf */
-
-#endif /* CAPTURE_ODOM_3D_H_ */
diff --git a/include/base/capture/capture_pose.h b/include/base/capture/capture_pose.h
deleted file mode 100644
index e96bda8643dcafabc0b1d8ff003708c3ad5cdd31..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_pose.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#ifndef CAPTURE_POSE_H_
-#define CAPTURE_POSE_H_
-
-//Wolf includes
-#include "base/capture/capture_base.h"
-#include "base/factor/factor_pose_2D.h"
-#include "base/factor/factor_pose_3D.h"
-#include "base/feature/feature_pose.h"
-
-//std includes
-//
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CapturePose);
-
-//class CapturePose
-class CapturePose : public CaptureBase
-{
- protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
-
- public:
-
- CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
-
- virtual ~CapturePose();
-
- virtual void emplaceFeatureAndFactor();
-
-};
-
-} //namespace wolf
-#endif
diff --git a/include/base/capture/capture_velocity.h b/include/base/capture/capture_velocity.h
deleted file mode 100644
index 4853fce2ac370b86e88c2d4776c514a3f4315455..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_velocity.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/**
- * \file capture_velocity.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_CAPTURE_VELOCITY_H_
-#define _WOLF_CAPTURE_VELOCITY_H_
-
-//wolf includes
-#include "base/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureVelocity);
-
-/**
- * @brief The CaptureVelocity class
- *
- * Represents a velocity.
- */
-class CaptureVelocity : public CaptureMotion
-{
-protected:
-
- using NodeBase::node_type_;
-
-public:
-
- /**
- * \brief Constructor
- **/
- CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr);
-
- CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
- const Eigen::MatrixXs& _velocity_cov,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- virtual ~CaptureVelocity() = default;
-
- const Eigen::VectorXs& getVelocity() const;
-
- const Eigen::MatrixXs& getVelocityCov() const;
-};
-
-} // namespace wolf
-
-#endif /* _WOLF_CAPTURE_VELOCITY_H_ */
diff --git a/include/base/capture/capture_void.h b/include/base/capture/capture_void.h
deleted file mode 100644
index b6b7a755ddd0a6044c28143988bb820b3337d9c7..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_void.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef CAPTURE_VOID_H_
-#define CAPTURE_VOID_H_
-
-//Wolf includes
-#include "base/capture/capture_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureVoid);
-
-
-//class CaptureVoid
-class CaptureVoid : public CaptureBase
-{
- public:
- CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
- virtual ~CaptureVoid();
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h
deleted file mode 100644
index a90cdb205178dd4c36cd302a7793e7e74c668df1..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_wheel_joint_position.h
+++ /dev/null
@@ -1,182 +0,0 @@
-/**
- * \file diff_drive_tools.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef CAPTURE_WHEEL_JOINT_POSITION_H_
-#define CAPTURE_WHEEL_JOINT_POSITION_H_
-
-//wolf includes
-#include "base/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureWheelJointPosition)
-
-/**
- * @brief The CaptureWheelJointPosition class
- *
- * Represents a list of wheel positions in radian.
- */
-class CaptureWheelJointPosition : public CaptureMotion
-{
-protected:
-
- using NodeBase::node_type_;
-
-public:
-
- /**
- * \brief Constructor
- **/
- CaptureWheelJointPosition(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _positions,
- FrameBasePtr _origin_frame_ptr);
-
- CaptureWheelJointPosition(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _positions,
- const Eigen::MatrixXs& _positions_cov,
- FrameBasePtr _origin_frame_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- virtual ~CaptureWheelJointPosition() = default;
-
- virtual VectorXs correctDelta(const VectorXs& _delta,
- const VectorXs& _delta_error) override;
-
- const Eigen::VectorXs& getPositions() const;
-
- const Eigen::MatrixXs& getPositionsCov() const;
-
-protected:
-
- Eigen::VectorXs positions_;
- Eigen::MatrixXs positions_cov_;
-};
-
-/// @todo Enforce some logic on the wheel joint pos data
-
-//template <typename E>
-//constexpr typename std::underlying_type<E>::type val(E&& e) noexcept
-//{
-// return static_cast<typename std::underlying_type<E>::type>(std::forward<E>(e));
-//}
-
-//template <std::size_t N>
-//struct NumWheelTraits
-//{
-// static constexpr std::size_t num_wheel = N;
-
-// struct Positions
-// {
-// using data_t = Eigen::Matrix<Scalar, num_wheel, 1>;
-// };
-//};
-
-//template <typename Derived>
-//struct MobileBaseControllerTraits
-//{
-// using controller_t = Derived;
-
-// static constexpr decltype(Derived::num_wheel) num_wheel = Derived::num_wheel;
-
-// using wheel_index_t = typename Derived::WheelsIndexes;
-
-// MobileBaseControllerTraits()
-// {
-// static_assert(true, "");
-// }
-//};
-
-//struct DiffDriveController : NumWheelTraits<2>
-//{
-// enum class WheelsIndexes : std::size_t
-// {
-// Left = 0,
-// Right = 1
-// };
-
-// class Positions
-// {
-// Eigen::Matrix<Scalar, num_wheel, 1> values_;
-
-// public:
-
-// Positions(const Scalar left_wheel_value,
-// const Scalar rigth_wheel_value) :
-// values_(Eigen::Matrix<Scalar, num_wheel, 1>(left_wheel_value, rigth_wheel_value))
-// { }
-// };
-//};
-
-//struct DiffDriveFourWheelsController : NumWheelTraits<4>
-//{
-// enum class WheelsIndexes : std::size_t
-// {
-// Front_Left = 0,
-// Front_Right = 1,
-// Rear_Left = 2,
-// Rear_Right = 3
-// };
-//};
-
-///**
-// * @brief The CaptureWheelJointPosition class
-// *
-// * Represents a list of wheel positions.
-// */
-//template <typename ControllerType>
-//class CaptureWheelJointPosition final :
-// public CaptureBase, MobileBaseControllerTraits<ControllerType>
-//{
-//public:
-
-// using MobileBaseControllerTraits<ControllerType>::controller_t;
-// using typename MobileBaseControllerTraits<ControllerType>::wheel_index_t;
-// using MobileBaseControllerTraits<ControllerType>::num_wheel;
-
-// /**
-// * \brief Constructor with ranges
-// **/
-// CaptureWheelJointPosition(const TimeStamp& _ts,
-// const SensorBasePtr& _sensor_ptr,
-// const Eigen::Matrix<Scalar, num_wheel, 1>& _positions) :
-// CaptureBase("WHEELS POSITION", _ts, _sensor_ptr),
-// positions_(_positions)
-// {
-// //
-// }
-
-// ~CaptureWheelJointPosition() = default;
-
-//// void setSensor(const SensorBasePtr sensor_ptr) override;
-
-// std::size_t getNumWheels() const noexcept
-// {
-// return num_wheel;
-// }
-
-// template <wheel_index_t wheel_index>
-// const Scalar& getPosition() const
-// {
-// return positions_(val(wheel_index));
-// }
-
-//protected:
-
-// Eigen::Matrix<Scalar, num_wheel, 1> positions_;
-
-// //SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
-//};
-
-//using CaptureDiffDriveWheelJointPosition = CaptureWheelJointPosition<DiffDriveController>;
-
-} // namespace wolf
-
-#endif /* CAPTURE_WHEEL_JOINT_POSITION_H_ */
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
deleted file mode 100644
index 9ee050cd738a4f257c138345da303c9fbae4f1ad..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ /dev/null
@@ -1,104 +0,0 @@
-#ifndef CERES_MANAGER_H_
-#define CERES_MANAGER_H_
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//wolf includes
-#include "base/solver/solver_manager.h"
-#include "base/ceres_wrapper/cost_function_wrapper.h"
-#include "base/ceres_wrapper/local_parametrization_wrapper.h"
-#include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
-
-namespace ceres {
-typedef std::shared_ptr<CostFunction> CostFunctionPtr;
-}
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CeresManager);
-
-/** \brief Ceres manager for WOLF
- *
- */
-
-class CeresManager : public SolverManager
-{
- protected:
-
- std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
- std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
-
- std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
-
- ceres::Solver::Options ceres_options_;
- ceres::Solver::Summary summary_;
- std::unique_ptr<ceres::Problem> ceres_problem_;
- std::unique_ptr<ceres::Covariance> covariance_;
-
- public:
-
- CeresManager(const ProblemPtr& _wolf_problem,
- const ceres::Solver::Options& _ceres_options
- = ceres::Solver::Options());
-
- ~CeresManager();
-
- ceres::Solver::Summary getSummary();
-
- std::unique_ptr<ceres::Problem>& getCeresProblemPtr()
- {
- return ceres_problem_;
- }
-
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
- = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
-
- virtual void computeCovariances(const StateBlockPtrList& st_list) override;
-
- virtual bool hasConverged() override;
-
- virtual SizeStd iterations() override;
-
- virtual Scalar initialCost() override;
-
- virtual Scalar finalCost() override;
-
- ceres::Solver::Options& getSolverOptions();
-
- void check();
-
- private:
-
- std::string solveImpl(const ReportVerbosity report_level) override;
-
- void addFactor(const FactorBasePtr& fac_ptr) override;
-
- void removeFactor(const FactorBasePtr& fac_ptr) override;
-
- void addStateBlock(const StateBlockPtr& state_ptr) override;
-
- void removeStateBlock(const StateBlockPtr& state_ptr) override;
-
- void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
-
- void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) override;
-
- ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
-};
-
-inline ceres::Solver::Summary CeresManager::getSummary()
-{
- return summary_;
-}
-
-inline ceres::Solver::Options& CeresManager::getSolverOptions()
-{
- return ceres_options_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h
deleted file mode 100644
index 5c7e72211e2348513a3266261db060da3c908a9f..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/cost_function_wrapper.h
+++ /dev/null
@@ -1,60 +0,0 @@
-#ifndef COST_FUNCTION_WRAPPER_H_
-#define COST_FUNCTION_WRAPPER_H_
-
-// WOLF
-#include "base/wolf.h"
-#include "base/factor/factor_analytic.h"
-
-// CERES
-#include "ceres/cost_function.h"
-
-// EIGEN
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
-
-class CostFunctionWrapper : public ceres::CostFunction
-{
- private:
-
- FactorBasePtr factor_ptr_;
-
- public:
-
- CostFunctionWrapper(FactorBasePtr _factor_ptr);
-
- virtual ~CostFunctionWrapper();
-
- virtual bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const;
-
- FactorBasePtr getFactor() const;
-};
-
-inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr) :
- ceres::CostFunction(), factor_ptr_(_factor_ptr)
-{
- for (auto st_block_size : factor_ptr_->getStateSizes())
- mutable_parameter_block_sizes()->push_back(st_block_size);
- set_num_residuals(factor_ptr_->getSize());
-}
-
-inline CostFunctionWrapper::~CostFunctionWrapper()
-{
-}
-
-inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, double* residuals, double** jacobians) const
-{
- //std::cout << "CostFunctionWrapper::Evaluate: type" << factor_ptr_->getType() << "\n";
- return factor_ptr_->evaluate(parameters, residuals, jacobians);
-}
-
-inline FactorBasePtr CostFunctionWrapper::getFactor() const
-{
- return factor_ptr_;
-}
-
-} // namespace wolf
-
-#endif /* TRUNK_SRC_COST_FUNCTION_WRAPPER_H_ */
diff --git a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
deleted file mode 100644
index fafa62aae9d0fae3f0a26e4bc08816dc33b9e73b..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * create_numeric_diff_cost_function.h
- *
- * Created on: Apr 5, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_
-#define SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_
-
-#include "ceres/cost_function.h"
-#include "ceres/numeric_diff_cost_function.h"
-
-// Factors
-#include "base/factor/factor_odom_2D.h"
-#include "base/factor/factor_base.h"
-
-namespace wolf {
-
-// Wolf ceres auto_diff creator
-template <class T>
-std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
- T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
- T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> > createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr)
-{
- return std::make_shared<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
- T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
- T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_factor_ptr).get());
-};
-
-inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(FactorBasePtr _fac_ptr)
-{
-// switch (_fac_ptr->getTypeId())
-// {
-// // just for testing
-// case FAC_ODOM_2D:
-// return createNumericDiffCostFunctionCeres<FactorOdom2D>(_fac_ptr);
-//
-// /* For adding a new factor, add the #include and a case:
-// case FAC_ENUM:
-// return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr);
-// */
-//
-// default:
- throw std::invalid_argument( "Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
-// }
-}
-
-} // namespace wolf
-
-#endif /* SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_ */
diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h
deleted file mode 100644
index fc046a7ec002c29b702133e89fb950b456a764d1..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/local_parametrization_wrapper.h
+++ /dev/null
@@ -1,67 +0,0 @@
-#ifndef LOCAL_PARAMETRIZATION_WRAPPER_H_
-#define LOCAL_PARAMETRIZATION_WRAPPER_H_
-
-// Fwd refs
-namespace wolf{
-class LocalParametrizationBase;
-}
-
-//Ceres includes
-#include "base/wolf.h"
-#include "ceres/ceres.h"
-
-namespace wolf {
-
-class LocalParametrizationWrapper : public ceres::LocalParameterization
-{
- private:
- LocalParametrizationBasePtr local_parametrization_ptr_;
-
- public:
-
- LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
-
- virtual ~LocalParametrizationWrapper() = default;
-
- virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const;
-
- virtual bool ComputeJacobian(const double* x, double* jacobian) const;
-
- virtual int GlobalSize() const;
-
- virtual int LocalSize() const;
-
- LocalParametrizationBasePtr getLocalParametrization() const;
-};
-
-using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>;
-
-} // namespace wolf
-
-#include "base/local_parametrization_base.h"
-
-namespace wolf {
-
-inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr) :
- local_parametrization_ptr_(_local_parametrization_ptr)
-{
-}
-
-inline int LocalParametrizationWrapper::GlobalSize() const
-{
- return local_parametrization_ptr_->getGlobalSize();
-}
-
-inline int LocalParametrizationWrapper::LocalSize() const
-{
- return local_parametrization_ptr_->getLocalSize();
-}
-
-inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametrization() const
-{
- return local_parametrization_ptr_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h
deleted file mode 100644
index d4945e066a372d5ec95578a87552e4369eec1b2e..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/qr_manager.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * qr_manager.h
- *
- * Created on: Jun 7, 2017
- * Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_QR_MANAGER_H_
-#define SRC_CERES_WRAPPER_QR_MANAGER_H_
-
-#include "base/solver/solver_manager.h"
-#include "base/solver_suitesparse/sparse_utils.h"
-
-namespace wolf
-{
-
-class QRManager : public SolverManager
-{
- protected:
- Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::COLAMDOrdering<int>> solver_;
- Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::NaturalOrdering<int>> covariance_solver_;
- Eigen::SparseMatrixs A_;
- Eigen::VectorXs b_;
- std::map<StateBlockPtr, unsigned int> sb_2_col_;
- std::map<FactorBasePtr, unsigned int> fac_2_row_;
- bool any_state_block_removed_;
- unsigned int new_state_blocks_;
- unsigned int N_batch_;
- bool pending_changes_;
-
- public:
-
- QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch);
-
- virtual ~QRManager();
-
- virtual std::string solve(const unsigned int& _report_level);
-
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
-
- virtual void computeCovariances(const StateBlockPtrList& _sb_list);
-
- private:
-
- bool computeDecomposition();
-
- virtual void addFactor(FactorBasePtr _fac_ptr);
-
- virtual void removeFactor(FactorBasePtr _fac_ptr);
-
- virtual void addStateBlock(StateBlockPtr _st_ptr);
-
- virtual void removeStateBlock(StateBlockPtr _st_ptr);
-
- virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
-
- void relinearizeFactor(FactorBasePtr _fac_ptr);
-};
-
-} /* namespace wolf */
-
-#endif /* SRC_CERES_WRAPPER_QR_MANAGER_H_ */
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
deleted file mode 100644
index 7252c409d2bf0c503e06b6fe74dec895e3754150..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/solver_manager.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef SOLVER_MANAGER_H_
-#define SOLVER_MANAGER_H_
-
-//wolf includes
-#include "base/wolf.h"
-#include "base/state_block.h"
-#include "base/factor/factor_base.h"
-
-namespace wolf {
-
-/** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
-typedef enum
-{
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
-} CovarianceBlocksToBeComputed;
-
-WOLF_PTR_TYPEDEFS(SolverManager);
-
-/** \brief Solver manager for WOLF
- *
- */
-
-class SolverManager
-{
- protected:
- ProblemPtr wolf_problem_;
-
- public:
- SolverManager(ProblemPtr _wolf_problem);
-
- virtual ~SolverManager();
-
- virtual std::string solve(const unsigned int& _report_level) = 0;
-
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS) = 0;
-
- virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
-
- virtual void update();
-
- virtual ProblemPtr getProblem();
-
- private:
-
- virtual void addFactor(FactorBasePtr _fac_ptr) = 0;
-
- virtual void removeFactor(FactorBasePtr _fac_ptr) = 0;
-
- virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
-
- virtual void removeStateBlock(StateBlockPtr _st_ptr) = 0;
-
- virtual void updateStateBlockStatus(StateBlockPtr _st_ptr) = 0;
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/ceres_wrapper/sparse_utils.h b/include/base/ceres_wrapper/sparse_utils.h
deleted file mode 100644
index 66f49b9c8f868e84f4223408ef6f85980e89bf1b..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/sparse_utils.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * sparse_utils.h
- *
- * Created on: Jul 2, 2015
- * Author: jvallve
- */
-
-#ifndef SPARSE_UTILS_H_
-#define SPARSE_UTILS_H_
-
-// eigen includes
-//#include <eigen3/Eigen/Sparse>
-
-namespace wolf
-{
-
-void eraseBlockRow(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
-{
- A.middleRows(_row,_n_rows) = Eigen::SparseMatrixs(_n_rows,A.cols());
- A.makeCompressed();
-}
-
-void eraseBlockRow(Eigen::SparseMatrix<Scalar, Eigen::ColMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
-{
- A.prune([&](int i, int, Scalar) { return i >= _row && i < _row + _n_rows; });
- A.makeCompressed();
-}
-
-void eraseBlockCol(Eigen::SparseMatrix<Scalar, Eigen::ColMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
-{
- A.middleCols(_col,_n_cols) = Eigen::SparseMatrixs(A.rows(),_n_cols);
- A.makeCompressed();
-}
-
-void eraseBlockCol(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
-{
- A.prune([&](int, int j, Scalar) { return j >= _col && j < _col + _n_cols; });
- A.makeCompressed();
-}
-
-template<int _Options, typename _StorageIndex>
-void assignSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
-{
- for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
- for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.coeffRef(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
-
- original.makeCompressed();
-}
-
-template<int _Options, typename _StorageIndex>
-void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
-{
- for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
- for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col);
-
- original.makeCompressed();
-}
-
-template<int _Options, typename _StorageIndex>
-void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
-{
- for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
- for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
-
- original.makeCompressed();
-}
-
-void assignBlockRow(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& ins, const unsigned int& _row)
-{
- assert(A.rows() >= _row + ins.rows() && A.cols() == ins.cols());
- A.middleRows(_row, ins.rows()) = ins;
- A.makeCompressed();
-}
-
-Eigen::SparseMatrixs createBlockDiagonal(const std::vector<Eigen::MatrixXs>& _diag_blocs)
-{
- unsigned int dim = _diag_blocs.front().rows();
- unsigned int size = dim * _diag_blocs.size();
-
- Eigen::SparseMatrixs M(size,size);
-
- unsigned int pos = 0;
- for (const Eigen::MatrixXs& omega_k : _diag_blocs)
- {
- insertSparseBlock(omega_k, M, pos, pos);
- pos += dim;
- }
-
- M.makeCompressed();
-
- return M;
-}
-
-template<int _Options, typename _StorageIndex>
-void getDiagonalBlocks(const Eigen::SparseMatrix<Scalar,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXs>& diag_, const unsigned int& dim)
-{
- assert(_M.rows() % dim == 0 && "number of rows must be multiple of dimension");
- diag_.clear();
- for (auto i = 0; i < _M.rows(); i += dim)
- diag_.push_back(Eigen::MatrixXs(_M.block(i,i,dim,dim)));
-}
-
-} // namespace wolf
-#endif /* SPARSE_UTILS_H_ */
diff --git a/include/base/diff_drive_tools.h b/include/base/diff_drive_tools.h
deleted file mode 100644
index 142b8c5022ccca18eaf1edc1214984b93bd816c4..0000000000000000000000000000000000000000
--- a/include/base/diff_drive_tools.h
+++ /dev/null
@@ -1,424 +0,0 @@
-/**
- * \file diff_drive_tools.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
-#define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
-
-#include "base/eigen_assert.h"
-
-namespace wolf {
-
-enum class DiffDriveModel : std::size_t
-{
- Two_Factor_Model = 0,
- Three_Factor_Model = 1,
- Five_Factor_Model = 2
-};
-
-constexpr double ANGULAR_VELOCITY_MIN(1e-8);
-
-/**
- * @brief computeDiffDriveComand. Compute wheels velocity comands given
- * linear and angular velocity.
- *
- * @param comand. Linear and angular velocity comands.
- * @param wheel_comand. Wheels velocity comands.
- *
- * @param left_radius. Left wheel radius.
- * @param right_radius. Right wheel radius.
- * @param separation. Wheels separation.
- */
-//template <typename T0, typename T1>
-//void computeDiffDriveComand(const Eigen::MatrixBase<T0> &comand,
-// Eigen::MatrixBase<T1> &wheel_comand,
-// const typename T1::Scalar left_radius,
-// const typename T1::Scalar right_radius,
-// const typename T1::Scalar separation)
-//{
-// Eigen::VectorSizeCheck<2>::check(comand);
-
-// using T = typename T1::Scalar;
-
-// const T linear = comand(0);
-// const T angular = comand(1);
-
-// wheel_comand = Eigen::Matrix<T, 2, 1>((linear - angular * separation * T(0.5)) / left_radius,
-// (linear + angular * separation * T(0.5)) / right_radius);
-//}
-
-/**
- * @brief computeDiffDriveComand. Compute wheels velocity comands given
- * linear and angular velocity.
- *
- * @param comand. Linear and angular velocity comands.
- * @param wheel_comand. Wheels velocity comands.
- *
- * @param wheel_radius. Wheel radius.
- * @param separation. Wheels separation.
- */
-//template <typename T0, typename T1>
-//void computeDiffDriveComand(const Eigen::MatrixBase<T0> &comand,
-// Eigen::MatrixBase<T1> &wheel_comand,
-// const typename T1::Scalar wheel_radius,
-// const typename T1::Scalar separation)
-//{
-// computeDiffDriveComand(comand, wheel_comand,
-// wheel_radius, wheel_radius, separation);
-//}
-
-/**
- * @brief VelocityComand. Holds a velocity comand vector
- * together with its covariance.
- *
- * @note
- * 2d : [linear_x, angular]
- *
- */
-template <typename Scalar = double>
-struct VelocityComand
-{
- Eigen::Matrix<Scalar, Eigen::Dynamic, 1> comand;
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> comand_cov;
-};
-
-namespace detail {
-
-template <DiffDriveModel>
-struct wheelPositionIncrementToVelocityHelper
-{
- template <typename T0, typename T1, typename T2>
- static std::tuple<VelocityComand<typename T0::Scalar>,
- Eigen::Matrix<typename T0::Scalar, 2, 2>,
- Eigen::Matrix<typename T0::Scalar, 2, 3>>
- wheelPositionIncrementToVelocity(const Eigen::MatrixBase<T0>& position_increment,
- const Eigen::MatrixBase<T1>& position_increment_cov,
- const typename T0::Scalar left_radius,
- const typename T0::Scalar right_radius,
- const typename T0::Scalar separation,
- const Eigen::MatrixBase<T2>& factors,
- const typename T0::Scalar dt);
-};
-
-} /* namespace detail */
-
-/**
- * @brief Convert from wheels joint positions to per-wheel velocity comands.
- * @param[in] position_increment. A vector containing the wheels position increments.
- * @param[in] position_increment_cov. The covariance associated to the vector position_increment.
- * @param[in] left_radius. The left wheel radius.
- * @param[in] right_radius. The right wheel radius.
- * @param[in] separation. The distance separing both wheels.
- * @param[in] factors. The diff-drive correction factors (calibration).
- * @param[in] dt. UNUSED.
- *
- * @return std::tuple. First element is the computed VelocityComand,
- * second element is the Jacobian matrix J_vel_data,
- * third element is the Jacobian matrix J_vel_factor.
- */
-template <DiffDriveModel M, typename T0, typename T1, typename T2>
-std::tuple<VelocityComand<typename T0::Scalar>,
- Eigen::Matrix<typename T0::Scalar, 2, 2>,
- Eigen::Matrix<typename T0::Scalar, 2, 3>>
-wheelPositionIncrementToVelocity(const Eigen::MatrixBase<T0>& position_increment,
- const Eigen::MatrixBase<T1>& position_increment_cov,
- const typename T0::Scalar left_radius,
- const typename T0::Scalar right_radius,
- const typename T0::Scalar separation,
- const Eigen::MatrixBase<T2>& factors,
- const typename T0::Scalar dt)
-{
- return detail::wheelPositionIncrementToVelocityHelper<M>::wheelPositionIncrementToVelocity(
- position_increment, position_increment_cov,
- left_radius, right_radius, separation,
- factors, dt);
-}
-
-/**
- * @brief WheelPositionAbsoluteToIncrement.
- * Simple class to convert from absolute position to increment position.
- *
- * @todo handle num wheels 4-6-etc
- */
-template <typename T>
-class WheelPositionAbsoluteToIncrement
-{
-public:
-
- WheelPositionAbsoluteToIncrement() = default;
- ~WheelPositionAbsoluteToIncrement() = default;
-
- template <typename T0>
- WheelPositionAbsoluteToIncrement(const Eigen::MatrixBase<T0>& positions)
- {
- init(positions);
- }
-
- inline bool init() const noexcept { return init_; }
-
- template <typename T0>
- void update(const Eigen::MatrixBase<T0>& positions)
- {
- Eigen::VectorSizeCheck<2>::check(positions);
-
- positions_ = positions.template cast<T>();
- }
-
- template <typename T0>
- void init(const Eigen::MatrixBase<T0>& positions)
- {
- update(positions);
-
- init_ = true;
- }
-
- template <typename T0, typename T1>
- void toIncrement(const Eigen::MatrixBase<T0>& positions,
- Eigen::MatrixBase<T1>& positions_increment)
- {
- Eigen::VectorSizeCheck<2>::check(positions);
-
- if (!init_) init(positions);
-
- positions_increment =
- (positions - positions_.template cast<typename T0::Scalar>()).
- template cast<typename T1::Scalar>();
- }
-
- template <typename T0, typename T1>
- void operator() (const Eigen::MatrixBase<T0>& positions,
- Eigen::MatrixBase<T1>& positions_increment)
- {
- toIncrement(positions, positions_increment);
- update(positions);
- }
-
-protected:
-
- bool init_ = false;
-
- Eigen::Matrix<T, Eigen::Dynamic, 1> positions_;
-};
-
-/**
- * @brief integrateRungeKutta. Runge-Kutta 2nd order integration.
- *
- * @param[in] data. The input linear and angular velocities.
- * @param[in] data_cov. The covariance matrix associated to data.
- * @param[out] delta. The computed delta (2d).
- * @param[out] delta_cov. The covariance matrix associated to delta.
- * @param[out] jacobian. The Jacobian matrix J_delta_vel
- *
- * @note
- *
- * Linear velocity [m] (linear displacement, i.e. m/s * dt)
- * Angular velocity [rad] (angular displacement, i.e. rad/s * dt)
- *
- * MATHS of delta
- * dx = dv * cos( dw * 0.5 )
- * dy = dv * sin( dw * 0.5 )
- * dth = dw
- */
-template <typename T0, typename T1, typename T2, typename T3, typename T4>
-void integrateRungeKutta(const Eigen::MatrixBase<T0> &data,
- const Eigen::MatrixBase<T1> &data_cov,
- Eigen::MatrixBase<T2> &delta,
- Eigen::MatrixBase<T3> &delta_cov,
- Eigen::MatrixBase<T4> &jacobian)
-{
- using std::sin;
- using std::cos;
-
- /// @note data is size 3 -> linear vel on x + angular vel on yaw
- Eigen::VectorSizeCheck<2>::check(data);
- /// @todo check dim
- Eigen::MatrixSizeCheck<2,2>::check(data_cov);
-
- using T = typename T2::Scalar;
-
- const T v = data(0);
- const T w = data(1);
-
- const T w_05 = w * T(.5);
-
- const T cos_w_05 = cos(w_05);
- const T sin_w_05 = sin(w_05);
-
- delta = Eigen::Matrix<T, 3, 1>(cos_w_05 * v,
- sin_w_05 * v,
- w );
- // Fill delta covariance
- Eigen::Matrix<typename T3::Scalar,
- Eigen::MatrixBase<T2>::RowsAtCompileTime,
- Eigen::MatrixBase<T0>::RowsAtCompileTime> J;
-
- J.setZero(delta.rows(), data.rows());
-
- // Compute jacobian
- jacobian =
- Eigen::Matrix<typename T4::Scalar,
- Eigen::MatrixBase<T2>::RowsAtCompileTime,
- Eigen::MatrixBase<T0>::RowsAtCompileTime>::Zero(delta.rows(), data.rows());
-
- Eigen::MatrixSizeCheck<3,2>::check(jacobian);
-
- jacobian(0,0) = cos_w_05;
- jacobian(1,0) = sin_w_05;
- jacobian(2,0) = typename T3::Scalar(0);
-
- jacobian(0,1) = -v * sin_w_05 * typename T3::Scalar(.5);
- jacobian(1,1) = v * cos_w_05 * typename T3::Scalar(.5);
- jacobian(2,1) = typename T3::Scalar(1);
-
- // Fill delta covariance
- delta_cov = J * data_cov * J.transpose();
-}
-
-/**
- * @brief integrateExact. Exact integration.
- *
- * @param[in] data. The input linear and angular velocities.
- * @param[in] data_cov. The covariance matrix associated to data.
- * @param[out] delta. The computed delta (2d).
- * @param[out] delta_cov. The covariance matrix associated to delta.
- * @param[out] jacobian. The Jacobian matrix J_delta_vel
- *
- * @note
- *
- * Linear velocity [m] (linear displacement, i.e. m/s * dt)
- * Angular velocity [rad] (angular displacement, i.e. rad/s * dt)
- *
- * MATHS of delta
- * dx = dv / dw * (sin(dw) - sin(0))
- * dy = -dv / dw * (cos(dw) - cos(0))
- * dth = dw
- */
-template <typename T0, typename T1, typename T2, typename T3, typename T4>
-void integrateExact(const Eigen::MatrixBase<T0> &data,
- const Eigen::MatrixBase<T1> &data_cov,
- Eigen::MatrixBase<T2> &delta,
- Eigen::MatrixBase<T3> &delta_cov,
- Eigen::MatrixBase<T4> &jacobian)
-{
- using std::sin;
- using std::cos;
-
- /// @note data is size 3 -> linear vel on x & y + angular vel on yaw
- Eigen::VectorSizeCheck<2>::check(data);
- /// @todo check dim
- Eigen::MatrixSizeCheck<2,2>::check(data_cov);
-
- using T = typename T2::Scalar;
-
- // Compute delta
-
- const T v = data(0);
- const T w = data(1);
-
- const T inv_w = T(1) / w;
-
- const T r = v * inv_w;
-
- const T theta_1 = w;
-
- const T sin_theta_0 = 0;
- const T cos_theta_0 = 1;
-
- const T sin_theta_1 = sin(theta_1);
- const T cos_theta_1 = cos(theta_1);
-
- const T sin_diff = sin_theta_1 - sin_theta_0;
- const T cos_diff = cos_theta_1 - cos_theta_0;
-
- delta = Eigen::Matrix<T, 3, 1>( r * sin_diff,
- -r * cos_diff,
- w );
-
- const T inv_w2 = inv_w * inv_w;
-
- // Compute jacobian
- jacobian =
- Eigen::Matrix<typename T4::Scalar,
- Eigen::MatrixBase<T2>::RowsAtCompileTime,
- Eigen::MatrixBase<T0>::RowsAtCompileTime>::Zero(delta.rows(), data.rows());
-
- Eigen::MatrixSizeCheck<3,2>::check(jacobian);
-
- jacobian(0,0) = sin_diff * inv_w;
- jacobian(1,0) = -cos_diff * inv_w;
- jacobian(2,0) = T(0);
-
- jacobian(0,1) = (v * cos_theta_1 * inv_w) - (v * sin_diff * inv_w2);
- jacobian(1,1) = (v * sin_theta_1 * inv_w) + (v * cos_diff * inv_w2);
- jacobian(2,1) = T(1);
-
- // Fill delta covariance
- delta_cov = jacobian * data_cov * jacobian.transpose();
-}
-
-/**
- * @brief integrate. Helper function to call either
- * `integrateRung` or `integrateExact` depending on the
- * angular velocity value.
- *
- * @see integrateRungeKutta
- * @see integrateExact
- * @see ANGULAR_VELOCITY_MIN
- */
-template <typename T0, typename T1, typename T2, typename T3, typename T4>
-void integrate(const Eigen::MatrixBase<T0>& data,
- const Eigen::MatrixBase<T1>& data_cov,
- Eigen::MatrixBase<T2>& delta,
- Eigen::MatrixBase<T3>& delta_cov,
- Eigen::MatrixBase<T4>& jacobian)
-{
- (data(1) < ANGULAR_VELOCITY_MIN) ?
- integrateRungeKutta(data, data_cov, delta, delta_cov, jacobian) :
- integrateExact(data, data_cov, delta, delta_cov, jacobian);
-}
-
-/**
- * @brief computeWheelJointPositionCov. Compute a covariance matrix to associate
- * to wheel position increment.
- *
- * Set covariance matrix (diagonal):
- * second term is the wheel resolution covariance,
- * similar to a DC offset equal to half of the resolution,
- * which is the theoretical average error.
- *
- * Introduction to Autonomous Mobile Robots, 1st Edition, 2004
- * Roland Siegwart, Illah R. Nourbakhsh
- * Section: 5.2.4 'An error model for odometric position estimation' (pp. 186-191)
- */
-template <typename T>
-Eigen::Matrix<typename T::Scalar, 2, 2> computeWheelJointPositionCov(
- const Eigen::MatrixBase<T>& data,
- const typename T::Scalar left_wheel_resolution,
- const typename T::Scalar right_wheel_resolution,
- const typename T::Scalar left_wheel_cov_gain,
- const typename T::Scalar right_wheel_cov_gain)
-{
- using std::abs;
-
- Eigen::VectorSizeCheck<2>::check(data);
-
- using S = typename T::Scalar;
-
- const S dp_var_l = left_wheel_cov_gain * abs(data(0));
- const S dp_var_r = right_wheel_cov_gain * abs(data(1));
-
- Eigen::Matrix<S, 2, 2> data_cov;
- data_cov << dp_var_l + (left_wheel_resolution * S(0.5)), 0,
- 0, dp_var_r + (right_wheel_resolution * S(0.5));
-
- return data_cov;
-}
-
-} // namespace wolf
-
-#include "base/diff_drive_tools.hpp"
-
-#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */
diff --git a/include/base/diff_drive_tools.hpp b/include/base/diff_drive_tools.hpp
deleted file mode 100644
index f70ca2c373796de8eccfff4debb6cb9ae9b7fab9..0000000000000000000000000000000000000000
--- a/include/base/diff_drive_tools.hpp
+++ /dev/null
@@ -1,115 +0,0 @@
-/**
- * \file diff_drive_tools.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_
-#define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_
-
-// un-comment for IDE highlight
-//#include "diff_drive_tools.h"
-#include <tuple>
-
-namespace wolf {
-namespace detail {
-
-template <>
-template <typename T0, typename T1, typename T2>
-std::tuple<VelocityComand<typename T0::Scalar>, Eigen::Matrix<typename T0::Scalar, 2, 2>, Eigen::Matrix<typename T0::Scalar, 2, 3>>
-wheelPositionIncrementToVelocityHelper<DiffDriveModel::Two_Factor_Model>::wheelPositionIncrementToVelocity(
- const Eigen::MatrixBase<T0>& position_increment,
- const Eigen::MatrixBase<T1>& position_increment_cov,
- const typename T0::Scalar left_radius,
- const typename T0::Scalar right_radius,
- const typename T0::Scalar separation,
- const Eigen::MatrixBase<T2>& factors,
- const typename T0::Scalar /*dt*/)
-{
- Eigen::VectorSizeCheck<2>::check(position_increment);
- Eigen::MatrixSizeCheck<2,2>::check(position_increment_cov);
-
- Eigen::VectorSizeCheck<2>::check(factors);
-
- using T = typename T0::Scalar;
-
- const T left_wheel_vel = (left_radius * factors(0)) * position_increment(0);
- const T right_wheel_vel = (right_radius * factors(0)) * position_increment(1);
-
- const T o_5(0.5);
- const T s_f = separation * factors(1);
-
- const Eigen::Matrix<T, 2, 1> comand =
- Eigen::Matrix<T, 2, 1>((right_wheel_vel + left_wheel_vel) * o_5,
- (right_wheel_vel - left_wheel_vel) / s_f) /* * dt*/;
-
- const T p_r = position_increment(1) * right_radius;
- const T p_l = position_increment(0) * left_radius;
-
- Eigen::Matrix<T, 2, 2> J_vel_posinc;
-
- J_vel_posinc << left_radius * factors(0) * o_5 , right_radius * factors(1) * o_5,
- left_radius * factors(0) / s_f, -right_radius * factors(1) / s_f;
-
- Eigen::Matrix<T, 2, 3> J_vel_factor;
-
- J_vel_factor << (p_l + p_r) * o_5, 0,
- (p_l - p_r) / s_f, ((p_r - p_l) * factors(0)) / (s_f * factors(1));
-
- const Eigen::Matrix<T, 2, 2> comand_cov = J_vel_posinc * position_increment_cov * J_vel_posinc.transpose();
-
- return std::make_tuple(VelocityComand<T>{comand, comand_cov}, J_vel_posinc, J_vel_factor);
-}
-
-template <>
-template <typename T0, typename T1, typename T2>
-std::tuple<VelocityComand<typename T0::Scalar>, Eigen::Matrix<typename T0::Scalar, 2, 2>, Eigen::Matrix<typename T0::Scalar, 2, 3>>
-wheelPositionIncrementToVelocityHelper<DiffDriveModel::Three_Factor_Model>::wheelPositionIncrementToVelocity(
- const Eigen::MatrixBase<T0>& position_increment,
- const Eigen::MatrixBase<T1>& position_increment_cov,
- const typename T0::Scalar left_radius,
- const typename T0::Scalar right_radius,
- const typename T0::Scalar separation,
- const Eigen::MatrixBase<T2>& factors,
- const typename T0::Scalar /*dt*/)
-{
- Eigen::VectorSizeCheck<2>::check(position_increment);
- Eigen::MatrixSizeCheck<2,2>::check(position_increment_cov);
-
- Eigen::VectorSizeCheck<3>::check(factors);
-
- using T = typename T0::Scalar;
-
- const T left_wheel_vel = (left_radius * factors(0)) * position_increment(0);
- const T right_wheel_vel = (right_radius * factors(1)) * position_increment(1);
-
- const T o_5(0.5);
- const T s_f = separation * factors(2);
-
- const Eigen::Matrix<T, 2, 1> comand =
- Eigen::Matrix<T, 2, 1>((right_wheel_vel + left_wheel_vel) * o_5,
- (right_wheel_vel - left_wheel_vel) / s_f) /* * dt*/;
-
- const T p_l = position_increment(0) * left_radius;
- const T p_r = position_increment(1) * right_radius;
-
- Eigen::Matrix<T, 2, 2> J_vel_posinc;
-
- J_vel_posinc << left_radius * factors(0) * o_5 , right_radius * factors(1) * o_5,
- left_radius * factors(0) / s_f, -right_radius * factors(1) / s_f;
-
- Eigen::Matrix<T, 2, 3> J_vel_factor;
-
- J_vel_factor << p_l * o_5, p_r * o_5, 0,
- p_l / s_f, -p_r / s_f, (p_r * factors(1) - p_l * factors(0)) / (s_f * factors(2));
-
- const Eigen::Matrix<T, 2, 2> comand_cov = J_vel_posinc * position_increment_cov * J_vel_posinc.transpose();
-
- return std::make_tuple(VelocityComand<T>{comand, comand_cov}, J_vel_posinc, J_vel_factor);
-}
-
-} /* namespace detail */
-} /* namespace wolf */
-
-#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_ */
diff --git a/include/base/eigen_assert.h b/include/base/eigen_assert.h
deleted file mode 100644
index 5c079da7aab383eed93578869b219c65854419d4..0000000000000000000000000000000000000000
--- a/include/base/eigen_assert.h
+++ /dev/null
@@ -1,89 +0,0 @@
-#ifndef _WOLF_EIGEN_ASSERT_H_
-#define _WOLF_EIGEN_ASSERT_H_
-
-#include <Eigen/Dense>
-
-namespace Eigen {
-
-//////////////////////////////////////////////////////////
-/** Check Eigen Matrix sizes, both statically and dynamically
- *
- * Help:
- *
- * The WOLF project implements many template functions using Eigen Matrix and Quaternions, in different versions
- * (Static size, Dynamic size, Map, Matrix expression).
- *
- * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
- *
- * EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
- * EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
- * EIGEN_STATIC_ASSERT_VECTOR_ONLY
- *
- * but they do not work if the evaluated types are of dynamic size.
- *
- * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind:
- *
- * template<typename Derived>
- * inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){
- *
- * MatrixSizeCheck<3,1>::check(_v); /// We check here the size of the input parameter
- *
- * typedef typename Derived::Scalar T;
- *
- * ... code ...
- *
- * return M;
- * }
- *
- * The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ).
- * This check is performed statically or dynamically, depending on the type of argument provided.
- */
-template<int Size, int DesiredSize>
-struct StaticDimCheck
-{
- template<typename T>
- StaticDimCheck(const T&)
- {
- static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
- }
-};
-
-template<int DesiredSize>
-struct StaticDimCheck<Eigen::Dynamic, DesiredSize>
-{
- template<typename T>
- StaticDimCheck(const T& t)
- {
- if (t != DesiredSize) std::cerr << "t : " << t << " != DesiredSize : " << DesiredSize << std::endl;
- assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
- }
-};
-
-template<int DesiredR, int DesiredC>
-struct MatrixSizeCheck
-{
- /** \brief Assert matrix size dynamically or statically
- *
- * @param t the variable for which we wish to assert the size.
- *
- * Usage: to assert that t is size (Rows x Cols)
- *
- * MatrixSizeCheck<Rows, Cols>::check(t);
- */
- template<typename T>
- static void check(const Eigen::MatrixBase<T>& t)
- {
- StaticDimCheck<Eigen::MatrixBase<T>::RowsAtCompileTime, DesiredR>(t.rows());
- StaticDimCheck<Eigen::MatrixBase<T>::ColsAtCompileTime, DesiredC>(t.cols());
- }
-};
-
-template <int Dim>
-using VectorSizeCheck = MatrixSizeCheck<Dim, 1>;
-
-template <int Dim>
-using RowVectorSizeCheck = MatrixSizeCheck<1, Dim>;
-
-} /* namespace Eigen */
-
-#endif /* _WOLF_EIGEN_ASSERT_H_ */
diff --git a/include/base/eigen_predicates.h b/include/base/eigen_predicates.h
deleted file mode 100644
index 55f2eeeb75df3360e504105bd6cb08660b1cc5ea..0000000000000000000000000000000000000000
--- a/include/base/eigen_predicates.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/**
- * \file eigen_predicates.h
- * \brief Some utils for comparing Eigen types
- * \author Jeremie Deray
- * Created on: 24/10/2017
- */
-
-#ifndef _WOLF_EIGEN_PREDICATES_H_
-#define _WOLF_EIGEN_PREDICATES_H_
-
-#include "base/rotations.h"
-
-namespace wolf {
-
-/// @brief check that each Matrix/Vector elem is approx equal to value +- precision
-///
-/// @note we overload this rather than using Eigen::Matrix::isConstant() since it is
-/// defined as :
-/// abs(x - y) <= (min)(abs(x), abs(y)) * prec
-/// which does not play nice with y = 0
-auto is_constant = [](const Eigen::MatrixXs& lhs, const Scalar val, const Scalar precision)
- {
- for (Eigen::MatrixXs::Index j = 0; j < lhs.cols(); ++j)
- for (Eigen::MatrixXs::Index i = 0; i < lhs.rows(); ++i)
- if (std::abs(lhs.coeff(i, j) - val) > precision)
- return false;
- return true;
- };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_zero = [](const Eigen::MatrixXs& lhs, const Scalar precision)
- { return is_constant(lhs, 0, precision); };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_diff_zero = [](const Eigen::MatrixXs& lhs, const Eigen::MatrixXs& rhs, const Scalar precision)
- { return pred_zero(lhs - rhs, precision); };
-
-/// @brief check that the Matrix/Vector difference norm is approx 0 +- precision
-auto pred_diff_norm_zero = [](const Eigen::MatrixXs& lhs, const Eigen::MatrixXs& rhs, const Scalar precision)
- { return std::abs((lhs - rhs).norm()) <= std::abs(precision); };
-
-/// @brief check that the lhs Matrix/Vector is elem-wise approx the rhs +-precision
-auto pred_is_approx = [](const Eigen::MatrixXs& lhs, const Eigen::MatrixXs& rhs, const Scalar precision)
- { return lhs.isApprox(rhs, precision); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to rhs
-/// is <= precision
-auto pred_quat_is_approx = [](const Eigen::Quaternions& lhs, const Eigen::Quaternions& rhs, const Scalar precision)
- { return pred_zero( log_q(rhs * lhs.inverse()), precision ); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to identity
-/// is <= precision
-auto pred_quat_identity = [](const Eigen::Quaternions& lhs, const Scalar precision)
- { return pred_quat_is_approx(lhs, Eigen::Quaternions::Identity(), precision); };
-
-/// @brief check that rotation angle to get from lhs angle to rhs is <= precision
-auto pred_angle_is_approx = [](const Scalar lhs, const Scalar rhs, const Scalar precision)
- { return std::abs(pi2pi(lhs - rhs)) <= std::abs(precision); };
-
-/// @brief check that rotation angle to get from lhs angle to 0 is <= precision
-auto pred_angle_zero = [](const Scalar lhs, const Scalar precision)
- { return pred_angle_is_approx(lhs, 0, precision); };
-
-/// @brief check that the lhs pose is approx rhs +- precision
-///
-/// @note
-/// Comparison is :
-/// d = inv(lhs) * rhs
-/// d.translation().elem_wise() ~ 0 (+- precision)
-///
-/// if pose 3d :
-/// d.rotation_as_quaternion() ~ quaternion.getIdentity (+- precision)
-///
-/// else if pose 2d:
-/// d.rotation_angle() ~ 0 (+- precision)
-///
-/// else throw std::runtime
-///
-/// @see pred_zero for translation comparison
-/// @see pred_quat_identity for 3d rotation comparison
-/// @see pred_angle_zero for 2d rotation comparison
-//auto pred_pose_is_approx = [](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs, const Scalar precision)
-// {
-// const Eigen::MatrixXs d = lhs.inverse() * rhs;
-// const bool tok = pred_zero(d.rightCols(1), precision);
-
-// const bool qok = (lhs.rows() == 3)?
-// pred_quat_identity(Eigen::Quaternions(d.block(0,0,3,1)),
-// precision)
-// : (lhs.rows() == 2)? pred_angle_zero(getYaw(d), precision)
-// : throw std::runtime_error("Canno't compare pose in Dim > 3 !");
-
-// return tok && qok;
-// };
-
-} // namespace wolf
-
-#endif /* _WOLF_EIGEN_PREDICATES_H_ */
diff --git a/include/base/factor/factor_AHP.h b/include/base/factor/factor_AHP.h
deleted file mode 100644
index a6448b06458658949bff014aa93da139f74f0e83..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_AHP.h
+++ /dev/null
@@ -1,200 +0,0 @@
-#ifndef FACTOR_AHP_H
-#define FACTOR_AHP_H
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/landmark/landmark_AHP.h"
-#include "base/sensor/sensor_camera.h"
-//#include "base/feature/feature_point_image.h"
-#include "base/pinhole_tools.h"
-
-#include <iomanip> //setprecision
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorAHP);
-
-//class
-class FactorAHP : public FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>
-{
- protected:
- Eigen::Vector3s anchor_sensor_extrinsics_p_;
- Eigen::Vector4s anchor_sensor_extrinsics_o_;
- Eigen::Vector4s intrinsic_;
- Eigen::VectorXs distortion_;
-
- public:
-
- FactorAHP(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorAHP() = default;
-
- template<typename T>
- void expectation(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _expectation) const;
-
- Eigen::VectorXs expectation() const;
-
- template<typename T>
- bool operator ()(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _residuals) const;
-
- // Static creator method
- static FactorAHPPtr create(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _lmk_ahp_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
-};
-
-inline FactorAHP::FactorAHP(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>("AHP",
- _landmark_ptr->getAnchorFrame(),
- nullptr,
- nullptr,
- _landmark_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getCapture()->getFrame()->getP(),
- _ftr_ptr->getCapture()->getFrame()->getO(),
- _landmark_ptr->getAnchorFrame()->getP(),
- _landmark_ptr->getAnchorFrame()->getO(),
- _landmark_ptr->getP()),
- anchor_sensor_extrinsics_p_(_ftr_ptr->getCapture()->getSensorP()->getState()),
- anchor_sensor_extrinsics_o_(_ftr_ptr->getCapture()->getSensorO()->getState()),
- intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState())
-{
- // obtain some intrinsics from provided sensor
- distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapture()->getSensor()))->getDistortionVector();
-}
-
-inline Eigen::VectorXs FactorAHP::expectation() const
-{
- FrameBasePtr frm_current = getFeature()->getCapture()->getFrame();
- FrameBasePtr frm_anchor = getFrameOther();
- LandmarkBasePtr lmk = getLandmarkOther();
-
- const Eigen::MatrixXs frame_current_pos = frm_current->getP()->getState();
- const Eigen::MatrixXs frame_current_ori = frm_current->getO()->getState();
- const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getP()->getState();
- const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getO()->getState();
- const Eigen::MatrixXs lmk_pos_hmg = lmk ->getP()->getState();
-
- Eigen::Vector2s exp;
- expectation(frame_current_pos.data(), frame_current_ori.data(), frame_anchor_pos.data(),
- frame_anchor_ori.data(), lmk_pos_hmg.data(), exp.data());
-
- return exp;
-}
-
-template<typename T>
-inline void FactorAHP::expectation(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _expectation) const
-{
- using namespace Eigen;
-
- // All involved transforms typedef
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
-
- // world to anchor robot transform
- Map<const Matrix<T, 3, 1> > p_w_r0(_anchor_frame_p);
- Translation<T, 3> t_w_r0(p_w_r0);
- Map<const Quaternion<T> > q_w_r0(_anchor_frame_o);
- TransformType T_W_R0 = t_w_r0 * q_w_r0;
-
- // world to current robot transform
- Map<const Matrix<T, 3, 1> > p_w_r1(_current_frame_p);
- Translation<T, 3> t_w_r1(p_w_r1);
- Map<const Quaternion<T> > q_w_r1(_current_frame_o);
- TransformType T_W_R1 = t_w_r1 * q_w_r1;
-
- // anchor robot to anchor camera transform
- Translation<T, 3> t_r0_c0(anchor_sensor_extrinsics_p_.cast<T>());
- Quaternion<T> q_r0_c0(anchor_sensor_extrinsics_o_.cast<T>());
- TransformType T_R0_C0 = t_r0_c0 * q_r0_c0;
-
- // current robot to current camera transform
- CaptureBasePtr current_capture = this->getFeature()->getCapture();
- Translation<T, 3> t_r1_c1 (current_capture->getSensorP()->getState().cast<T>());
- Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorO()->getState()));
- Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>();
- TransformType T_R1_C1 = t_r1_c1 * q_r1_c1;
-
- // hmg point in current camera frame C1
- Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg_c0(_lmk_hmg);
- Eigen::Matrix<T, 4, 1> landmark_hmg_c1 = T_R1_C1.inverse(Eigen::Affine)
- * T_W_R1. inverse(Eigen::Affine)
- * T_W_R0
- * T_R0_C0
- * landmark_hmg_c0;
-
- // lmk direction vector
- Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c1.head(3);
-
- // camera parameters
- Matrix<T, 4, 1> intrinsic = intrinsic_.cast<T>();
- Eigen::Matrix<T, Eigen::Dynamic, 1> distortion = distortion_.cast<T>();
-
- // project point and exit
- Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
- expectation = pinhole::projectPoint(intrinsic, distortion, v_dir);
-}
-
-template<typename T>
-inline bool FactorAHP::operator ()(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _residuals) const
-{
- // expected
- Eigen::Matrix<T, 2, 1> expected;
- expectation(_current_frame_p, _current_frame_o, _anchor_frame_p, _anchor_frame_o, _lmk_hmg, expected.data());
-
- // measured
- Eigen::Matrix<T, 2, 1> measured = getMeasurement().cast<T>();
-
- // residual
- Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals(_residuals);
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected - measured);
- return true;
-}
-
-inline FactorAHPPtr FactorAHP::create(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _lmk_ahp_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status)
-{
- // construct factor
- FactorAHPPtr fac_ahp = std::make_shared<FactorAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
-
- return fac_ahp;
-}
-
-} // namespace wolf
-
-#endif // FACTOR_AHP_H
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
deleted file mode 100644
index a3ce0e60979b6241fbf2d4f871543764430c8015..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_GPS_2D.h
+++ /dev/null
@@ -1,41 +0,0 @@
-
-#ifndef FACTOR_GPS_2D_H_
-#define FACTOR_GPS_2D_H_
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorGPS2D);
-
-class FactorGPS2D : public FactorAutodiff<FactorGPS2D, 2, 2>
-{
- public:
-
- FactorGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP())
- {
- //
- }
-
- virtual ~FactorGPS2D() = default;
-
- template<typename T>
- bool operator ()(const T* const _x, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorGPS2D::operator ()(const T* const _x, T* _residuals) const
-{
- _residuals[0] = (T(getMeasurement()(0)) - _x[0]) / T(sqrt(getMeasurementCovariance()(0, 0)));
- _residuals[1] = (T(getMeasurement()(1)) - _x[1]) / T(sqrt(getMeasurementCovariance()(1, 1)));
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_GPS_pseudorange_2D.h b/include/base/factor/factor_GPS_pseudorange_2D.h
deleted file mode 100644
index 82b186fc65e43f124bf1fd9e4f905b4d14928379..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_GPS_pseudorange_2D.h
+++ /dev/null
@@ -1,241 +0,0 @@
-#ifndef FACTOR_GPS_PSEUDORANGE_2D_H_
-#define FACTOR_GPS_PSEUDORANGE_2D_H_
-
-#define LIGHT_SPEED_ 299792458
-
-//Wolf includes
-#include "base/sensor/sensor_GPS.h"
-#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/factor/factor_autodiff.h"
-
-//std
-#include <string>
-#include <sstream>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorGPSPseudorange2D);
-
-/*
- * NB:
- * FROM THIS CLASS AND ALL THE CLASS INCLUDED, THE LIBRARY RAW_GPS_UTILS
- * MUST NOT BE REACHABLE!
- * OTHERWISE WOLF WON'T COMPILE WITHOUT INSTALLING THIS LIBRARY.
- *
- * TODO maybe is no more true
- */
-class FactorGPSPseudorange2D : public FactorAutodiff<FactorGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>
-{
- public:
- FactorGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
- _pr_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to the initial pos frame
- _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame
- _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to the vehicle frame
- // orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapP()), // map position with respect to ecef
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapO())) // map orientation with respect to ecef
- {
- sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
- pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
- //std::cout << "FactorGPSPseudorange2D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
- }
-
- virtual ~FactorGPSPseudorange2D() = default;
-
- template<typename T>
- bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
- const T* const _bias, const T* const _map_p, const T* const _map_o,
- T* _residual) const;
-
- protected:
- Eigen::Vector3s sat_position_;
- Scalar pseudorange_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
-};
-
-/*
- * naming convention for transformation matrix:
- * T_a_b is "transformation from b to a"
- * To transform a point from b to a: p_a = T_a_b * P_b
- * T_a_b also means "the pose of b expressed in frame a"
- */
-
-template<typename T>
-inline bool FactorGPSPseudorange2D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
- const T* const _sensor_p, const T* const _bias,
- const T* const _map_p, const T* const _map_o,
- T* _residual) const
-{
- int verbose_level_ = 0; // 0=nothing printed. only for debug purpose
-
- std::stringstream aux;
- aux << std::setprecision(12);
- std::cout << std::setprecision(12);
-
- if (verbose_level_ >= 2)
- {
- std::cout << "+OPERATOR()+" << id() << std::endl;
- std::cout << "_sensor_p(_base): " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl;
- std::cout << "_vehicle_p(_map): " << _vehicle_p[0] << ", " << _vehicle_p[1] << std::endl;
- std::cout << "_vehicle_o(_map): " << _vehicle_o[0] << std::endl;
- std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
- std::cout << "_map_o: " << _map_o[0] << std::endl;
- }
- //Filling Eigen vectors
- Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect base frame
-
- /*
- * Base-to-map transform matrix
- */
- Eigen::Matrix<T, 4, 4> T_map_base = Eigen::Matrix<T, 4, 4>::Identity();
- T_map_base(0, 0) = T(cos(_vehicle_o[0]));
- T_map_base(0, 1) = T(-sin(_vehicle_o[0]));
- T_map_base(1, 0) = T(sin(_vehicle_o[0]));
- T_map_base(1, 1) = T(cos(_vehicle_o[0]));
- T_map_base(0, 3) = T(_vehicle_p[0]);
- T_map_base(1, 3) = T(_vehicle_p[1]);
-
- // sensor position with respect to map frame
- Eigen::Matrix<T, 4, 1> sensor_p_map = T_map_base * sensor_p_base;
-
- if (verbose_level_ >= 2)
- {
- aux.str(std::string());
- aux << sensor_p_map(0);
- if (aux.str().substr(0, 1) != "[")
- std::cout << "!!! sensor_p_map: " << sensor_p_map[0] << ", " << sensor_p_map[1] << ", " << sensor_p_map[2] << std::endl;
- else {
- std::cout << "!!! sensor_p_map: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_map(1);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_map(2);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1) << std::endl;
- aux.str(std::string());
- }
- }
-
- /*
- * _map_p from ECEF to LLA (math from https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf )
- */
- // WGS84 ellipsoid constants
- T a = T(6378137); // earth's radius
- T e = T(8.1819190842622e-2); // eccentricity
- T asq = a * a;
- T esq = e * e;
- T b = T(sqrt(asq * (T(1) - esq)));
- T bsq = T(b * b);
- T ep = T(sqrt((asq - bsq) / bsq));
- T p = T(sqrt(_map_p[0] * _map_p[0] + _map_p[1] * _map_p[1]));
- T th = T(atan2(a * _map_p[2], b * p));
- T lon = T(atan2(_map_p[1], _map_p[0]));
- T lat = T(atan2((_map_p[2] + ep * ep * b * pow(sin(th), 3)), (p - esq * a * pow(cos(th), 3))));
-
- if (verbose_level_ >= 3)
- {
- std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
- std::cout << "_map_p LLA: " << lat << ", " << lon << std::endl;
- std::cout << "_map_p LLA degrees: " << lat * T(180 / M_PI) << ", " << lon * T(180 / M_PI) << std::endl;
- }
-
- /*
- * map-to-ECEF transform matrix
- * made by the product of the next 4 matrixes
- */
- Eigen::Matrix<T, 4, 4> T_ecef_aux = Eigen::Matrix<T, 4, 4>::Identity();
- T_ecef_aux(0, 3) = T(_map_p[0]);
- T_ecef_aux(1, 3) = T(_map_p[1]);
- T_ecef_aux(2, 3) = T(_map_p[2]);
-
- Eigen::Matrix<T, 4, 4> T_aux_lon = Eigen::Matrix<T, 4, 4>::Identity();
- T_aux_lon(0, 0) = T(cos(lon));
- T_aux_lon(0, 1) = T(-sin(lon));
- T_aux_lon(1, 0) = T(sin(lon));
- T_aux_lon(1, 1) = T(cos(lon));
-
- Eigen::Matrix<T, 4, 4> T_lon_lat = Eigen::Matrix<T, 4, 4>::Identity();
- T_lon_lat(0, 0) = T(cos(lat));
- T_lon_lat(0, 2) = T(-sin(lat));
- T_lon_lat(2, 0) = T(sin(lat));
- T_lon_lat(2, 2) = T(cos(lat));
-
- Eigen::Matrix<T, 4, 4> T_lat_enu = Eigen::Matrix<T, 4, 4>::Zero();
- T_lat_enu(0, 2) = T_lat_enu(1, 0) = T_lat_enu(2, 1) = T_lat_enu(3, 3) = T(1);
-
- Eigen::Matrix<T, 4, 4> T_enu_map = Eigen::Matrix<T, 4, 4>::Identity();
- T_enu_map(0, 0) = T(cos(_map_o[0]));
- T_enu_map(0, 1) = T(-sin(_map_o[0]));
- T_enu_map(1, 0) = T(sin(_map_o[0]));
- T_enu_map(1, 1) = T(cos(_map_o[0]));
-
- Eigen::Matrix<T, 4, 4> T_ecef_map = T_ecef_aux * T_aux_lon * T_lon_lat * T_lat_enu * T_enu_map;
-
- //sensor position with respect to ecef coordinate system
- Eigen::Matrix<T, 4, 1> sensor_p_ecef = T_ecef_map * sensor_p_map;
-
- /*
- * calculate the residual
- */
- T square_sum = T(0);
- for (int i = 0; i < 3; ++i)
- square_sum += (sensor_p_ecef[i] - T(sat_position_[i]))*(sensor_p_ecef[i] - T(sat_position_[i]));
-
- T distance = (square_sum != T(0)) ? sqrt(square_sum) : T(0);
-
- // error = (expected measurement) - (actual measurement)
- _residual[0] = (distance + _bias[0] * T(LIGHT_SPEED_)) - T(pseudorange_);
-
- if (verbose_level_ >= 2)
- std::cout << "!!! Residual: " << _residual[0] << "\n";
-
- // normalizing by the covariance
- _residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0)));
-
- if (verbose_level_ >= 1)
- {
- aux.str(std::string());
- aux << sensor_p_ecef(0);
- if (aux.str().substr(0, 1) != "[")
- std::cout << "!!! sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << sensor_p_ecef[2];
- else {
- std::cout << "!!! sensor_p_ecef: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_ecef(1);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_ecef(2);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1);
- }
-
- //std::cout << "Expected: " << (distance + _bias[0]*T(LIGHT_SPEED)) << "\nreceived = " << pseudorange_ << "\n";
- aux.str(std::string());
- aux << _residual[0];
- if (aux.str().substr(0,1) != "[" )
- std::cout << "\t Residual norm: " << _residual[0] << "\n";
- else
- {
- std::cout << "\t Residual norm: " << aux.str().substr(1, aux.str().find(" ") - 1) << "\n";
- }
- }
-
- return true;
-}
-
-} // namespace wolf
-
-#endif //FACTOR_GPS_PSEUDORANGE_2D_H_
diff --git a/include/base/factor/factor_GPS_pseudorange_3D.h b/include/base/factor/factor_GPS_pseudorange_3D.h
deleted file mode 100644
index 72e44d4f62f55358e33cf2ebb14254ca532f1bd3..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_GPS_pseudorange_3D.h
+++ /dev/null
@@ -1,141 +0,0 @@
-#ifndef FACTOR_GPS_PSEUDORANGE_3D_H_
-#define FACTOR_GPS_PSEUDORANGE_3D_H_
-
-#define LIGHT_SPEED 299792458
-
-//Wolf includes
-#include "base/sensor/sensor_GPS.h"
-#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/factor/factor_autodiff.h"
-
-namespace wolf {
-
-// Set ClassPtr, ClassConstPtr and ClassWPtr typedefs;
-WOLF_PTR_TYPEDEFS(FactorGPSPseudorange3D);
-
-/*
- * NB:
- * FROM THIS CLASS AND ALL THE CLASS INCLUDED, THE LIBRARY RAW_GPS_UTILS
- * MUST NOT BE REACHABLE!
- * OTHERWISE WOLF WON'T COMPILE WITHOUT INSTALLING THIS LIBRARY.
- *
- * TODO maybe is no more true
- */
-class FactorGPSPseudorange3D: public FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>
-{
-
- public:
-
- FactorGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
- nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
- _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to map frame
- _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame wrt map frame
- _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to base frame
- // orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapP(), // initial vehicle position wrt ecef frame
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapO()) // initial vehicle orientation wrt ecef frame
- {
- sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
- pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
-
- //std::cout << "FactorGPSPseudorange3D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
- }
-
- virtual ~FactorGPSPseudorange3D() = default;
-
- template<typename T>
- bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
- const T* const _bias, const T* const _init_vehicle_p, const T* const _init_vehicle_o,
- T* _residual) const;
-
- protected:
- Eigen::Vector3s sat_position_;
- Scalar pseudorange_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- static FactorBasePtr create(FeatureBasePtr _feature_ptr, //
- NodeBasePtr _correspondant_ptr = nullptr)
- {
- return std::make_shared<FactorGPSPseudorange3D>(_feature_ptr);
- }
-
-};
-
-/*
- * naming convention for transformation matrix:
- * T_a_b is "transformation from b to a"
- * To transform a point from b to a: p_a = T_a_b * P_b
- * T_a_b also means "the pose of b expressed in frame a"
- */
-
-template<typename T>
-inline bool FactorGPSPseudorange3D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
- const T* const _sensor_p, const T* const _bias,
- const T* const _map_p, const T* const _map_o,
- T* _residual) const
-{
-// std::cout << "OPERATOR()\n";
-// std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
-// std::cout << "_map_o: " << _map_o[0] << ", " << _map_o[1] << ", " << _map_o[2] << ", " << _map_o[3] << std::endl;
-// std::cout << "_vehicle_p: " << _vehicle_p[0] << ", " << _vehicle_p[1] << ", " << _vehicle_p[2] << std::endl;
-// std::cout << "_vehicle_o: " << _vehicle_o[0] << ", " << _vehicle_o[1] << ", " << _vehicle_o[2] << ", " << _vehicle_o[3] << std::endl;
-// std::cout << "_sensor_p: " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl;
- Eigen::Matrix<T, 4, 1> sensor_p_ecef; //sensor position with respect to ecef coordinate system
- Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect to the base (the vehicle)
-
- /*
- * map-to-ECEF transformation matrix: To transform a point from map to ecef
- */
- Eigen::Matrix<T, 4, 4> T_ecef_map = Eigen::Matrix<T, 4, 4>::Identity();
- Eigen::Quaternion<T> map_o_quat(_map_o[0], _map_o[1], _map_o[2], _map_o[3]);
- Eigen::Matrix<T, 3, 3> rot_matr_init = map_o_quat.toRotationMatrix();
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- T_ecef_map(i, j) = rot_matr_init(i, j);
- for (int i = 0; i < 3; ++i)
- T_ecef_map(i, 3) = _map_p[i];
-
- /*
- * Base-to-map transformation matrix: To transform a point from base to map
- */
- Eigen::Matrix<T, 4, 4> T_map_base = Eigen::Matrix<T, 4, 4>::Identity();
- Eigen::Quaternion<T> vehicle_o_quat(_vehicle_o[0], _vehicle_o[1], _vehicle_o[2], _vehicle_o[3]);
- Eigen::Matrix<T, 3, 3> rot_matr_vehicle = vehicle_o_quat.toRotationMatrix();
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- T_map_base(i, j) = rot_matr_vehicle(i, j);
- for (int i = 0; i < 3; ++i)
- T_map_base(i, 3) = _vehicle_p[i];
-
- /*
- * Compute sensor_p wrt ECEF
- */
- sensor_p_ecef = T_ecef_map * T_map_base * sensor_p_base;
- //std::cout << "sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << sensor_p_ecef[2] << std::endl;
-
- /*
- * Compute residual
- */
- T square_sum = T(0);
- for (int i = 0; i < 3; ++i)
- square_sum += pow(sensor_p_ecef[i] - T(sat_position_[i]), 2);
-
- T distance = (square_sum != T(0)) ? sqrt(square_sum) : T(0);
- // error = (expected measurement) - (actual measurement)
- _residual[0] = (distance + _bias[0] * T(LIGHT_SPEED)) - (pseudorange_);
-
- // Normalize by covariance
- _residual[0] = _residual[0] / T(sqrt(getMeasurementCovariance()(0, 0)));
-
- return true;
-}
-
-} // namespace wolf
-
-#endif //FACTOR_GPS_PSEUDORANGE_3D_H_
diff --git a/include/base/factor/factor_IMU.h b/include/base/factor/factor_IMU.h
deleted file mode 100644
index fbb29d1642e91d9fe90f55da673f3b8a87b6c335..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_IMU.h
+++ /dev/null
@@ -1,498 +0,0 @@
-#ifndef FACTOR_IMU_THETA_H_
-#define FACTOR_IMU_THETA_H_
-
-//Wolf includes
-#include "base/feature/feature_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/rotations.h"
-
-//Eigen include
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorIMU);
-
-//class
-class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
-{
- public:
- FactorIMU(const FeatureIMUPtr& _ftr_ptr,
- const CaptureIMUPtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorIMU() = default;
-
- /* \brief : compute the residual from the state blocks being iterated by the solver.
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- */
- template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _o2,
- const T* const _v2,
- const T* const _b2,
- T* _res) const;
-
- /* \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- * params :
- * Vector3s _p1 : position in imu frame
- * Vector4s _q1 : orientation quaternion in imu frame
- * Vector3s _v1 : velocity in imu frame
- * Vector3s _ab : accelerometer bias in imu frame
- * Vector3s _wb : gyroscope bias in imu frame
- * Vector3s _p2 : position in current frame
- * Vector4s _q2 : orientation quaternion in current frame
- * Vector3s _v2 : velocity in current frame
- * Matrix<9,1, Scalar> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
- */
- template<typename D1, typename D2, typename D3>
- bool residual(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
- Eigen::MatrixBase<D3> & _res) const;
-
- /* Function expectation(...)
- * params :
- * Vector3s _p1 : position in imu frame
- * Vector4s _q1 : orientation quaternion in imu frame
- * Vector3s _v1 : velocity in imu frame
- * Vector3s _p2 : position in current frame
- * Vector4s _q2 : orientation quaternion in current frame
- * Vector3s _v2 : velocity in current frame
- * Scalar _dt : time interval between the two states
- * _pe : expected position delta
- * _qe : expected quaternion delta
- * _ve : expected velocity delta
- */
- template<typename D1, typename D2, typename D3, typename D4>
- void expectation(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const;
-
- /* \brief : return the expected delta given the state blocks in the wolf tree
- */
- Eigen::VectorXs expectation() const;
-
- private:
- /// Preintegrated delta
- Eigen::Vector3s dp_preint_;
- Eigen::Quaternions dq_preint_;
- Eigen::Vector3s dv_preint_;
-
- // Biases used during preintegration
- Eigen::Vector3s acc_bias_preint_;
- Eigen::Vector3s gyro_bias_preint_;
-
- // Jacobians of preintegrated deltas wrt biases
- Eigen::Matrix3s dDp_dab_;
- Eigen::Matrix3s dDv_dab_;
- Eigen::Matrix3s dDp_dwb_;
- Eigen::Matrix3s dDv_dwb_;
- Eigen::Matrix3s dDq_dwb_;
-
- /// Metrics
- const Scalar dt_; ///< delta-time and delta-time-squared between keyframes
- const Scalar ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
-
- /* bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
- const Eigen::Matrix3s sqrt_A_r_dt_inv;
- const Eigen::Matrix3s sqrt_W_r_dt_inv;
-
-};
-
-///////////////////// IMPLEMENTAITON ////////////////////////////
-
-inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
- const CaptureIMUPtr& _cap_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
- "IMU",
- _cap_origin_ptr->getFrame(),
- _cap_origin_ptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _cap_origin_ptr->getFrame()->getP(),
- _cap_origin_ptr->getFrame()->getO(),
- _cap_origin_ptr->getFrame()->getV(),
- _cap_origin_ptr->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
- dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
- dq_preint_(_ftr_ptr->getMeasurement().data()+3),
- dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
- acc_bias_preint_(_ftr_ptr->getAccBiasPreint()), // state biases at preintegration time
- gyro_bias_preint_(_ftr_ptr->getGyroBiasPreint()),
- dDp_dab_(_ftr_ptr->getJacobianBias().block(0,0,3,3)), // Jacs of dp dv dq wrt biases
- dDv_dab_(_ftr_ptr->getJacobianBias().block(6,0,3,3)),
- dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
- dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
- dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
- wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
- sqrt_A_r_dt_inv((Eigen::Matrix3s::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
- sqrt_W_r_dt_inv((Eigen::Matrix3s::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
-{
- //
-}
-
-template<typename T>
-inline bool FactorIMU::operator ()(const T* const _p1,
- const T* const _q1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _q2,
- const T* const _v2,
- const T* const _b2,
- T* _res) const
-{
- using namespace Eigen;
-
- // MAPS
- Map<const Matrix<T,3,1> > p1(_p1);
- Map<const Quaternion<T> > q1(_q1);
- Map<const Matrix<T,3,1> > v1(_v1);
- Map<const Matrix<T,3,1> > ab1(_b1);
- Map<const Matrix<T,3,1> > wb1(_b1 + 3);
-
- Map<const Matrix<T,3,1> > p2(_p2);
- Map<const Quaternion<T> > q2(_q2);
- Map<const Matrix<T,3,1> > v2(_v2);
- Map<const Matrix<T,3,1> > ab2(_b2);
- Map<const Matrix<T,3,1> > wb2(_b2 + 3);
-
- Map<Matrix<T,15,1> > res(_res);
-
- residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, res);
-
- return true;
-}
-
-template<typename D1, typename D2, typename D3>
-inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
- Eigen::MatrixBase<D3> & _res) const
-{
-
- /* Help for the IMU residual function
- *
- * Notations:
- * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv]
- * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv]
- * b* : a bias
- * J* : a Jacobian matrix
- *
- * We use the following functions from imu_tools.h:
- * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
- * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T
- * T = diff(D1,D2) : minus operator, T = D2 (-) D1
- *
- * Two methods are possible (select with #define below this note) :
- *
- * Computations common to methods 1 and 2:
- * D_exp = betweenStates(x1,x2,dt) // Predict delta from states
- * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
- *
- * Method 1:
- * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change
- * T_err = diff(D_exp, D_corr) // compare against expected delta
- * res = W.sqrt * T_err
- *
- * results in:
- * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
- *
- * Method 2:
- * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta
- * T_err = T_diff - T_step // the difference should match the correction step due to bias change
- * res = W.sqrt * err
- *
- * results in :
- * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
- *
- * NOTE: See optimization report at the end of this file for comparisons of both methods.
- */
-#define METHOD_1 // if commented, then METHOD_2 will be applied
-
- //needed typedefs
- typedef typename D1::Scalar T;
-
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 15);
-
- // 1. Expected delta from states
- Eigen::Matrix<T, 3, 1 > dp_exp;
- Eigen::Quaternion<T> dq_exp;
- Eigen::Matrix<T, 3, 1> dv_exp;
-
- imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
-
- // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
-
- // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
- Eigen::Matrix<T, 9, 1> d_step;
-
- d_step.block(0,0,3,1) = dDp_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDp_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(3,0,3,1) = dDq_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(6,0,3,1) = dDv_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDv_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
-
-#ifdef METHOD_1 // method 1
- Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1);
- Eigen::Matrix<T, 3, 1> do_step = d_step.block(3,0,3,1);
- Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6,0,3,1);
-
- // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
- Eigen::Matrix<T,3,1> dp_correct;
- Eigen::Quaternion<T> dq_correct;
- Eigen::Matrix<T,3,1> dv_correct;
-
- imu::plus(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
- dp_step, do_step, dv_step,
- dp_correct, dq_correct, dv_correct);
-
- // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
- // Note the Dt here is zero because it's the delta-time between the same time stamps!
- Eigen::Matrix<T, 9, 1> d_error;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);
-
- Eigen::Matrix<T, 3, 3> J_do_dq1, J_do_dq2;
- Eigen::Matrix<T, 9, 9> J_err_corr;
-
-//#define WITH_JAC
-#ifdef WITH_JAC
- imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error, J_do_dq1, J_do_dq2);
-
- J_err_corr.setIdentity();
- J_err_corr.block(3,3,3,3) = J_do_dq2;
-
- // 4. Residuals are the weighted errors
- _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
-#else
- imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error);
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
-#endif
-
-#else // method 2
-
- Eigen::Matrix<T, 9, 1> d_diff;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_diff(d_diff.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_diff(d_diff.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_diff(d_diff.data() + 6);
-
- imu::diff(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
- dp_exp, dq_exp, dv_exp,
- dp_diff, do_diff, dv_diff);
-
- Eigen::Matrix<T, 9, 1> d_error;
- d_error << d_diff - d_step;
-
- // 4. Residuals are the weighted errors
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
-
-#endif
-
- // Errors between biases
- Eigen::Matrix<T,3,1> ab_error(_ab1 - _ab2); // KF1.bias - KF2.bias
- Eigen::Matrix<T,3,1> wb_error(_wb1 - _wb2);
-
- // 4. Residuals are the weighted errors
- _res.segment(9,3) = sqrt_A_r_dt_inv.cast<T>() * ab_error;
- _res.tail(3) = sqrt_W_r_dt_inv.cast<T>() * wb_error;
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////// PRINT VALUES ///////////////////////////////////
-#if 0
- // print values -----------------------
- Matrix<T, 10, 1> x1; x1 << _p1 , _q1.coeffs(), _v1;
- Matrix<T, 10, 1> x2; x2 << _p2 , _q2.coeffs(), _v2;
- print("x1 ", x1);
- print("x2 ", x2);
- Matrix<T, 6, 1> bp; bp << acc_bias_preint_.cast<T>(), gyro_bias_preint_.cast<T>();
- Matrix<T, 6, 1> b1; b1 << _ab1, _wb1;
- Matrix<T, 6, 1> b2; b2 << _ab2, _wb2;
- print("bp ", bp);
- print("b1 ", b1);
- print("b2 ", b2);
- Matrix<T, 10, 1> exp; exp << dp_exp , dq_exp.coeffs(), dv_exp;
- print("D exp", exp);
- Matrix<T, 10, 1> pre; pre << dp_preint_.cast<T>() , dq_preint_.cast<T>().coeffs(), dv_preint_.cast<T>();
- print("D preint", pre);
- Matrix<T, 9, 6> J_b_r0; J_b_r0.setZero();
- J_b_r0.block(0,0,3,3) = dDp_dab_.cast<T>();
- J_b_r0.block(0,3,3,3) = dDp_dwb_.cast<T>();
- J_b_r0.block(3,3,3,3) = dDq_dwb_.cast<T>();
- J_b_r0.block(6,0,3,3) = dDv_dab_.cast<T>();
- J_b_r0.block(6,3,3,3) = dDv_dwb_.cast<T>();
- print("J bias", J_b_r0);
- print("D step", d_step);
-#ifndef METHOD_1
- Matrix<T, 9, 1> dif; dif << dp_diff , do_diff, dv_diff;
- print("D diff", dif);
-#endif
- print("D err", d_error);
- WOLF_TRACE("-----------------------------------------")
-#endif
- /////////////////////////////////////////////////////////////////////////////////////////////
-
- return true;
-}
-
-inline Eigen::VectorXs FactorIMU::expectation() const
-{
- FrameBasePtr frm_current = getFeature()->getFrame();
- FrameBasePtr frm_previous = getFrameOther();
-
- //get information on current_frame in the FactorIMU
- const Eigen::Vector3s frame_current_pos = (frm_current->getP()->getState());
- const Eigen::Quaternions frame_current_ori (frm_current->getO()->getState().data());
- const Eigen::Vector3s frame_current_vel = (frm_current->getV()->getState());
-
- // get info on previous frame in the FactorIMU
- const Eigen::Vector3s frame_previous_pos = (frm_previous->getP()->getState());
- const Eigen::Quaternions frame_previous_ori (frm_previous->getO()->getState().data());
- const Eigen::Vector3s frame_previous_vel = (frm_previous->getV()->getState());
-
- // Define results vector and Map bits over it
- Eigen::Matrix<Scalar, 10, 1> exp;
- Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > pe(exp.data() );
- Eigen::Map<Eigen::Quaternion<Scalar> > qe(exp.data() + 3);
- Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > ve(exp.data() + 7);
-
- expectation(frame_previous_pos, frame_previous_ori, frame_previous_vel,
- frame_current_pos, frame_current_ori, frame_current_vel,
- dt_,
- pe, qe, ve);
-
- return exp;
-}
-
-template<typename D1, typename D2, typename D3, typename D4>
-inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const
-{
- imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve);
-}
-
-} // namespace wolf
-
-#endif
-
-/*
- * Optimization results
- * ================================================
- *
- * Using gtest_IMU.cpp
- *
- * Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations.
- *
- * You can verify this by looking at the 'Iterations' and 'Final costs' in the Ceres reports below.
- *
- * With Method 1:
- *
-[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
-[trace][10:58:16] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][10:58:16] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
-[----------] 3 tests from Process_Factor_IMU (159 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][10:58:16] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][10:58:16] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (120 ms total)
-*
-* With Method 2:
-*
-[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
-[trace][11:15:43] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][11:15:43] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
-[----------] 3 tests from Process_Factor_IMU (133 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][11:15:43] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][11:15:43] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (127 ms total)
-*
-*/
diff --git a/include/base/factor/factor_analytic.h b/include/base/factor/factor_analytic.h
deleted file mode 100644
index 14604bdb21ff2d0ed8d13b0d5fcf55055a637bf7..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_analytic.h
+++ /dev/null
@@ -1,166 +0,0 @@
-
-#ifndef FACTOR_ANALYTIC_H_
-#define FACTOR_ANALYTIC_H_
-
-//Wolf includes
-#include "base/factor/factor_base.h"
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorAnalytic);
-
-class FactorAnalytic: public FactorBase
-{
- protected:
- std::vector<StateBlockPtr> state_ptr_vector_;
- std::vector<unsigned int> state_block_sizes_vector_;
-
- public:
-
- /** \brief Constructor of category FAC_ABSOLUTE
- *
- * Constructor of category FAC_ABSOLUTE
- *
- **/
- FactorAnalytic(const std::string& _tp,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr = nullptr,
- StateBlockPtr _state2Ptr = nullptr,
- StateBlockPtr _state3Ptr = nullptr,
- StateBlockPtr _state4Ptr = nullptr,
- StateBlockPtr _state5Ptr = nullptr,
- StateBlockPtr _state6Ptr = nullptr,
- StateBlockPtr _state7Ptr = nullptr,
- StateBlockPtr _state8Ptr = nullptr,
- StateBlockPtr _state9Ptr = nullptr ) ;
-
- FactorAnalytic(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr = nullptr,
- StateBlockPtr _state2Ptr = nullptr,
- StateBlockPtr _state3Ptr = nullptr,
- StateBlockPtr _state4Ptr = nullptr,
- StateBlockPtr _state5Ptr = nullptr,
- StateBlockPtr _state6Ptr = nullptr,
- StateBlockPtr _state7Ptr = nullptr,
- StateBlockPtr _state8Ptr = nullptr,
- StateBlockPtr _state9Ptr = nullptr );
-
- virtual ~FactorAnalytic() = default;
-
- /** \brief Returns a vector of pointers to the states
- *
- * Returns a vector of pointers to the state in which this factor depends
- *
- **/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
-
- /** \brief Returns a vector of sizes of the state blocks
- *
- * Returns a vector of sizes of the state blocks
- *
- **/
- virtual std::vector<unsigned int> getStateSizes() const override;
-
- /** \brief Returns the factor residual size
- *
- * Returns the factor residual size
- *
- **/
-// virtual unsigned int getSize() const = 0;
-
- /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
- **/
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override
- {
- // load parameters evaluation value
- std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
- for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
- state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>((Scalar*)parameters[i], state_block_sizes_vector_[i]));
-
- // residuals
- Eigen::Map<Eigen::VectorXs> residuals_map((Scalar*)residuals, getSize());
- residuals_map = evaluateResiduals(state_blocks_map_);
-
- // also compute jacobians
- if (jacobians != nullptr)
- {
- std::vector<Eigen::Map<Eigen::MatrixXs>> jacobians_map_;
- std::vector<bool> compute_jacobians_(state_block_sizes_vector_.size());
-
- for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
- {
- compute_jacobians_[i] = (jacobians[i] != nullptr);
- if (jacobians[i] != nullptr)
- jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>((Scalar*)jacobians[i], getSize(), state_block_sizes_vector_[i]));
- else
- jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(nullptr, 0, 0)); //TODO: check if it can be done
- }
-
- // evaluate jacobians
- evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
- }
- return true;
-
- return true;
- };
-
- /** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
- **/
- // TODO
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override
- {
- };
-
- /** \brief Returns the residual evaluated in the states provided
- *
- * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
- *
- **/
- virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const = 0;
-
- /** \brief Returns the normalized jacobians evaluated in the states
- *
- * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
- * 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
- *
- **/
- virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXs>>& jacobians, const std::vector<bool>& _compute_jacobian) const = 0;
-
- /** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
- *
- * Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
- *
- * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
- *
- **/
- virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const = 0;
-
- /** \brief Returns the jacobians computation method
- *
- * Returns the jacobians computation method
- *
- **/
- virtual JacobianMethod getJacobianMethod() const override;
-
- private:
- void resizeVectors();
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_autodiff.h b/include/base/factor/factor_autodiff.h
deleted file mode 100644
index be35c772aacb9c7fce4e780d413fa0df675c1655..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_autodiff.h
+++ /dev/null
@@ -1,2275 +0,0 @@
-
-#ifndef FACTOR_AUTODIFF_H_
-#define FACTOR_AUTODIFF_H_
-
-//Wolf includes
-#include "base/factor/factor_base.h"
-#include "base/state_block.h"
-
-// CERES
-#include "ceres/jet.h"
-
-// GENERAL
-#include <array>
-
-namespace wolf {
-
-//template class FactorAutodiff
-template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
-class FactorAutodiff : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int n_blocks = 10;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
- std::array<WolfJet, B7>* jets_7_;
- std::array<WolfJet, B8>* jets_8_;
- std::array<WolfJet, B9>* jets_9_;
-
- public:
- /** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
- **/
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>),
- jets_7_(new std::array<WolfJet, B7>),
- jets_8_(new std::array<WolfJet, B8>),
- jets_9_(new std::array<WolfJet, B9>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete jets_8_;
- delete jets_9_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- /** \brief Returns the residual and jacobians given the state values
- *
- * Returns the residual and jacobians given the state values
- *
- **/
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- parameters[9],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- jets_9_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- /** \brief Updates all jets real part with values of parameters
- *
- **/
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
- for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i].a = parameters[9][i];
- }
-
- /** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
- *
- **/
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- jets_9_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- /** \brief Returns a vector of pointers to the states
- *
- * Returns a vector of pointers to the state in which this factor depends
- *
- **/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- /** \brief Returns a vector of the states sizes
- *
- **/
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- /** \brief Returns the residual size
- *
- * Returns the residual size
- *
- **/
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 9;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
- std::array<WolfJet, B7>* jets_7_;
- std::array<WolfJet, B8>* jets_8_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>),
- jets_7_(new std::array<WolfJet, B7>),
- jets_8_(new std::array<WolfJet, B8>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete jets_8_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 8;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
- std::array<WolfJet, B7>* jets_7_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>),
- jets_7_(new std::array<WolfJet, B7>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 7;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
- B5 + B6> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 6;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
- B5> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 5;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 4;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 3;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1 + B2> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 2;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0 + B1> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete jets_1_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0>
-class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = 0;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int n_blocks = 1;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<Scalar, B0> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>)
- {
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- virtual ~FactorAutodiff()
- {
- delete jets_0_;
- delete residuals_jets_;
- }
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<Scalar const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
- }
-
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // init jacobian
- for(unsigned int i = 0; i < n_blocks; ++i)
- {
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
- jacobians_.push_back(Ji);
- }
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- residuals_jets_->data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
- {
- return state_ptrs_;
- }
-
- virtual std::vector<unsigned int> getStateSizes() const
- {
- return state_block_sizes_;
- }
-
- virtual unsigned int getSize() const
- {
- return RES;
- }
-};
-
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-// STATIC CONST VECTORS INITIALIZATION
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-// state_block_sizes_
-// 10 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
-// 9 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
-// 8 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
-// 7 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
-// 6 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
-// 5 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
-// 4 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
-// 3 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
-// 2 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
-// 1 BLOCK
-template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
-
-// jacobian_locations_
-// 10 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8};
-// 9 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7};
-// 8 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6};
-// 7 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5};
-// 6 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4};
-// 5 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3};
-// 4 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2};
-// 3 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1};
-// 2 BLOCKS
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0};
-// 1 BLOCK
-template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
deleted file mode 100644
index a2d74f77361d5de2a5e6fc495ab128981b5824e8..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_autodiff_distance_3D.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/**
- * \file factor_autodiff_distance_3D.h
- *
- * Created on: Apr 10, 2018
- * \author: jsola
- */
-
-#ifndef FACTOR_AUTODIFF_DISTANCE_3D_H_
-#define FACTOR_AUTODIFF_DISTANCE_3D_H_
-
-#include "base/factor/factor_autodiff.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(FactorAutodiffDistance3D);
-
-class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D, 1, 3, 3>
-{
- public:
- FactorAutodiffDistance3D(const FeatureBasePtr& _feat,
- const FrameBasePtr& _frm_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function=false,
- FactorStatus _status=FAC_ACTIVE) :
- FactorAutodiff("DISTANCE 3D",
- _frm_other,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFrame()->getP(),
- _frm_other->getP())
- {
- setFeature(_feat);
- }
-
- virtual ~FactorAutodiffDistance3D() { /* nothing */ }
-
- template<typename T>
- bool operator () (const T* const _pos1,
- const T* const _pos2,
- T* _residual) const
- {
- using namespace Eigen;
-
- Map<const Matrix<T,3,1>> pos1(_pos1);
- Map<const Matrix<T,3,1>> pos2(_pos2);
- Map<Matrix<T,1,1>> res(_residual);
-
- Matrix<T,1,1> dist_exp ( sqrt( ( pos2 - pos1 ).squaredNorm() ) );
- Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
- Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
-
- res = sqrt_info_upper * (dist_meas - dist_exp);
-
- return true;
- }
-};
-
-} /* namespace wolf */
-
-#endif /* FACTOR_AUTODIFF_DISTANCE_3D_H_ */
diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h
deleted file mode 100644
index 4562d5f75b39995fc6578b8498fc5ee95e223e61..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_base.h
+++ /dev/null
@@ -1,243 +0,0 @@
-#ifndef FACTOR_BASE_H_
-#define FACTOR_BASE_H_
-
-// Forward declarations for node templates
-namespace wolf{
-class FeatureBase;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-
-//std includes
-
-namespace wolf {
-
-/** \brief Enumeration of factor status
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- */
-typedef enum
-{
- FAC_INACTIVE = 0, ///< Factor established with a frame (odometry).
- FAC_ACTIVE = 1 ///< Factor established with absolute reference.
-} FactorStatus;
-
-/** \brief Enumeration of jacobian computation method
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- */
-typedef enum
-{
- JAC_AUTO = 1, ///< Auto differentiation (AutoDiffCostFunctionWrapper or ceres::NumericDiffCostFunction).
- JAC_NUMERIC, ///< Numeric differentiation (ceres::NumericDiffCostFunction).
- JAC_ANALYTIC ///< Analytic jacobians.
-} JacobianMethod;
-
-//class FactorBase
-class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBase>
-{
- private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
-
- static unsigned int factor_id_count_;
-
- protected:
- unsigned int factor_id_;
- FactorStatus status_; ///< status of factor (types defined at wolf.h)
- bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer (for category FAC_FRAME)
- CaptureBaseWPtr capture_other_ptr_; ///< CaptureBase pointer
- FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer (for category FAC_FEATURE)
- LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer (for category FAC_LANDMARK)
- ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
-
- public:
-
- /** \brief Constructor of category FAC_ABSOLUTE
- **/
- FactorBase(const std::string& _tp,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- /** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
- **/
- FactorBase(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorBase() = default;
-
- virtual void remove();
-
- unsigned int id() const;
-
- /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
- **/
- virtual bool evaluate(Scalar const* const* _parameters, Scalar* _residuals, Scalar** _jacobians) const = 0;
-
- /** Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr and the residual vector
- **/
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& _residual, std::vector<Eigen::MatrixXs>& _jacobians) const = 0;
-
- /** \brief Returns the jacobians computation method
- **/
- virtual JacobianMethod getJacobianMethod() const = 0;
-
- /** \brief Returns a vector of pointers to the states in which this factor depends
- **/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
-
- /** \brief Returns a vector of the states sizes
- **/
- virtual std::vector<unsigned int> getStateSizes() const = 0;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXs& getMeasurement() const;
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const;
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
-
- /** \brief Returns a pointer to the feature constrained from
- **/
- FeatureBasePtr getFeature() const;
- void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
-
- /** \brief Returns a pointer to its capture
- **/
- CaptureBasePtr getCapture() const;
-
- /** \brief Returns the factor residual size
- **/
- virtual unsigned int getSize() const = 0;
-
- /** \brief Gets the status
- */
- FactorStatus getStatus() const;
-
- /** \brief Sets the status
- */
- void setStatus(FactorStatus _status);
-
- /** \brief Gets the apply loss function flag
- */
- bool getApplyLossFunction();
-
- /** \brief Gets the apply loss function flag
- */
- void setApplyLossFunction(const bool _apply);
-
- /** \brief Returns a pointer to the frame constrained to
- **/
- FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
- void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
-
- /** \brief Returns a pointer to the frame constrained to
- **/
- CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
- void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
-
- /** \brief Returns a pointer to the feature constrained to
- **/
- FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
- void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
-
- /** \brief Returns a pointer to the landmark constrained to
- **/
- LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
- void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
-
- /**
- * @brief getProcessor
- * @return
- */
- ProcessorBasePtr getProcessor() const;
-
- /**
- * @brief setProcessor
- * @param _processor_ptr
- */
- void setProcessor(const ProcessorBasePtr& _processor_ptr);
-
-// protected:
-// template<typename D>
-// void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
-// template<int R, int C>
-// void print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const; // Normal print if Scalar type is wolf::Scalar
-};
-
-}
-
-// IMPLEMENTATION //
-
-#include "base/problem.h"
-#include "base/frame_base.h"
-#include "base/feature/feature_base.h"
-#include "base/sensor/sensor_base.h"
-#include "base/landmark/landmark_base.h"
-
-namespace wolf{
-
-//template<typename D>
-//inline void FactorBase::print(const std::string& name, const Eigen::MatrixBase<D>& mat) const {} // Do nothing if input Scalar type is ceres::Jet
-//template<int R, int C>
-//inline void FactorBase::print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const // Normal print if Scalar type is wolf::Scalar
-//{
-// if (mat.cols() == 1)
-// {
-// WOLF_TRACE(name, ": ", mat.transpose());
-// }
-// else if (mat.rows() == 1)
-// {
-// WOLF_TRACE(name, ": ", mat);
-// }
-// else
-// {
-// WOLF_TRACE(name, ":\n", mat);
-// }
-//}
-
-inline unsigned int FactorBase::id() const
-{
- return factor_id_;
-}
-
-inline FeatureBasePtr FactorBase::getFeature() const
-{
- return feature_ptr_.lock();
-}
-
-inline FactorStatus FactorBase::getStatus() const
-{
- return status_;
-}
-
-inline bool FactorBase::getApplyLossFunction()
-{
- return apply_loss_function_;
-}
-
-inline ProcessorBasePtr FactorBase::getProcessor() const
-{
- return processor_ptr_.lock();
-}
-
-inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
-{
- processor_ptr_ = _processor_ptr;
-}
-
-} // namespace wolf
-#endif
diff --git a/include/base/factor/factor_block_absolute.h b/include/base/factor/factor_block_absolute.h
deleted file mode 100644
index 4e0da9d764f50d5f55c14c596696b138c9b76035..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_block_absolute.h
+++ /dev/null
@@ -1,141 +0,0 @@
-/**
- * \file factor_block_absolute.h
- *
- * Created on: Dec 15, 2017
- * \author: AtDinesh
- */
-
-#ifndef FACTOR_BLOCK_ABSOLUTE_H_
-#define FACTOR_BLOCK_ABSOLUTE_H_
-
-//Wolf includes
-#include "base/factor/factor_analytic.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorBlockAbsolute);
-
-//class
-class FactorBlockAbsolute : public FactorAnalytic
-{
- private:
- SizeEigen sb_size_; // the size of the state block
- SizeEigen sb_constrained_start_; // the index of the first state element that is constrained
- SizeEigen sb_constrained_size_; // the size of the state segment that is constrained
- Eigen::MatrixXs J_; // Jacobian
-
- public:
-
- /** \brief Constructor
- *
- * \param _sb_ptr the constrained state block
- * \param _start_idx (default=0) the index of the first state element that is constrained
- * \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
- *
- */
- FactorBlockAbsolute(StateBlockPtr _sb_ptr,
- unsigned int _start_idx = 0,
- int _size = -1,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
- _apply_loss_function,
- _status,
- _sb_ptr),
- sb_size_(_sb_ptr->getSize()),
- sb_constrained_start_(_start_idx),
- sb_constrained_size_(_size == -1 ? sb_size_ : _size)
- {
- assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
-
- // precompute Jacobian (Identity)
- if (sb_constrained_start_ == 0)
- J_ = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_size_);
- else
- {
- J_ = Eigen::MatrixXs::Zero(sb_constrained_size_,sb_size_);
- J_.middleCols(sb_constrained_start_,sb_constrained_size_) = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_constrained_size_);
- }
- }
-
- virtual ~FactorBlockAbsolute() = default;
-
- /** \brief Returns the residual evaluated in the states provided
- *
- * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
- *
- **/
- virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const;
-
- /** \brief Returns the normalized jacobians evaluated in the states
- *
- * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
- * 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
- *
- **/
- virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians,
- const std::vector<bool>& _compute_jacobian) const;
-
- /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
- *
- * Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
- *
- * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
- *
- **/
- virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const;
-
- /** \brief Returns the factor residual size
- **/
- virtual unsigned int getSize() const;
-};
-
-inline Eigen::VectorXs FactorBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
-{
- assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
- assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size");
-
- // residual
- if (sb_constrained_size_ == _st_vector.front().size())
- return getMeasurementSquareRootInformationUpper() * (_st_vector.front() - getMeasurement());
- else
- return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement());
-}
-
-inline void FactorBlockAbsolute::evaluateJacobians(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
-{
- assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
- assert(jacobians.size() == 1 && "Wrong number of jacobians!");
- assert(_compute_jacobian.size() == 1 && "Wrong number of _compute_jacobian booleans!");
- assert(_st_vector.front().size() == sb_size_ && "Wrong StateBlock size");
- assert(_st_vector.front().size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
- // normalized jacobian
- if (_compute_jacobian.front())
- jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
-}
-
-inline void FactorBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
-{
- assert(jacobians.size() == 1 && "Wrong number of jacobians!");
-
- jacobians.front() = J_;
-}
-
-inline unsigned int FactorBlockAbsolute::getSize() const
-{
- return sb_constrained_size_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_container.h b/include/base/factor/factor_container.h
deleted file mode 100644
index 93d455a8e7e36ce85e1688c5c70a91b020d963c9..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_container.h
+++ /dev/null
@@ -1,143 +0,0 @@
-#ifndef FACTOR_CONTAINER_H_
-#define FACTOR_CONTAINER_H_
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/landmark/landmark_container.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorContainer);
-
-class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
-{
- protected:
- LandmarkContainerWPtr lmk_ptr_;
- unsigned int corner_;
-
- public:
-
- FactorContainer(const FeatureBasePtr& _ftr_ptr,
- const LandmarkContainerPtr& _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- const unsigned int _corner,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorContainer,3,2,1,2,1>("CONTAINER",
- nullptr,
- nullptr,
- nullptr,
- _lmk_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _lmk_ptr->getP(),
- _lmk_ptr->getO()),
- lmk_ptr_(_lmk_ptr),
- corner_(_corner)
- {
- assert(/*_corner >= 0 &&*/ _corner <= 3 && "Wrong corner id in factor container constructor");
-
- std::cout << "new factor container: corner idx = " << corner_ << std::endl;
- }
-
- virtual ~FactorContainer() = default;
-
- LandmarkContainerPtr getLandmark()
- {
- return lmk_ptr_.lock();
- }
-
- template <typename T>
- bool operator()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP, const T* const _landmarkO, T* _residuals) const
- {
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
-
- //std::cout << "getSensorPosition: " << std::endl;
- //std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
- //std::cout << "getSensorRotation: " << std::endl;
- //std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
- //std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
-
- // sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot information
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
- Eigen::Matrix<T,2,2> R_landmark = Eigen::Rotation2D<T>(_landmarkO[0]).matrix();
- Eigen::Matrix<T,2,1> corner_position = lmk_ptr_.lock()->getCorner(corner_).head<2>().cast<T>();
-
- // Expected measurement
- Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map + R_landmark * corner_position) - sensor_position);
- T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapture()->getSensor()->getO()->getState()(0)) + T(lmk_ptr_.lock()->getCorner(corner_)(2));
-
- // Error
- residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
- residuals_map(2) = expected_measurement_orientation - T(getMeasurement()(2));
-
- // pi 2 pi
- while (_residuals[2] > T(M_PI))
- _residuals[2] = _residuals[2] - T(2*M_PI);
- while (_residuals[2] <= T(-M_PI))
- _residuals[2] = _residuals[2] + T(2*M_PI);
-
- // Residuals
- residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * residuals_map;
-
- //std::cout << "\nFACTOR: " << id() << std::endl;
- //std::cout << "Feature: " << getFeature()->id() << std::endl;
- //std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
- //std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
- //
- //std::cout << "robot pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _robotP[i];
- //std::cout << "\n\t" << _robotO[0];
- //std::cout << std::endl;
- //
- //std::cout << "landmark pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _landmarkP[i];
- //std::cout << "\n\t" << _landmarkO[0];
- //std::cout << std::endl;
- //
- //std::cout << "landmark pose (w.r.t sensor):";
- //Eigen::Matrix<T,2,1> relative_landmark_position = inverse_R_sensor * (inverse_R_robot * (landmark_position - robot_position) - sensor_position);
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << relative_landmark_position.data()[i];
- //std::cout << "\n\t" << _landmarkO[0] - _robotO[0] - T( *(getCapture()->getSensor()->getO()->get()) );
- //std::cout << std::endl;
- //
- //std::cout << "expected_measurement: ";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << expected_measurement_position.data()[i];
- //std::cout << "\n\t" << expected_measurement_orientation << std::endl;
- //
- //std::cout << "_residuals: "<< std::endl;
- //for (int i=0; i < 3; i++)
- // std::cout << "\n\t" << _residuals[i] << " ";
- //std::cout << std::endl;
-
- return true;
- }
-
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr)
- {
- unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
-
- return std::make_shared<FactorContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), _processor_ptr, corner);
- }
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_corner_2D.h b/include/base/factor/factor_corner_2D.h
deleted file mode 100644
index 2bcacaac7600e55cdbcfe672e6c1d4c88697a18e..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_corner_2D.h
+++ /dev/null
@@ -1,124 +0,0 @@
-#ifndef FACTOR_CORNER_2D_THETA_H_
-#define FACTOR_CORNER_2D_THETA_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/landmark/landmark_corner_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorCorner2D);
-
-class FactorCorner2D: public FactorAutodiff<FactorCorner2D, 3,2,1,2,1>
-{
- public:
-
- FactorCorner2D(const FeatureBasePtr _ftr_ptr,
- const LandmarkCorner2DPtr _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorCorner2D,3,2,1,2,1>("CORNER 2D",
- nullptr,
- nullptr,
- nullptr,
- _lmk_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _lmk_ptr->getP(),
- _lmk_ptr->getO())
- {
- //
- }
-
- virtual ~FactorCorner2D() = default;
-
- LandmarkCorner2DPtr getLandmark()
- {
- return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
- }
-
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const;
-
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr = nullptr)
- {
- return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
- }
-
-};
-
-template<typename T>
-inline bool FactorCorner2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const
-{
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
-
- //std::cout << "getSensorPosition: " << std::endl;
- //std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
- //std::cout << "getSensorRotation: " << std::endl;
- //std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
- //std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
-
- // sensor transformation
- Eigen::Matrix<T, 2, 1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T, 2, 1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map) - sensor_position);
- T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapture()->getSensor()->getO()->getState()(0));
-
- // Error
- residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
- residuals_map(2) = expected_measurement_orientation - T(getMeasurement()(2));
-
- // pi 2 pi
- while (_residuals[2] > T(M_PI))
- _residuals[2] = _residuals[2] - T(2*M_PI);
- while (_residuals[2] <= T(-M_PI))
- _residuals[2] = _residuals[2] + T(2*M_PI);
-
- // Residuals
- residuals_map = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>().cast<T>() * residuals_map;
-
- //std::cout << "\nFACTOR: " << id() << std::endl;
- //std::cout << "Feature: " << getFeature()->id() << std::endl;
- //std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
- //std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
- //
- //std::cout << "robot pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _robotP[i];
- //std::cout << "\n\t" << _robotO[0];
- //std::cout << std::endl;
- //
- //std::cout << "landmark pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _landmarkP[i];
- //std::cout << "\n\t" << _landmarkO[0];
- //std::cout << std::endl;
- //
- //std::cout << "expected_measurement: ";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << expected_measurement_position.data()[i];
- //std::cout << std::endl;
- //
- //std::cout << "_residuals: "<< std::endl;
- //for (int i=0; i < 3; i++)
- // std::cout << "\n\t" << _residuals[i] << " ";
- //std::cout << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_diff_drive.h b/include/base/factor/factor_diff_drive.h
deleted file mode 100644
index 89078b139473251b40ec0587d7e3256cbdc85bfd..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_diff_drive.h
+++ /dev/null
@@ -1,152 +0,0 @@
-/**
- * \file factor_diff_drive.h
- *
- * Created on: Oct 27, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef WOLF_FACTOR_DIFF_DRIVE_H_
-#define WOLF_FACTOR_DIFF_DRIVE_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/feature/feature_diff_drive.h"
-#include "base/capture/capture_wheel_joint_position.h"
-
-namespace
-{
-
-constexpr std::size_t RESIDUAL_SIZE = 3;
-constexpr std::size_t POSITION_STATE_BLOCK_SIZE = 2;
-constexpr std::size_t ORIENTATION_STATE_BLOCK_SIZE = 1;
-
-/// @todo This should be dynamic (2/3/5)
-constexpr std::size_t INTRINSICS_STATE_BLOCK_SIZE = 3;
-
-}
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorDiffDrive);
-
-class FactorDiffDrive :
- public FactorAutodiff<FactorDiffDrive,
- RESIDUAL_SIZE,
- POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
- POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
- INTRINSICS_STATE_BLOCK_SIZE >
-{
- using Base = FactorAutodiff<FactorDiffDrive,
- RESIDUAL_SIZE,
- POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
- POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
- INTRINSICS_STATE_BLOCK_SIZE>;
-
-public:
-
- FactorDiffDrive(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- const bool _apply_loss_function = false,
- const FactorStatus _status = FAC_ACTIVE) :
- Base("DIFF DRIVE",
- _frame_other_ptr,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
- capture_wheel_joint_ptr_(std::static_pointer_cast<CaptureWheelJointPosition>(_ftr_ptr->getCapture()))
- {
- //
- }
-
- /**
- * \brief Default destructor (not recommended)
- *
- * Default destructor.
- *
- **/
- virtual ~FactorDiffDrive() = default;
-
- template<typename T>
- bool operator ()(const T* const _p1, const T* const _o1,
- const T* const _p2, const T* const _o2, const T* const _c,
- T* _residuals) const;
-
- /**
- * \brief Returns the jacobians computation method
- **/
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
-protected:
-
- CaptureWheelJointPositionPtr capture_wheel_joint_ptr_;
-};
-
-template<typename T>
-inline bool
-FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
- const T* const _p2, const T* const _o2, const T* const _c,
- T* _residuals) const
-{
- //std::cout << "FactorDiffDrive::operator:\n";
-
- // MAPS
- Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals_map(_residuals);
-
- Eigen::Map<const Eigen::Matrix<T, POSITION_STATE_BLOCK_SIZE, 1> > p1_map(_p1);
- Eigen::Map<const Eigen::Matrix<T, POSITION_STATE_BLOCK_SIZE, 1> > p2_map(_p2);
-
- Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c_map(_c);
-
- // Compute corrected delta
-
- /// Is this my delta_preint ?
- const Eigen::Matrix<T, 3, 1> delta_preintegrated = getMeasurement().cast<T>();
- //std::cout << "delta_preintegrated:\n" << delta_preintegrated(0) << std::endl << delta_preintegrated(1) << std::endl << delta_preintegrated(2) << std::endl;
-
- Eigen::Matrix<T, 3, 1> delta_corrected = delta_preintegrated + capture_wheel_joint_ptr_->getJacobianCalib().cast<T>() * (c_map - capture_wheel_joint_ptr_->getCalibrationPreint().cast<T>());
-
- //std::cout << "delta_corrected:\n" << delta_corrected(0) << std::endl << delta_corrected(1) << std::endl << delta_corrected(2) << std::endl;
-
- // First 2d pose residual
-
- Eigen::Matrix<T, 3, 1> delta_predicted;
-
- // position
- delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2_map - p1_map);
-
- // orientation
- delta_predicted(2) = _o2[0] - _o1[0];
- //std::cout << "delta_predicted:\n" << delta_predicted(0) << std::endl << delta_predicted(1) << std::endl << delta_predicted(2) << std::endl;
-
-
- // residual
- residuals_map = delta_corrected - delta_predicted;
-
- // angle remapping
- while (residuals_map(2) > T(M_PI))
- residuals_map(2) = residuals_map(2) - T(2. * M_PI);
- while (residuals_map(2) <= T(-M_PI))
- residuals_map(2) = residuals_map(2) + T(2. * M_PI);
-
- // weighted residual
- residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * residuals_map;
-
- return true;
-}
-
-} // namespace wolf
-
-#endif /* WOLF_FACTOR_DIFF_DRIVE_H_ */
diff --git a/include/base/factor/factor_epipolar.h b/include/base/factor/factor_epipolar.h
deleted file mode 100644
index 42f0e855894c6f247ef30f15c77508157824fb68..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_epipolar.h
+++ /dev/null
@@ -1,69 +0,0 @@
-#ifndef FACTOR_EPIPOLAR_H
-#define FACTOR_EPIPOLAR_H
-
-#include "base/factor/factor_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorEpipolar);
-
-
-class FactorEpipolar : public FactorBase
-{
- public:
- FactorEpipolar(const FeatureBasePtr& _feature_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorEpipolar() = default;
-
- /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
- **/
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};
-
- /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
- **/
- virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override {};
-
- /** \brief Returns the jacobians computation method
- **/
- virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
-
- /** \brief Returns a vector of pointers to the states in which this factor depends
- **/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
-
- /** \brief Returns the factor residual size
- **/
- virtual unsigned int getSize() const override {return 0;}
-
- /** \brief Returns the factor states sizes
- **/
- virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});}
-
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr);
-
-};
-
-inline FactorEpipolar::FactorEpipolar(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorBase("EPIPOLAR", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
-{
- //
-}
-
-inline FactorBasePtr FactorEpipolar::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr)
-{
- return std::make_shared<FactorEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
-}
-
-} // namespace wolf
-
-#endif // FACTOR_EPIPOLAR_H
diff --git a/include/base/factor/factor_fix_bias.h b/include/base/factor/factor_fix_bias.h
deleted file mode 100644
index 6c1f321dc0f7fcaf965476dc802080e3d3bb23b5..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_fix_bias.h
+++ /dev/null
@@ -1,85 +0,0 @@
-
-#ifndef FACTOR_FIX_BIAS_H_
-#define FACTOR_FIX_BIAS_H_
-
-//Wolf includes
-#include "base/capture/capture_IMU.h"
-#include "base/feature/feature_IMU.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
-
-//#include "ceres/jet.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorFixBias);
-
-//class
-class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
-{
- public:
- FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorFixBias, 6, 3, 3>("FIX BIAS",
- nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
- std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
- {
- // std::cout << "created FactorFixBias " << std::endl;
- }
-
- virtual ~FactorFixBias() = default;
-
- template<typename T>
- bool operator ()(const T* const _ab, const T* const _wb, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorFixBias::operator ()(const T* const _ab, const T* const _wb, T* _residuals) const
-{
- // measurement
- Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
- Eigen::Matrix<T,3,1> ab(_ab);
- Eigen::Matrix<T,3,1> wb(_wb);
-
- // error
- Eigen::Matrix<T,6,1> er;
- er.head(3) = meas.head(3) - ab;
- er.tail(3) = meas.tail(3) - wb;
-
- // residual
- Eigen::Map<Eigen::Matrix<T,6,1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
- ////////////////////////////////////////////////////////
- // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
-// using ceres::Jet;
-// Eigen::MatrixXs J(6,6);
-// J.row(0) = ((Jet<Scalar, 3>)(er(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(er(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(er(2))).v;
-// J.row(3) = ((Jet<Scalar, 3>)(er(3))).v;
-// J.row(4) = ((Jet<Scalar, 3>)(er(4))).v;
-// J.row(5) = ((Jet<Scalar, 3>)(er(5))).v;
-
-// J.row(0) = ((Jet<Scalar, 3>)(res(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(res(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
-// J.row(3) = ((Jet<Scalar, 3>)(res(3))).v;
-// J.row(4) = ((Jet<Scalar, 3>)(res(4))).v;
-// J.row(5) = ((Jet<Scalar, 3>)(res(5))).v;
-
-// if (sizeof(er(0)) != sizeof(double))
-// {
-// std::cout << "FactorFixBias::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorFixBias::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
- ////////////////////////////////////////////////////////
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_odom_2D.h b/include/base/factor/factor_odom_2D.h
deleted file mode 100644
index 95cf7559282c9bd356c53c92cab6f1234172e993..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_odom_2D.h
+++ /dev/null
@@ -1,99 +0,0 @@
-#ifndef FACTOR_ODOM_2D_THETA_H_
-#define FACTOR_ODOM_2D_THETA_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-
-//#include "ceres/jet.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorOdom2D);
-
-//class
-class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
-{
- public:
- FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
- {
- //
- }
-
- virtual ~FactorOdom2D() = default;
-
- template<typename T>
- bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
- T* _residuals) const;
-
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
- {
- return std::make_shared<FactorOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
- }
-
-};
-
-template<typename T>
-inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
- const T* const _o2, T* _residuals) const
-{
-
- // MAPS
- Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
- Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
- Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
- T o1 = _o1[0];
- T o2 = _o2[0];
- Eigen::Matrix<T, 3, 1> expected_measurement;
- Eigen::Matrix<T, 3, 1> er; // error
-
- // Expected measurement
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-o1) * (p2 - p1);
- expected_measurement(2) = o2 - o1;
-
- // Error
- er = expected_measurement - getMeasurement().cast<T>();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
-
- // Residuals
- res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
- ////////////////////////////////////////////////////////
- // 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 << "FactorOdom2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2D::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
- ////////////////////////////////////////////////////////
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_odom_2D_analytic.h b/include/base/factor/factor_odom_2D_analytic.h
deleted file mode 100644
index 2eb3296240c9b28766919748875eaea9720d87e0..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_odom_2D_analytic.h
+++ /dev/null
@@ -1,108 +0,0 @@
-#ifndef FACTOR_ODOM_2D_ANALYTIC_H_
-#define FACTOR_ODOM_2D_ANALYTIC_H_
-
-//Wolf includes
-#include "base/factor/factor_relative_2D_analytic.h"
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorOdom2DAnalytic);
-
-//class
-class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
-{
- public:
- FactorOdom2DAnalytic(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2DAnalytic("ODOM_2D", _ftr_ptr,
- _frame_ptr, _processor_ptr, _apply_loss_function, _status)
- {
- //
- }
-
- virtual ~FactorOdom2DAnalytic() = default;
-
-// /** \brief Returns the factor residual size
-// *
-// * Returns the factor residual size
-// *
-// **/
-// virtual unsigned int getSize() const
-// {
-// return 3;
-// }
-//
-// /** \brief Returns the residual evaluated in the states provided
-// *
-// * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
-// *
-// **/
-// virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const
-// {
-// Eigen::VectorXs residual(3);
-// Eigen::VectorXs expected_measurement(3);
-//
-// // Expected measurement
-// Eigen::Matrix2s R = Eigen::Rotation2D<Scalar>(-_st_vector[1](0)).matrix();
-// expected_measurement.head(2) = R * (_st_vector[2]-_st_vector[0]); // rotar menys l'angle de primer (-_o1)
-// expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
-//
-// // Residual
-// residual = expected_measurement - getMeasurement();
-// while (residual(2) > M_PI)
-// residual(2) = residual(2) - 2*M_PI;
-// while (residual(2) <= -M_PI)
-// residual(2) = residual(2) + 2*M_PI;
-// residual = getMeasurementSquareRootInformationUpper() * residual;
-//
-// return residual;
-// }
-//
-// /** \brief Returns the jacobians evaluated in the states provided
-// *
-// * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
-// * 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
-// *
-// **/
-// virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXs>>& jacobians, const std::vector<bool>& _compute_jacobian) const
-// {
-// jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
-// sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
-// 0, 0;
-// jacobians[0] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-// jacobians[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)),
-// -1;
-// jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-// jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-// -sin(_st_vector[1](0)),cos(_st_vector[1](0)),
-// 0, 0;
-// jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-// jacobians[3] << 0, 0, 1;
-// jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-// }
-
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr)
- {
- return std::make_shared<FactorOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
- }
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_odom_3D.h b/include/base/factor/factor_odom_3D.h
deleted file mode 100644
index ce3456ef0e8c286c7b082a79a41fa268e4608a8e..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_odom_3D.h
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
- * factor_odom_3D.h
- *
- * Created on: Oct 7, 2016
- * Author: jsola
- */
-
-#ifndef FACTOR_ODOM_3D_H_
-#define FACTOR_ODOM_3D_H_
-
-#include "base/factor/factor_autodiff.h"
-#include "base/rotations.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(FactorOdom3D);
-
-//class
-class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
-{
- public:
- FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorOdom3D() = 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,
- 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,
- T* _expectation_dp,
- T* _expectation_dq) const;
-
- Eigen::VectorXs expectation() const;
-
- private:
- template<typename T>
- void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
-
-};
-
-template<typename T>
-inline void FactorOdom3D::printRes(const Eigen::Matrix<T, 6, 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;
- std::cout << r(3) << std::endl;
- std::cout << r(4) << std::endl;
- std::cout << r(5) << std::endl;
-}
-
-template<>
-inline void FactorOdom3D::printRes (const Eigen::Matrix<Scalar,6,1> & r) const
-{
- std::cout << "Scalar residual = " << std::endl;
- std::cout << r.transpose() << std::endl;
-}
-
-inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorOdom3D, 6, 3, 4, 3, 4>("ODOM 3D", // type
- _frame_past_ptr, // frame other
- nullptr, // capture other
- nullptr, // feature other
- nullptr, // landmark other
- _processor_ptr, // processor
- _apply_loss_function,
- _status,
- _ftr_current_ptr->getFrame()->getP(), // current frame P
- _ftr_current_ptr->getFrame()->getO(), // current frame Q
- _frame_past_ptr->getP(), // past frame P
- _frame_past_ptr->getO()) // past frame Q
-{
-// WOLF_TRACE("Constr ODOM 3D (f", _ftr_current_ptr->id(),
-// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
-// ") (Fo", _frame_past_ptr->id(), ")");
-//
-// WOLF_TRACE("delta preint: ", _ftr_current_ptr->getMeasurement().transpose());
-//
-// WOLF_TRACE("Omega_delta.sqrt: \n", _ftr_current_ptr->getMeasurementSquareRootInformationUpper());
- //
-}
-
-template<typename T>
-inline bool FactorOdom3D::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
-{
- 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<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";
-
- // estimate motion increment, dp, dq;
- expectation_dp = q_past.conjugate() * (p_current - p_past);
- expectation_dq = q_past.conjugate() * q_current;
-
-// std::cout << "exp_p: " << expectation_dp(0) << " "
-// << expectation_dp(1) << " "
-// << expectation_dp(2) << "\n";
-// std::cout << "exp_q: " << expectation_dq.coeffs()(0) << " "
-// << expectation_dq.coeffs()(1) << " "
-// << expectation_dq.coeffs()(2) << " "
-// << expectation_dq.coeffs()(3) << "\n";
-
- return true;
-}
-
-inline Eigen::VectorXs FactorOdom3D::expectation() const
-{
- Eigen::VectorXs exp(7);
- FrameBasePtr frm_current = getFeature()->getFrame();
- FrameBasePtr frm_past = getFrameOther();
- const Eigen::VectorXs frame_current_pos = frm_current->getP()->getState();
- const Eigen::VectorXs frame_current_ori = frm_current->getO()->getState();
- const Eigen::VectorXs frame_past_pos = frm_past->getP()->getState();
- const Eigen::VectorXs 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(),
- exp.data(),
- exp.data()+3);
- return exp;
-}
-
-template<typename T>
-inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
- const T* const _q_past, T* _residuals) const
-{
-
- /* Residual expression
- * -------------------
- *
- * Given two states x_i, x_j, with
- *
- * x_i = [p_i , q_i] // position and quaternion: PQ pose
- *
- * we define the (-) operator as
- *
- * x_j (-) x_i = [q_i.conj * (p_j - p_i) , q_i.conj * q_j] // PQ pose increment
- *
- * we also define Log and Exp maps as
- *
- * Log(x) = [p, Log(q)] // PQ pose to vector pose
- * Exp(v) = [p, Exp(o)] // vector pose to PQ pose
- *
- * where
- *
- * v = [p,o] is the vector representation of a [p,q] pose.
- *
- * Note: The Log(q) and Exp(o) maps are here implemented as q2v() and v2q() respectively.
- *
- * Finally the residual is developed as follows. Given a measurement m
- *
- * m = [p_m, o_m] \in R^6
- *
- * then
- *
- * r = log [ exp(m) (-) ( x_j (-) x_i ) ]
- *
- * In the code below, we use _i and _j indices as follows:
- *
- * x_i = x_past
- * x_j = x_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);
-
- // measured motion increment, dp_m, dq_m
- Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
- Eigen::Quaternion<T> dq_m(getMeasurement().tail<4>().cast<T>());
-
- Eigen::Matrix<T,3,1> dp = expected.head(3);
- Eigen::Quaternion<T> dq;
- dq.x() = expected(3);
- dq.y() = expected(4);
- dq.z() = expected(5);
- dq.w() = expected(6);
-
- residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
- residuals.tail(3) = q2v(dq.conjugate() * dq_m);
-
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
-
- return true;
-}
-
-} /* namespace wolf */
-
-#endif /* FACTOR_ODOM_3D_H_ */
diff --git a/include/base/factor/factor_point_2D.h b/include/base/factor/factor_point_2D.h
deleted file mode 100644
index 4d9686e3805c9fb5d337ed033e90a904cbb4ae53..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_point_2D.h
+++ /dev/null
@@ -1,140 +0,0 @@
-#ifndef FACTOR_POINT_2D_THETA_H_
-#define FACTOR_POINT_2D_THETA_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPoint2D);
-
-/**
- * @brief The FactorPoint2D class
- */
-class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
-{
- protected:
- unsigned int feature_point_id_;
- int landmark_point_id_;
- StateBlockPtr point_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
-
- public:
-
- FactorPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
- feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
- {
- //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
- //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
- }
-
- virtual ~FactorPoint2D() = default;
-
- /**
- * @brief getLandmarkPtr
- * @return
- */
- LandmarkPolyline2DPtr getLandmark()
- {
- return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
- }
-
- /**
- * @brief getLandmarkPointId
- * @return
- */
- int getLandmarkPointId()
- {
- return landmark_point_id_;
- }
-
- /**
- * @brief getFeaturePointId
- * @return
- */
- unsigned int getFeaturePointId()
- {
- return feature_point_id_;
- }
-
- /**
- * @brief getLandmarkPointPtr
- * @return
- */
- StateBlockPtr getLandmarkPoint()
- {
- return point_state_ptr_;
- }
-
- /**
- *
- */
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXs& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
-};
-
-template<typename T>
-inline bool FactorPoint2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const
-{
- //std::cout << "FactorPointToLine2D::operator" << std::endl;
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,2,1>> residuals_map(_residuals);
-
- // Landmark point global position
- Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginO) * landmark_position_map;
-
- // sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_point - robot_position_map) - sensor_position);
-
- // Residuals
- residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_measurement_position - getMeasurement().head<2>().cast<T>());
-
- //std::cout << "residuals_map" << residuals_map[0] << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h
deleted file mode 100644
index c08a86b076de7a870e3752b83916b66cfbd41fd4..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_point_to_line_2D.h
+++ /dev/null
@@ -1,151 +0,0 @@
-#ifndef FACTOR_POINT_TO_LINE_2D_H_
-#define FACTOR_POINT_TO_LINE_2D_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPointToLine2D);
-
-//class
-class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>
-{
- protected:
- int landmark_point_id_;
- int landmark_point_aux_id_;
- unsigned int feature_point_id_;
- StateBlockPtr point_state_ptr_;
- StateBlockPtr point_aux_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
-
- public:
-
- FactorPointToLine2D(const FeaturePolyline2DPtr& _ftr_ptr,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
- landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
- {
- //std::cout << "FactorPointToLine2D" << std::endl;
- //std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
- assert(_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_));
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
- }
-
- virtual ~FactorPointToLine2D() = default;
-
- LandmarkPolyline2DPtr getLandmark()
- {
- return std::static_pointer_cast<LandmarkPolyline2D>( landmark_other_ptr_.lock() );
- }
-
- int getLandmarkPointId()
- {
- return landmark_point_id_;
- }
-
- int getLandmarkPointAuxId()
- {
- return landmark_point_aux_id_;
- }
-
- unsigned int getFeaturePointId()
- {
- return feature_point_id_;
- }
-
- StateBlockPtr getLandmarkPoint()
- {
- return point_state_ptr_;
- }
-
- StateBlockPtr getLandmarkPointAux()
- {
- return point_state_ptr_;
- }
-
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXs& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
-};
-
-template<typename T>
-inline bool FactorPointToLine2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const
-{
- //std::cout << "FactorPointToLine2D::operator" << std::endl;
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginPosition);
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_aux_position_map(_landmarkPointAux);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
-
- Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginOrientation) * landmark_position_map;
- Eigen::Matrix<T,2,1> landmark_point_aux = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginOrientation) * landmark_aux_position_map;
-
- // sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T,2,1> expected_P = inverse_R_sensor * (inverse_R_robot * (landmark_point - robot_position_map) - sensor_position);
- Eigen::Matrix<T,2,1> expected_Paux = inverse_R_sensor * (inverse_R_robot * (landmark_point_aux - robot_position_map) - sensor_position);
-
- // Case projection inside the segment P-Paux
-
- // Case projection ouside (P side)
-
- // Case projection ouside (Paux side)
-
- // normal vector
- Eigen::Matrix<T,2,1> normal(expected_Paux(1)-expected_P(1), expected_P(0)-expected_Paux(0));
- normal = normal / normal.norm();
-
- // Residual: distance = projection to the normal vector
- _residuals[0] = ((expected_P-measurement_.head<2>().cast<T>()).dot(normal));
-
- T projected_cov = normal.transpose() * measurement_covariance_.cast<T>() * normal;
- _residuals[0] = _residuals[0] / sqrt(projected_cov);
-
- //std::cout << "landmark points:" << std::endl;
- //std::cout << "A: " << expected_P(0) << " " << expected_P(1) << std::endl;
- //std::cout << "Aaux: " << expected_Paux(0) << " " << expected_Paux(1) << std::endl;
- //std::cout << "B: " << measurement_.transpose() << std::endl;
- //std::cout << "d: " << _residuals[0] << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_pose_2D.h b/include/base/factor/factor_pose_2D.h
deleted file mode 100644
index 306b5e15ebf9e2e67eb41c8e5072aa050f7d6b32..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_pose_2D.h
+++ /dev/null
@@ -1,76 +0,0 @@
-
-#ifndef FACTOR_POSE_2D_H_
-#define FACTOR_POSE_2D_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
-
-//#include "ceres/jet.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPose2D);
-
-//class
-class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
-{
- public:
- FactorPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
- {
-// std::cout << "created FactorPose2D " << std::endl;
- }
-
- virtual ~FactorPose2D() = default;
-
- template<typename T>
- bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
-{
- // measurement
- Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
-
- // error
- Eigen::Matrix<T,3,1> er;
- er(0) = meas(0) - _p[0];
- er(1) = meas(1) - _p[1];
- er(2) = meas(2) - _o[0];
- while (er[2] > T(M_PI))
- er(2) = er(2) - T(2*M_PI);
- while (er(2) <= T(-M_PI))
- er(2) = er(2) + T(2*M_PI);
-
- // residual
- Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
- ////////////////////////////////////////////////////////
- // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
-// using ceres::Jet;
-// Eigen::MatrixXs J(3,3);
-// J.row(0) = ((Jet<Scalar, 3>)(er(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(er(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(er(2))).v;
-// J.row(0) = ((Jet<Scalar, 3>)(res(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(res(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
-// if (sizeof(er(0)) != sizeof(double))
-// {
-// std::cout << "FactorPose2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorPose2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
- ////////////////////////////////////////////////////////
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_pose_3D.h b/include/base/factor/factor_pose_3D.h
deleted file mode 100644
index b9c4da39516235081a4ec91334fa06958478264c..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_pose_3D.h
+++ /dev/null
@@ -1,58 +0,0 @@
-
-#ifndef FACTOR_POSE_3D_H_
-#define FACTOR_POSE_3D_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPose3D);
-
-//class
-class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
-{
- public:
-
- FactorPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
- {
- //
- }
-
- virtual ~FactorPose3D() = default;
-
- template<typename T>
- bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
-{
-
- // states
- Eigen::Matrix<T, 3, 1> p(_p);
- Eigen::Quaternion<T> q(_o);
-
- // measurements
- Eigen::Vector3s p_measured(getMeasurement().data() + 0);
- Eigen::Quaternions q_measured(getMeasurement().data() + 3);
-
- // error
- Eigen::Matrix<T, 6, 1> er;
- er.head(3) = p_measured.cast<T>() - p;
- er.tail(3) = q2v(q.conjugate() * q_measured.cast<T>());
-
- // residual
- Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_quaternion_absolute.h b/include/base/factor/factor_quaternion_absolute.h
deleted file mode 100644
index 1864a6c8eede4bd07190c4da737a7e1f25da117f..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_quaternion_absolute.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/**
- * \file factor_quaternion_absolute.h
- *
- * Created on: Dec 15, 2017
- * \author: AtDinesh
- */
-
-#ifndef FACTOR_QUATERNION_ABSOLUTE_H_
-#define FACTOR_QUATERNION_ABSOLUTE_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/local_parametrization_quaternion.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorQuaternionAbsolute);
-
-//class
-class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3,4>
-{
- public:
-
- FactorQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorQuaternionAbsolute,3,4>("QUATERNION ABS",
- nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
- {
- //
- }
-
- virtual ~FactorQuaternionAbsolute() = default;
-
- template<typename T>
- bool operator ()(const T* const _o, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residuals) const
-{
-
- // states
- Eigen::Quaternion<T> q1(_o);
-
- // measurements
- Eigen::Quaternions q2(getMeasurement().data() + 0); //q_measured
-
- /* error
- * to compute the difference between both quaternions, we do
- * diff = log(q2 * q1.conj)
- * isolating q2 we get
- * q2 = exp(diff) * q1 ==> exp on the left means global.
- *
- * In rotations.h, we have
- * minus(q1,q2) = minus_right(q1,q2) = log_q(q1.conjugate() * q2); --> this is a local 'minus'
- * minus_left(q1,q2) = log_q(q2.conjugate() * q1); --> this is a global 'minus'
- */
-
- Eigen::Matrix<T, 3, 1> er;
- er = minus_left( q1, q2.cast<T>() );
-
- // residual
- Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_relative_2D_analytic.h b/include/base/factor/factor_relative_2D_analytic.h
deleted file mode 100644
index 5c3aec2333bb85999c6892c6ce365e0de3c5ac2e..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_relative_2D_analytic.h
+++ /dev/null
@@ -1,158 +0,0 @@
-#ifndef FACTOR_RELATIVE_2D_ANALYTIC_H_
-#define FACTOR_RELATIVE_2D_ANALYTIC_H_
-
-//Wolf includes
-#include "base/factor/factor_analytic.h"
-#include "base/landmark/landmark_base.h"
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorRelative2DAnalytic);
-
-//class
-class FactorRelative2DAnalytic : public FactorAnalytic
-{
- public:
-
- /** \brief Constructor of category FAC_FRAME
- **/
- FactorRelative2DAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
- {
- //
- }
-
- /** \brief Constructor of category FAC_FEATURE
- **/
- FactorRelative2DAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FeatureBasePtr& _ftr_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, 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(), _ftr_other_ptr->getFrame()->getO() )
- {
- //
- }
-
- /** \brief Constructor of category FAC_LANDMARK
- **/
- FactorRelative2DAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const LandmarkBasePtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
- {
- //
- }
-
- virtual ~FactorRelative2DAnalytic() = default;
-
- /** \brief Returns the factor residual size
- **/
- virtual unsigned int getSize() const override
- {
- return 3;
- }
-
- /** \brief Returns the residual evaluated in the states provided
- *
- * Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXs
- **/
- virtual Eigen::VectorXs evaluateResiduals(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _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::VectorXs.
- * 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
- *
- **/
- virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& 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
- *
- **/
- virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const override;
-
-};
-
-/// IMPLEMENTATION ///
-
-inline Eigen::VectorXs FactorRelative2DAnalytic::evaluateResiduals(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
-{
- Eigen::VectorXs residual(3);
- Eigen::VectorXs expected_measurement(3);
- // Expected measurement
- Eigen::Matrix2s R = Eigen::Rotation2D<Scalar>(-_st_vector[1](0)).matrix();
- expected_measurement.head(2) = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
- expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
- // Residual
- residual = expected_measurement - getMeasurement();
- while (residual(2) > M_PI)
- residual(2) = residual(2) - 2 * M_PI;
- while (residual(2) <= -M_PI)
- residual(2) = residual(2) + 2 * M_PI;
- residual = getMeasurementSquareRootInformationUpper() * residual;
- return residual;
-}
-
-inline void FactorRelative2DAnalytic::evaluateJacobians(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
-{
- jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)), sin(_st_vector[1](0)), -cos(_st_vector[1](0)), 0, 0;
- jacobians[0] = getMeasurementSquareRootInformationUpper() * jacobians[0];
- jacobians[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)), -1;
- jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[1];
- jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)), -sin(_st_vector[1](0)), cos(_st_vector[1](0)), 0, 0;
- jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[2];
- jacobians[3] << 0, 0, 1;
- jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[3];
-}
-
-inline void FactorRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
-{
- jacobians[0]
- << -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)), sin(
- getStateBlockPtrVector()[1]->getState()(0)), -cos(getStateBlockPtrVector()[1]->getState()(0)), 0, 0;
-
- jacobians[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)), -1;
-
- jacobians[2]
- << cos(getStateBlockPtrVector()[1]->getState()(0)), sin(getStateBlockPtrVector()[1]->getState()(0)), -sin(
- getStateBlockPtrVector()[1]->getState()(0)), cos(getStateBlockPtrVector()[1]->getState()(0)), 0, 0;
-
- jacobians[3]
- << 0, 0, 1;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factory.h b/include/base/factory.h
deleted file mode 100644
index b161126e45c18e4cfd8a953a32b5247d58fa1d27..0000000000000000000000000000000000000000
--- a/include/base/factory.h
+++ /dev/null
@@ -1,381 +0,0 @@
-/**
- * \file factory.h
- *
- * Created on: May 16, 2016
- * \author: jsola
- */
-
-#ifndef FACTORY_H_
-#define FACTORY_H_
-
-// wolf
-#include "base/wolf.h"
-
-// std
-#include <string>
-#include <map>
-#include <iostream>
-#include <iomanip>
-
-// yaml
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-/** \brief Singleton template factory
- *
- * This class implements a generic factory as a singleton.
- *
- * > IMPORTANT: This template factory can be used to construct many different objects except:
- * > - Objects deriving from SensorBase --> see SensorFactory
- * > - Objects deriving from ProcessorBase --> see ProcessorFactory
- * >
- * > The reason for this is that the two cases above need a more elaborated API than the one in this template class.
- *
- * \param TypeBase base type of all the objects created by the factory
- * \param TypeInput type of the input argument. Typical cases are std::string for file names, and YAML::Node for YAML nodes.
- *
- * - The class is templatized on the class of the produced objects, __TypeBase__.
- * The produced objects are always of a class deriving from TypeBase.
- * The returned data is always a pointer to TypeBase.
- *
- * For example, you may use as __TypeBase__ the following types:
- * - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers ````LandmarkBasePtr```` to them
- * - IntrinsicsBase: the Factory creates intrinsic parameters deriving from IntrinsicsBase and returns base pointers ````IntrinsicsBasePtr```` to them
- * - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBasePtr```` to them.
- *
- * - The class in also templatized on the type of the input parameter of the creator, __TypeInput__:
- * - ````std::string```` is used when the input parameter is a file name from which to read data (typically a YAML file).
- * - ````YAML::Node```` is used when the input parameter is a YAML node with structured data.
- *
- * ### Operation of the factory
- *
- * #### Rationale
- *
- * This factory can create objects of classes deriving from TypeBase.
- *
- * > For example, if you want to make a Landmark factory, set TypeBase = LandmarkBase.\n
- * > Then, the factory will create specific landmarks deriving from LandmarkBase.\n
- * > The specific type of landmark (e.g. LandmarkCorner2D, LandmarkAHP, LandmarkPolyline2D, etc) is indicated by a string that we call TYPE in this documentation.
- *
- * Specific object creation is invoked by the method ````create(TYPE, params ... )````, where
- * - the TYPE of object to create is identified with a string
- * - the params may be provided in different forms -- see TypeInput.
- *
- * The methods to create specific objects are called __creators__.
- * Creators must be registered to the factory before they can be invoked for object creation.
- *
- * This documentation shows you how to:
- * - Define correct TYPE names
- * - Access the factory
- * - Write object creators
- * - Register and unregister object creators to the factory
- * - Create objects using the factory
- * - Examples: Write and register a landmark creator for LandmarkPolyline2D.
- *
- * #### Define correct TYPE names
- * The rule to make new TYPE strings unique is that you skip the generic 'Type' prefix from your class name,
- * and you build a string in CAPITALS with space separators, e.g.:
- * - IntrinsicsCamera -> ````"CAMERA"````
- * - IntrinsicsLaser2D -> ````"LASER 2D"````
- * - LandmarkPolyline2D -> ````"POLYLINE 2D"````
- * - etc.
- *
- * #### Access the factory
- * The first thing to know is that we have defined typedefs for the templates that we are using. For example:
- *
- * \code
- * typedef Factory<IntrinsicsBase, std::string> IntrinsicsFactory;
- * typedef Factory<ProcessorParamsBase, std::string> ProcessorParamsFactory;
- * typedef Factory<LandmarkBase, YAML::Node> LandmarkFactory;
- * \endcode
- *
- * Second to know, the Factory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
- * To access it, use the static method get(),
- *
- * \code
- * Factory<MyTypeBase, MyTypeInput>::get()
- * \endcode
- *
- * where, of course, you better make use of the appropriate typedef in place of ````Factory<MyTypeBase, MyTypeInput>````.
- *
- * You can then call the methods you like, e.g. to create a landmark, you use:
- *
- * \code
- * LandmarkFactory::get().create(...); // see below for creating objects ...
- * \endcode
- *
- * #### Write creator methods (in your derived object classes)
- * The method LandmarkPolyline2D::create(...) exists in the LandmarkPolyline2D class as a static method.
- * All these ````XxxXxx::create()```` methods need to have exactly the same API, regardless of the object type.
- * The API puts into play the two template parameters:
- *
- * \code
- * static TypeBase* create( const TypeInput& );
- * \endcode
- *
- * This API includes an element of type TypeInput, which might be either a std::string, or a YAML::node:
- * - ````std::string```` is used to indicate the name of a configuration file. These files are usually YAML files containing configuration data to create your object.
- * - ````YAML::Node```` is used to access parts of a YAML file already encoded as nodes, such as when loading landmarks from a SLAM map stored as a YAML file.
- *
- * Two examples:
- *
- * \code
- * static IntrinsicsBasePtr create(const std::string& _intrinsics_dot_yaml)
- * static LandmarkBasePtr create(const YAML::Node& _lmk_yaml_node)
- * \endcode
- *
- * See further down for an implementation example.
- *
- * #### Register object creators
- * Prior to invoking the creation of an object of a particular TYPE,
- * you must register the creator for this type into the factory.
- *
- * Registering object creators into the factory is done through registerCreator().
- * You provide an object TYPE string (above), and a pointer to a static method
- * that knows how to create your specific object, e.g.:
- *
- * \code
- * LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
- * \endcode
- *
- * #### Automatic registration
- * Currently, registering is performed in specific source files, object_xxxx.cpp.
- * For example, in sensor_camera_yaml.cpp we find the line:
- *
- * \code
- * const bool registered_camera_intr = IntrinsicsFactory::get().registerCreator("CAMERA", createIntrinsicsCamera);
- * \endcode
- *
- * which is a static invocation (i.e., it is placed at global scope outside of the IntrinsicsCamera class).
- *
- * Therefore, at application level, all objects that have a .cpp file compiled are automatically registered.
- *
- * #### Unregister object creators
- * The method unregisterCreator() unregisters the ObjectXxx::create() method. It only needs to be passed the string of the object type.
- *
- * \code
- * Factory<MyTypeBase, MyTypeInput>::get().unregisterCreator("CAMERA");
- * \endcode
- *
- * #### Create objects using the factory
- * Note: Prior to invoking the creation of a object of a particular type,
- * you must register the creator for this type into the factory.
- *
- * To create e.g. a LandmarkPolyline2D from a YAML node you type:
- *
- * \code
- * LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("POLYLINE 2D", lmk_yaml_node);
- * \endcode
- *
- * or even better, make use of the convenient typedefs:
- *
- * \code
- * LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create("POLYLINE 2D", lmk_yaml_node);
- * \endcode
- *
- * ### Examples
- * #### Example 1: Writing the creator of LandmarkPolyline2D from a YAML node
- *
- * You can find this code in the landmark_polyline_2D.cpp file.
- *
- * \code
- * // Creator (this method is static):
- * LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
- * {
- * // Parse YAML node with lmk info and data
- * unsigned int id = _lmk_node["id"].as<unsigned int>();
- * int first_id = _lmk_node["first_id"].as<int>();
- * bool first_defined = _lmk_node["first_defined"].as<bool>();
- * bool last_defined = _lmk_node["last_defined"].as<bool>();
- * unsigned int npoints = _lmk_node["points"].size();
- * Eigen::MatrixXs points(2,npoints);
- * for (unsigned int i = 0; i < npoints; i++)
- * {
- * points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2s>();
- * }
- *
- * // Create a new landmark
- * LandmarkBasePtr lmk_ptr = new LandmarkPolyline2D(points, first_defined, last_defined, first_id);
- * lmk_ptr->setId(id);
- *
- * return lmk_ptr;
- * }
- * \endcode
- *
- * #### Example 2: Registering the creator of LandmarkPolyline2D from a YAML node
- *
- * You can find this code in the landmark_polyline_2D.cpp file.
- *
- * \code
- * // Register landmark creator (put the register code inside an unnamed namespace):
- * namespace
- * {
- * const bool registered_lmk_polyline_2D = LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
- * }
- *
- * \endcode
- *
- * ### More information
- * - IntrinsicsFactory: typedef of this template to create intrinsic structs deriving from IntrinsicsBase directly from YAML files.
- * - ProcessorParamsFactory: typedef of this template to create processor params structs deriving from ProcessorParamsBase directly from YAML files.
- * - LandmarkFactory: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML nodes.
- * - Problem::loadMap() : to load a maps directly from YAML files.
- * - You can also check the code in the example file ````src/examples/test_map_yaml.cpp````.
- *
- * #### See also
- * - SensorFactory: to create sensors
- * - ProcessorFactory: to create processors.
- * - Problem::installSensor() : to install sensors in WOLF Problem.
- * - Problem::installProcessor() : to install processors in WOLF Problem.
- *
- */
-template<class TypeBase, typename... TypeInput>
-class Factory
-{
- typedef std::shared_ptr<TypeBase> TypeBasePtr;
- public:
- // example of creator callback (see typedefs below)
- typedef TypeBasePtr (*CreatorCallback)(TypeInput... _input);
- private:
- typedef std::map<std::string, CreatorCallback> CallbackMap;
- public:
- bool registerCreator(const std::string& _type, CreatorCallback createFn);
- bool unregisterCreator(const std::string& _type);
- TypeBasePtr create(const std::string& _type, TypeInput... _input);
- std::string getClass();
-
- private:
- CallbackMap callbacks_;
-
- // Singleton ---------------------------------------------------
- // This class is a singleton. The code below guarantees this.
- // See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
- public:
- static Factory& get();
- public:
- Factory(const Factory&) = delete;
- void operator=(Factory const&) = delete;
- private:
- Factory() { }
- ~Factory() { }
-};
-
-template<class TypeBase, typename... TypeInput>
-inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
-{
- bool reg = callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
- if (reg)
- std::cout << std::setw(22) << std::left << getClass() << " <-- registered " << _type << std::endl;
- else
- std::cout << std::setw(22) << std::left << getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
-
- return reg;
-}
-
-template<class TypeBase, typename... TypeInput>
-inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string& _type)
-{
- return callbacks_.erase(_type) == 1;
-}
-
-template<class TypeBase, typename... TypeInput>
-inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
-{
- typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
-
- if (creator_callback_it == callbacks_.end())
- // not found
- throw std::runtime_error(getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
-
- // Invoke the creation function
- return (creator_callback_it->second)(std::forward<TypeInput>(_input)...);
-}
-
-template<class TypeBase, typename... TypeInput>
-inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
-{
- static Factory instance_;
- return instance_;
-}
-
-template<class TypeBase, typename... TypeInput>
-inline std::string Factory<TypeBase, TypeInput...>::getClass()
-{
- return "Factory<class TypeBase>";
-}
-
-} // namespace wolf
-
-namespace wolf
-{
-
-// Some specializations
-//======================
-
-// Intrinsics
-struct IntrinsicsBase;
-typedef Factory<IntrinsicsBase,
- const std::string&> IntrinsicsFactory;
-template<>
-inline std::string IntrinsicsFactory::getClass()
-{
- return "IntrinsicsFactory";
-}
-
-// ProcessorParams
-struct ProcessorParamsBase;
-typedef Factory<ProcessorParamsBase,
- const std::string&> ProcessorParamsFactory;
-template<>
-inline std::string ProcessorParamsFactory::getClass()
-{
- return "ProcessorParamsFactory";
-}
-
-// Landmarks from YAML
-class LandmarkBase;
-typedef Factory<LandmarkBase,
- const YAML::Node&> LandmarkFactory;
-template<>
-inline std::string LandmarkFactory::getClass()
-{
- return "LandmarkFactory";
-}
-
-// Frames
-class TimeStamp;
-} // namespace wolf
-#include "base/frame_base.h"
-namespace wolf{
-typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
-template<>
-inline std::string FrameFactory::getClass()
-{
- return "FrameFactory";
-}
-
-//#define UNUSED(x) (void)x;
-//#define UNUSED(x) (void)(sizeof((x), 0));
-
-#ifdef __GNUC__
- #define WOLF_UNUSED __attribute__((used))
-#elif defined _MSC_VER
- #pragma warning(disable: Cxxxxx)
- #define WOLF_UNUSED
-#elif defined(__LCLINT__)
-# define WOLF_UNUSED /*@unused@*/
-#elif defined(__cplusplus)
-# define WOLF_UNUSED
-#else
-# define UNUSED(x) x
-#endif
-
-#define WOLF_REGISTER_FRAME(FrameType, FrameName) \
- namespace{ const bool WOLF_UNUSED FrameName##Registered = \
- wolf::FrameFactory::get().registerCreator(FrameType, FrameName::create); }\
-
-} /* namespace wolf */
-
-#endif /* FACTORY_H_ */
diff --git a/include/base/feature/feature_GPS_fix.h b/include/base/feature/feature_GPS_fix.h
deleted file mode 100644
index d694465480d2dbc47eb886c27ed0444ca47e7616..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_GPS_fix.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef FEATURE_GPS_FIX_H_
-#define FEATURE_GPS_FIX_H_
-
-//Wolf includes
-#include "base/factor/factor_GPS_2D.h"
-#include "base/feature/feature_base.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureGPSFix);
-
-//class FeatureGPSFix
-class FeatureGPSFix : public FeatureBase
-{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureGPSFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
- virtual ~FeatureGPSFix();
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_GPS_pseudorange.h b/include/base/feature/feature_GPS_pseudorange.h
deleted file mode 100644
index 57f327e2520b94827a222e0b80703a24c3049c01..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_GPS_pseudorange.h
+++ /dev/null
@@ -1,36 +0,0 @@
-
-#ifndef FEATURE_GPS_PSEUDORANGE_H_
-#define FEATURE_GPS_PSEUDORANGE_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-//std includes
-#include <iomanip>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureGPSPseudorange);
-
-// TODO manage covariance
-class FeatureGPSPseudorange : public FeatureBase
-{
-protected:
- Eigen::Vector3s sat_position_;
- Scalar pseudorange_;
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- FeatureGPSPseudorange(Eigen::Vector3s& _sat_position, Scalar _pseudorange, Scalar _covariance);
- virtual ~FeatureGPSPseudorange();
-
- const Eigen::Vector3s & getSatPosition() const;
-
- Scalar getPseudorange() const;
-
-};
-
-} // namespace wolf
-
-#endif //FEATURE_GPS_PSEUDORANGE_H_
diff --git a/include/base/feature/feature_IMU.h b/include/base/feature/feature_IMU.h
deleted file mode 100644
index dfce11ff78a8e974731efef729c17b0fa72a9fe8..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_IMU.h
+++ /dev/null
@@ -1,76 +0,0 @@
-#ifndef FEATURE_IMU_H_
-#define FEATURE_IMU_H_
-
-//Wolf includes
-#include "base/capture/capture_IMU.h"
-#include "base/feature/feature_base.h"
-#include "base/wolf.h"
-
-//std includes
-
-namespace wolf {
-
-//WOLF_PTR_TYPEDEFS(CaptureIMU);
-WOLF_PTR_TYPEDEFS(FeatureIMU);
-
-class FeatureIMU : public FeatureBase
-{
- public:
-
- /** \brief Constructor from and measures
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
- * \param acc_bias accelerometer bias of origin frame
- * \param gyro_bias gyroscope bias of origin frame
- * \param _cap_imu_ptr pointer to parent CaptureMotion
- */
- FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::Vector6s& _bias,
- const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
- CaptureMotionPtr _cap_imu_ptr = nullptr);
-
- /** \brief Constructor from capture pointer
- *
- * \param _cap_imu_ptr pointer to parent CaptureMotion
- */
- FeatureIMU(CaptureMotionPtr _cap_imu_ptr);
-
- virtual ~FeatureIMU();
-
- const Eigen::Vector3s& getAccBiasPreint() const;
- const Eigen::Vector3s& getGyroBiasPreint() const;
- const Eigen::Matrix<Scalar, 9, 6>& getJacobianBias() const;
-
- private:
-
- // Used biases
- Eigen::Vector3s acc_bias_preint_; ///< Acceleration bias used for delta preintegration
- Eigen::Vector3s gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
-
- Eigen::Matrix<Scalar, 9, 6> jacobian_bias_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-};
-
-inline const Eigen::Vector3s& FeatureIMU::getAccBiasPreint() const
-{
- return acc_bias_preint_;
-}
-
-inline const Eigen::Vector3s& FeatureIMU::getGyroBiasPreint() const
-{
- return gyro_bias_preint_;
-}
-
-inline const Eigen::Matrix<Scalar, 9, 6>& FeatureIMU::getJacobianBias() const
-{
- return jacobian_bias_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
deleted file mode 100644
index 3732bc55247d9d39b90992a07afe7bb04d7f7681..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_base.h
+++ /dev/null
@@ -1,165 +0,0 @@
-#ifndef FEATURE_BASE_H_
-#define FEATURE_BASE_H_
-
-// Forward declarations for node templates
-namespace wolf{
-class CaptureBase;
-class FactorBase;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-
-//std includes
-
-namespace wolf {
-
-//class FeatureBase
-class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
-{
- private:
- CaptureBaseWPtr capture_ptr_;
- FactorBasePtrList factor_list_;
- FactorBasePtrList constrained_by_list_;
-
- static unsigned int feature_id_count_;
-
- protected:
- unsigned int feature_id_;
- unsigned int track_id_; // ID of the feature track
- unsigned int landmark_id_; // ID of the landmark
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_upper_; ///< the squared root information matrix
- Eigen::VectorXs expectation_; ///< expectation
-
- public:
-
- /** \brief Constructor from capture pointer and measure
- * \param _tp type of feature -- see wolf.h
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- */
- FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
-
- virtual ~FeatureBase();
- virtual void remove();
-
- virtual void setProblem(ProblemPtr _problem) final;
-
- // properties
- unsigned int id();
- unsigned int trackId(){return track_id_;}
- void setTrackId(unsigned int _tr_id){track_id_ = _tr_id;}
- unsigned int landmarkId(){return landmark_id_;}
- void setLandmarkId(unsigned int _lmk_id){landmark_id_ = _lmk_id;}
-
- // values
- /* \brief Returns _ii component of measurement vector
- *
- * WARNING: To be fast, it does not check that index _ii is smaller than dimension.
- */
- Scalar getMeasurement(unsigned int _ii) const;
- const Eigen::VectorXs& getMeasurement() const;
- void setMeasurement(const Eigen::VectorXs& _meas);
- void setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov);
- void setMeasurementInformation(const Eigen::MatrixXs & _meas_info);
- const Eigen::MatrixXs& getMeasurementCovariance() const;
- Eigen::MatrixXs getMeasurementInformation() const;
- const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
-
- const Eigen::VectorXs& getExpectation() const;
- void setExpectation(const Eigen::VectorXs& expectation);
-
- // wolf tree access
- FrameBasePtr getFrame() const;
-
- CaptureBasePtr getCapture() const;
- void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
-
- FactorBasePtr addFactor(FactorBasePtr _co_ptr);
- FactorBasePtrList& getFactorList();
-
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
-
- // all factors
- void getFactorList(FactorBasePtrList & _fac_list);
-
- private:
- Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
-};
-
-}
-
-// IMPLEMENTATION
-
-#include "base/factor/factor_base.h"
-
-namespace wolf{
-
-inline unsigned int FeatureBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
-inline FactorBasePtrList& FeatureBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
-
-inline unsigned int FeatureBase::id()
-{
- return feature_id_;
-}
-
-inline CaptureBasePtr FeatureBase::getCapture() const
-{
- return capture_ptr_.lock();
-}
-
-inline Scalar FeatureBase::getMeasurement(unsigned int _ii) const
-{
- return measurement_(_ii);
-}
-
-inline const Eigen::VectorXs& FeatureBase::getMeasurement() const
-{
- return measurement_;
-}
-
-inline const Eigen::MatrixXs& FeatureBase::getMeasurementCovariance() const
-{
- return measurement_covariance_;
-}
-
-inline Eigen::MatrixXs FeatureBase::getMeasurementInformation() const
-{
- return measurement_sqrt_information_upper_.transpose() * measurement_sqrt_information_upper_;
-}
-
-inline const Eigen::MatrixXs& FeatureBase::getMeasurementSquareRootInformationUpper() const
-{
- return measurement_sqrt_information_upper_;
-}
-
-inline void FeatureBase::setMeasurement(const Eigen::VectorXs& _meas)
-{
- measurement_ = _meas;
-}
-
-inline const Eigen::VectorXs& FeatureBase::getExpectation() const
-{
- return expectation_;
-}
-
-inline void FeatureBase::setExpectation(const Eigen::VectorXs& expectation)
-{
- expectation_ = expectation;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_corner_2D.h b/include/base/feature/feature_corner_2D.h
deleted file mode 100644
index 8e206749db1261edc254e9fd09eeb9cde60337cc..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_corner_2D.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef FEATURE_CORNER_2D_H_
-#define FEATURE_CORNER_2D_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureCorner2D);
-
-//class FeatureCorner2D
-class FeatureCorner2D : public FeatureBase
-{
- public:
-
- FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance);
- virtual ~FeatureCorner2D();
-
- /** \brief Returns aperture
- *
- * Returns aperture
- *
- **/
- Scalar getAperture() const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_diff_drive.h b/include/base/feature/feature_diff_drive.h
deleted file mode 100644
index 52877f4f5e134d444951c7b1667f67b6d02711fe..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_diff_drive.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/**
- * \file feature_diff_drive.h
- *
- * Created on: Oct 27, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_FEATURE_DIFF_DRIVE_H_
-#define _WOLF_FEATURE_DIFF_DRIVE_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
-
-class FeatureDiffDrive : public FeatureBase
-{
-public:
-
- FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::VectorXs& _diff_drive_factors,
- const Eigen::MatrixXs& _jacobian_diff_drive_factors);
-
- virtual ~FeatureDiffDrive() = default;
-
- const Eigen::MatrixXs& getJacobianFactor() const;
-
-protected:
-
- Eigen::VectorXs diff_drive_factors_;
- Eigen::MatrixXs jacobian_diff_drive_factors_;
-};
-
-} /* namespace wolf */
-
-#endif /* _WOLF_FEATURE_DIFF_DRIVE_H_ */
diff --git a/include/base/feature/feature_line_2D.h b/include/base/feature/feature_line_2D.h
deleted file mode 100644
index 5d5e6162dc84217058f520676b768c24dddb671d..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_line_2D.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#ifndef FEATURE_LINE_2D_H_
-#define FEATURE_LINE_2D_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureLine2D);
-
-/** \brief class FeatureLine2D
- *
- * Line segment feature.
- *
- * Measurement is a 3-vector of its homogeneous coordinates,
- * normalized according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
- *
- * Descriptor are the homogeneous coordinates of its first and last point, which may or may not be extreme points of the line
- *
- **/
-class FeatureLine2D : public FeatureBase
-{
- protected:
- Eigen::Vector3s first_point_;
- Eigen::Vector3s last_point_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- /** \brief Constructor
- *
- * Constructor, with measurement and descriptor
- *
- */
- FeatureLine2D(const Eigen::Vector3s & _line_homogeneous_params, const Eigen::Matrix3s & _params_covariance,
- Eigen::Vector3s & _point1, Eigen::Vector3s & _point2);
-
- virtual ~FeatureLine2D();
-
- /** \brief Returns a const reference to first_point_
- */
- const Eigen::Vector3s & firstPoint() const;
-
- /** \brief Returns a const reference to last_point_
- */
- const Eigen::Vector3s & lastPoint() const;
-
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_match.h b/include/base/feature/feature_match.h
deleted file mode 100644
index b96ce00239f1fb6293e3dd10b24e6acd345ae8cf..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_match.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef FEATURE_MATCH_H_
-#define FEATURE_MATCH_H_
-
-// Wolf includes
-#include "base/wolf.h"
-
-//wolf nampseace
-namespace wolf {
-
-//forward declaration to typedef class pointers
-WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatch);
-
-/** \brief Map of feature matches
- *
- * corr feature <-------------- actual feature
- *
- * map<FeatureBasePtr actual_feature, FeatureMatchPtr corresponding_feature_match>
- *
- */
-typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is also typedefined
-
-/** \brief Match between a feature and a feature
- *
- * Match between a feature and a feature (feature-feature correspondence)
- *
- */
-struct FeatureMatch
-{
- FeatureBasePtr feature_ptr_; ///< Corresponding feature
- Scalar normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
-};
-
-}//end namespace
-
-#endif
-
diff --git a/include/base/feature/feature_match_polyline_2D.h b/include/base/feature/feature_match_polyline_2D.h
deleted file mode 100644
index 6e3a6752831a448b456b7efa57442fb6969f9218..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_match_polyline_2D.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef FEATURE_MATCH_POLYLINE_2D_H_
-#define FEATURE_MATCH_POLYLINE_2D_H_
-
-// Wolf includes
-#include "base/wolf.h"
-#include "base/feature/feature_match.h"
-
-//wolf nampseace
-namespace wolf {
-
-//forward declaration to typedef class pointers
-WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatchPolyline2D);
-
-/** \brief Match between a feature_polyline_2D and a feature_polyline_2D
- *
- * Match between a feature_polyline_2D and a feature_polyline_2D (feature-feature correspondence)
- *
- */
-struct FeatureMatchPolyline2D : public FeatureMatch
-{
- int feature_last_from_id_;
- int feature_last_to_id_;
- int feature_incoming_from_id_;
- int feature_incoming_to_id_;
- Eigen::Matrix3s T_incoming_last_;
-};
-
-}//end namespace
-
-#endif
-
diff --git a/include/base/feature/feature_motion.h b/include/base/feature/feature_motion.h
deleted file mode 100644
index 92331b6b1d7f41bbb342c4ef528b64f488fc1c48..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_motion.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * feature_motion.h
- *
- * Created on: Aug 11, 2017
- * Author: jsola
- */
-
-#ifndef FEATURE_MOTION_H_
-#define FEATURE_MOTION_H_
-
-#include "base/feature/feature_base.h"
-#include "base/capture/capture_motion.h"
-
-namespace wolf
-{
-
-class FeatureMotion : public FeatureBase
-{
- public:
- FeatureMotion(const std::string& _type, const CaptureMotionPtr& _capture_motion);
- virtual ~FeatureMotion();
-
- Eigen::VectorXs getCalibrationPreint();
- Eigen::MatrixXs getJacobianCalibration();
-
- Eigen::VectorXs computeDeltaCorrection(const Eigen::VectorXs& _calib) const;
- Eigen::VectorXs getDeltaCorrected(const Eigen::VectorXs& _calib) const;
-
- virtual Eigen::VectorXs correctDelta(const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta_correction) const = 0;
-
- private:
- Eigen::VectorXs& calib_preint_;
- Eigen::MatrixXs& jacobian_calib_;
-};
-
-inline Eigen::VectorXs FeatureMotion::getCalibrationPreint()
-{
- return calib_preint_;
-}
-
-inline Eigen::MatrixXs FeatureMotion::getJacobianCalibration()
-{
- return jacobian_calib_;
-}
-
-inline Eigen::VectorXs FeatureMotion::computeDeltaCorrection(const Eigen::VectorXs& _calib) const
-{
- return jacobian_calib_ * (_calib - calib_preint_);
-}
-
-inline Eigen::VectorXs FeatureMotion::getDeltaCorrected(const Eigen::VectorXs& _calib) const
-{
- // delta_preint is stored in FeatureBase::measurement_;
- Eigen::VectorXs delta_step = computeDeltaCorrection(_calib);
- return correctDelta(measurement_, delta_step);
-}
-
-} /* namespace wolf */
-
-#endif /* FEATURE_MOTION_H_ */
diff --git a/include/base/feature/feature_odom_2D.h b/include/base/feature/feature_odom_2D.h
deleted file mode 100644
index 27db3e538b7dfd8172a6f4a815db21ca31969e32..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_odom_2D.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef FEATURE_ODOM_2D_H_
-#define FEATURE_ODOM_2D_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-#include "base/factor/factor_odom_2D.h"
-#include "base/factor/factor_odom_2D_analytic.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureOdom2D);
-
-//class FeatureOdom2D
-class FeatureOdom2D : public FeatureBase
-{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
-
- virtual ~FeatureOdom2D();
-
- /** \brief Generic interface to find factors
- *
- * TBD
- * Generic interface to find factors between this feature and a map (static/slam) or a previous feature
- *
- **/
- virtual void findFactors();
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_point_image.h b/include/base/feature/feature_point_image.h
deleted file mode 100644
index aa07d80dfd836d51440383bea1acf7ab5cea0451..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_point_image.h
+++ /dev/null
@@ -1,137 +0,0 @@
-#ifndef FEATURE_IMAGE_H
-#define FEATURE_IMAGE_H
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-// Vision Utils includes
-#include <vision_utils.h>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeaturePointImage);
-
-//class FeaturePointImage
-class FeaturePointImage : public FeatureBase
-{
- private:
- cv::KeyPoint keypoint_; ///< Warning: every write operation to this member needs to write measurement_. See setKeypoint() as an example.
- unsigned int index_keypoint_;
- cv::Mat descriptor_;
- bool is_known_;
- Scalar score_;
- cv::Rect tracker_roi_;
-
- public:
-
- /// Constructor from Eigen measured pixel
- FeaturePointImage(const Eigen::Vector2s & _measured_pixel,
- const unsigned int& _index_keypoint,
- const cv::Mat& _descriptor,
- const Eigen::Matrix2s& _meas_covariance) :
- FeatureBase("POINT IMAGE", _measured_pixel, _meas_covariance),
- keypoint_(_measured_pixel(0), _measured_pixel(1), 1), // Size 1 is a dummy value
- index_keypoint_(_index_keypoint),
- descriptor_( _descriptor),
- is_known_(false),
- score_(0)
- {
- keypoint_.pt.x = measurement_(0);
- keypoint_.pt.y = measurement_(1);
- }
-
- /// Constructor from OpenCV measured keypoint
- FeaturePointImage(const cv::KeyPoint& _keypoint,
- const unsigned int& _index_keypoint,
- const cv::Mat& _descriptor,
- const Eigen::Matrix2s& _meas_covariance) :
- FeatureBase("POINT IMAGE", Eigen::Vector2s::Zero(), _meas_covariance),
- keypoint_(_keypoint),
- index_keypoint_(_index_keypoint),
- descriptor_(_descriptor),
- is_known_(false),
- score_(0)
- {
- measurement_(0) = Scalar(_keypoint.pt.x);
- measurement_(1) = Scalar(_keypoint.pt.y);
- }
-
- virtual ~FeaturePointImage();
-
- const cv::KeyPoint& getKeypoint() const;
- void setKeypoint(const cv::KeyPoint& _kp);
-
- const cv::Mat& getDescriptor() const;
- void setDescriptor(const cv::Mat& _descriptor);
-
- SizeStd getIndexKeyPoint() const
- { return index_keypoint_; }
-
- bool isKnown();
- void setIsKnown(bool _is_known);
-
- /*Eigen::VectorXs & getMeasurement(){
- measurement_(0) = Scalar(keypoint_.pt.x);
- measurement_(1) = Scalar(keypoint_.pt.y);
- return measurement_;
- }*/
-
- const cv::Rect& getTrackerRoi() const;
- void setTrackerRoi(const cv::Rect& tracker_roi);
-
- Scalar getScore() const
- {
- return score_;
- }
-
- void setScore(Scalar score)
- {
- score_ = score;
- }
-};
-
-inline const cv::KeyPoint& FeaturePointImage::getKeypoint() const
-{
- return keypoint_;
-}
-
-inline void FeaturePointImage::setKeypoint(const cv::KeyPoint& _kp)
-{
- keypoint_ = _kp;
- measurement_(0) = _kp.pt.x;
- measurement_(1) = _kp.pt.y;
-}
-
-inline const cv::Mat& FeaturePointImage::getDescriptor() const
-{
- return descriptor_;
-}
-
-inline void FeaturePointImage::setDescriptor(const cv::Mat& _descriptor)
-{
- descriptor_ = _descriptor;
-}
-
-inline bool FeaturePointImage::isKnown()
-{
- return is_known_;
-}
-
-inline void FeaturePointImage::setIsKnown(bool _is_known)
-{
- is_known_ = _is_known;
-}
-
-inline const cv::Rect& FeaturePointImage::getTrackerRoi() const
-{
- return tracker_roi_;
-}
-
-inline void FeaturePointImage::setTrackerRoi(const cv::Rect& tracker_roi)
-{
- tracker_roi_ = tracker_roi;
-}
-
-} // namespace wolf
-
-#endif // FEATURE_IMAGE_H
diff --git a/include/base/feature/feature_polyline_2D.h b/include/base/feature/feature_polyline_2D.h
deleted file mode 100644
index 071f46be51102ca3384cc1ba967d3817e4f8731e..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_polyline_2D.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * \file feature_polyline_2D.h
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#ifndef FEATURE_POLYLINE_2D_H_
-#define FEATURE_POLYLINE_2D_H_
-
-#include "base/feature/feature_base.h"
-
-namespace wolf
-{
-WOLF_PTR_TYPEDEFS(FeaturePolyline2D);
-WOLF_LIST_TYPEDEFS(FeaturePolyline2D);
-
-//class
-class FeaturePolyline2D : public FeatureBase
-{
- protected:
- Eigen::MatrixXs points_; // homogeneous 2D points (each column is a point)
- Eigen::MatrixXs points_cov_;
- bool first_defined_;
- bool last_defined_;
-
- public:
- FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined);
- virtual ~FeaturePolyline2D();
- const Eigen::MatrixXs& getPoints() const;
- const Eigen::MatrixXs& getPointsCov() const;
- bool isFirstDefined() const;
- bool isLastDefined() const;
- int getNPoints() const;
-};
-
-inline FeaturePolyline2D::FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined) :
- FeatureBase("POLYLINE 2D", Eigen::Vector1s::Zero(), Eigen::Vector1s::Ones()),
- points_(_points),
- points_cov_(_points_cov),
- first_defined_(_first_defined),
- last_defined_(_last_defined)
-{
- assert(points_.rows() == 3 && points_cov_.rows() == 2 && points_cov_.cols() == 2*points_.cols() && "FeaturePolyline2D::FeaturePolyline2D: Bad points or covariance matrix size");
-}
-
-inline FeaturePolyline2D::~FeaturePolyline2D()
-{
- //
-}
-
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPoints() const
-{
- return points_;
-}
-
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPointsCov() const
-{
- return points_cov_;
-}
-
-inline bool FeaturePolyline2D::isFirstDefined() const
-{
- return first_defined_;
-}
-
-inline bool FeaturePolyline2D::isLastDefined() const
-{
- return last_defined_;
-}
-
-inline int FeaturePolyline2D::getNPoints() const
-{
- return points_.cols();
-}
-
-} /* namespace wolf */
-
-#endif /* FEATURE_POLYLINE_2D_H_ */
diff --git a/include/base/feature/feature_pose.h b/include/base/feature/feature_pose.h
deleted file mode 100644
index 1431f920765c2450a2d71d02b3d6adf4e95700e7..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_pose.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef FEATURE_POSE_H_
-#define FEATURE_POSE_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-//std includes
-//
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeaturePose);
-
-//class FeaturePose
-class FeaturePose : public FeatureBase
-{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
- virtual ~FeaturePose();
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
deleted file mode 100644
index 0177bd8e9253dbd9840ec8fe3a765720a0754fc6..0000000000000000000000000000000000000000
--- a/include/base/frame_base.h
+++ /dev/null
@@ -1,275 +0,0 @@
-
-#ifndef FRAME_BASE_H_
-#define FRAME_BASE_H_
-
-// Fwd refs
-namespace wolf{
-class TrajectoryBase;
-class CaptureBase;
-class StateBlock;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/time_stamp.h"
-#include "base/node_base.h"
-
-//std includes
-
-namespace wolf {
-
-/** \brief Enumeration of frame types: key-frame or non-key-frame
- */
-typedef enum
-{
- NON_KEY_FRAME = 0, ///< Regular frame. It does not play at optimization.
- KEY_FRAME = 1 ///< key frame. It plays at optimizations.
-} FrameType;
-
-//class FrameBase
-class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
-{
- private:
- TrajectoryBaseWPtr trajectory_ptr_;
- CaptureBasePtrList capture_list_;
- FactorBasePtrList constrained_by_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
-
- static unsigned int frame_id_count_;
-
- protected:
- unsigned int frame_id_;
- FrameType type_; ///< type of frame. Either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h)
- TimeStamp time_stamp_; ///< frame time stamp
-
- public:
- /** \brief Constructor of non-key Frame with only time stamp
- *
- * Constructor with only time stamp
- * \param _ts is the time stamp associated to this frame, provided in seconds
- * \param _p_ptr StateBlock pointer to the position (default: nullptr)
- * \param _o_ptr StateBlock pointer to the orientation (default: nullptr). Pass a StateQuaternion if needed.
- * \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
- **/
- FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
-
- /** \brief Constructor with type, time stamp and state pointer
- *
- * Constructor with type, time stamp and state pointer
- * \param _tp indicates frame type. Generally either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _ts is the time stamp associated to this frame, provided in seconds
- * \param _p_ptr StateBlock pointer to the position (default: nullptr)
- * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
- **/
- FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
-
- virtual ~FrameBase();
- virtual void remove();
-
- // Frame properties -----------------------------------------------
- public:
- unsigned int id();
-
- // KeyFrame / NonKeyFrame
- bool isKey() const;
- void setKey();
-
- // Frame values ------------------------------------------------
- public:
- void setTimeStamp(const TimeStamp& _ts);
- TimeStamp getTimeStamp() const;
- void getTimeStamp(TimeStamp& _ts) const;
-
- // State blocks
- public:
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
- protected:
- StateBlockPtr getStateBlock(unsigned int _i) const;
- void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
- void resizeStateBlockVec(unsigned int _size);
-
- public:
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
- StateBlockPtr getV() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
- void setV(const StateBlockPtr _v_ptr);
- void registerNewStateBlocks();
- void removeStateBlocks();
-
- // Fixed / Estimated
- public:
- void fix();
- void unfix();
- bool isFixed() const;
-
- // States
- public:
- void setState(const Eigen::VectorXs& _state);
- Eigen::VectorXs getState() const;
- void getState(Eigen::VectorXs& _state) const;
- unsigned int getSize() const;
- bool getCovariance(Eigen::MatrixXs& _cov) const;
-
- // Wolf tree access ---------------------------------------------------
- public:
- virtual void setProblem(ProblemPtr _problem) final;
-
- TrajectoryBasePtr getTrajectory() const;
- void setTrajectory(TrajectoryBasePtr _trj_ptr);
-
- FrameBasePtr getPreviousFrame() const;
- FrameBasePtr getNextFrame() const;
-
- CaptureBasePtrList& getCaptureList();
- CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
- CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr);
- CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
- CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
- void unlinkCapture(CaptureBasePtr _cap_ptr);
-
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
-
- void getFactorList(FactorBasePtrList& _fac_list);
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
-
- public:
- static FrameBasePtr create_PO_2D (const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(3));
- static FrameBasePtr create_PO_3D (const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(7));
- static FrameBasePtr create_POV_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(10));
-};
-
-} // namespace wolf
-
-// IMPLEMENTATION //
-
-#include "base/trajectory_base.h"
-#include "base/capture/capture_base.h"
-#include "base/factor/factor_base.h"
-#include "base/state_block.h"
-
-namespace wolf {
-
-inline unsigned int FrameBase::id()
-{
- return frame_id_;
-}
-
-inline bool FrameBase::isKey() const
-{
- return (type_ == KEY_FRAME);
-}
-
-inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
-{
- _ts = time_stamp_;
-}
-
-inline TimeStamp FrameBase::getTimeStamp() const
-{
- return time_stamp_;
-}
-
-inline const std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline StateBlockPtr FrameBase::getP() const
-{
- return state_block_vec_[0];
-}
-inline void FrameBase::setP(const StateBlockPtr _p_ptr)
-{
- state_block_vec_[0] = _p_ptr;
-}
-
-inline StateBlockPtr FrameBase::getO() const
-{
- return state_block_vec_[1];
-}
-inline void FrameBase::setO(const StateBlockPtr _o_ptr)
-{
- state_block_vec_[1] = _o_ptr;
-}
-
-inline StateBlockPtr FrameBase::getV() const
-{
- return state_block_vec_[2];
-}
-
-inline void FrameBase::setV(const StateBlockPtr _v_ptr)
-{
- state_block_vec_[2] = _v_ptr;
-}
-
-inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
-{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
-}
-
-inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
-{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- state_block_vec_[_i] = _sb_ptr;
-}
-
-inline TrajectoryBasePtr FrameBase::getTrajectory() const
-{
- return trajectory_ptr_.lock();
-}
-
-inline CaptureBasePtrList& FrameBase::getCaptureList()
-{
- return capture_list_;
-}
-
-inline void FrameBase::resizeStateBlockVec(unsigned int _size)
-{
- if (_size > state_block_vec_.size())
- state_block_vec_.resize(_size);
-}
-
-inline unsigned int FrameBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
-inline void FrameBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto cap : capture_list_)
- cap->setProblem(_problem);
-}
-
-inline FactorBasePtrList& FrameBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
-
-inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
-{
- trajectory_ptr_ = _trj_ptr;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/hardware_base.h b/include/base/hardware_base.h
deleted file mode 100644
index 6dd60648b3961dd8f88244b240eebf7c4d88e37d..0000000000000000000000000000000000000000
--- a/include/base/hardware_base.h
+++ /dev/null
@@ -1,43 +0,0 @@
-#ifndef HARDWARE_BASE_H_
-#define HARDWARE_BASE_H_
-
-// Fwd dependencies
-namespace wolf{
-class Problem;
-class SensorBase;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-
-namespace wolf {
-
-//class HardwareBase
-class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase>
-{
- private:
- SensorBasePtrList sensor_list_;
-
- public:
- HardwareBase();
- virtual ~HardwareBase();
-
- virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
- SensorBasePtrList& getSensorList();
-};
-
-} // namespace wolf
-
-// IMPLEMENTATION
-
-namespace wolf {
-
-inline SensorBasePtrList& HardwareBase::getSensorList()
-{
- return sensor_list_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/landmark/landmark_AHP.h b/include/base/landmark/landmark_AHP.h
deleted file mode 100644
index a706f87dadcfa938d4ec0666a2d1a241ff40c253..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_AHP.h
+++ /dev/null
@@ -1,89 +0,0 @@
-#ifndef LANDMARK_AHP_H
-#define LANDMARK_AHP_H
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-
-// yaml
-#include <yaml-cpp/yaml.h>
-
-// Vision utils
-#include <vision_utils/vision_utils.h>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkAHP);
-
-/* Landmark - Anchored Homogeneous Point*/
-class LandmarkAHP : public LandmarkBase
-{
- protected:
- cv::Mat cv_descriptor_;
- FrameBasePtr anchor_frame_;
- SensorBasePtr anchor_sensor_;
-
- public:
- LandmarkAHP(Eigen::Vector4s _position_homogeneous, FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor, cv::Mat _2D_descriptor);
-
- virtual ~LandmarkAHP();
-
- const cv::Mat& getCvDescriptor() const;
- void setCvDescriptor(const cv::Mat& _descriptor);
-
- const FrameBasePtr getAnchorFrame () const;
- const SensorBasePtr getAnchorSensor() const;
-
- void setAnchorFrame (FrameBasePtr _anchor_frame );
- void setAnchorSensor (SensorBasePtr _anchor_sensor);
- void setAnchor (FrameBasePtr _anchor_frame , SensorBasePtr _anchor_sensor);
- Eigen::Vector3s getPointInAnchorSensor() const;
- Eigen::Vector3s point() const;
-
- YAML::Node saveToYaml() const;
-
- /** \brief Creator for Factory<LandmarkBase, YAML::Node>
- * Caution: This creator does not set the landmark's anchor frame and sensor.
- * These need to be set afterwards.
- */
- static LandmarkBasePtr create(const YAML::Node& _node);
-};
-
-inline const cv::Mat& LandmarkAHP::getCvDescriptor() const
-{
- return cv_descriptor_;
-}
-
-inline void LandmarkAHP::setCvDescriptor(const cv::Mat& _descriptor)
-{
- cv_descriptor_ = _descriptor;
-}
-
-inline const FrameBasePtr LandmarkAHP::getAnchorFrame() const
-{
- return anchor_frame_;
-}
-
-inline void LandmarkAHP::setAnchorFrame(FrameBasePtr _anchor_frame)
-{
- anchor_frame_ = _anchor_frame;
-}
-
-inline const SensorBasePtr LandmarkAHP::getAnchorSensor() const
-{
- return anchor_sensor_;
-}
-
-inline void LandmarkAHP::setAnchorSensor(SensorBasePtr _anchor_sensor)
-{
- anchor_sensor_ = _anchor_sensor;
-}
-
-inline void LandmarkAHP::setAnchor(FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor)
-{
- anchor_frame_ = _anchor_frame;
- anchor_sensor_ = _anchor_sensor;
-}
-
-} // namespace wolf
-
-#endif // LANDMARK_AHP_H
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
deleted file mode 100644
index 2db5480f8d0901510219a4a847dd86505a2c06ee..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_base.h
+++ /dev/null
@@ -1,208 +0,0 @@
-#ifndef LANDMARK_BASE_H_
-#define LANDMARK_BASE_H_
-
-// Fwd references
-namespace wolf{
-class MapBase;
-class StateBlock;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
-
-//std includes
-
-// yaml
-#include "yaml-cpp/yaml.h"
-
-namespace wolf {
-
-//class LandmarkBase
-class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
-{
- private:
- MapBaseWPtr map_ptr_;
- FactorBasePtrList constrained_by_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O.
-
- static unsigned int landmark_id_count_;
-
- protected:
- unsigned int landmark_id_; ///< landmark unique id
- TimeStamp stamp_; ///< stamp of the creation of the landmark
- Eigen::VectorXs descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
-
- public:
-
- /** \brief Constructor with type, time stamp and the position state pointer (optional orientation state pointer)
- *
- * Constructor with type, and state pointer
- * \param _tp indicates landmark type.(types defined at wolf.h)
- * \param _p_ptr StateBlock pointer to the position
- * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
- *
- **/
- LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
- virtual ~LandmarkBase();
- virtual void remove();
- virtual YAML::Node saveToYaml() const;
-
- // Properties
- unsigned int id();
- void setId(unsigned int _id);
-
- // Fix / unfix
- void fix();
- void unfix();
- bool isFixed() const;
-
- // is removing
- bool isRemoving() const;
-
- // State blocks
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
- std::vector<StateBlockPtr> getUsedStateBlockVec() const;
- StateBlockPtr getStateBlock(unsigned int _i) const;
- void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
- virtual void registerNewStateBlocks();
- Eigen::VectorXs getState() const;
- void getState(Eigen::VectorXs& _state) const;
- bool getCovariance(Eigen::MatrixXs& _cov) const;
-
- protected:
- virtual void removeStateBlocks();
-
- // Descriptor
- public:
- const Eigen::VectorXs& getDescriptor() const;
- Scalar getDescriptor(unsigned int _ii) const;
- void setDescriptor(const Eigen::VectorXs& _descriptor);
-
- // Navigate wolf tree
- virtual void setProblem(ProblemPtr _problem) final;
-
- FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
-
- void setMap(const MapBasePtr _map_ptr);
- MapBasePtr getMap();
-
-};
-
-}
-
-#include "base/map_base.h"
-#include "base/factor/factor_base.h"
-#include "base/state_block.h"
-
-namespace wolf{
-
-inline void LandmarkBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
-}
-
-inline MapBasePtr LandmarkBase::getMap()
-{
- return map_ptr_.lock();
-}
-
-inline void LandmarkBase::setMap(const MapBasePtr _map_ptr)
-{
- map_ptr_ = _map_ptr;
-}
-
-inline unsigned int LandmarkBase::id()
-{
- return landmark_id_;
-}
-
-inline void LandmarkBase::setId(unsigned int _id)
-{
- landmark_id_ = _id;
- if (_id > landmark_id_count_)
- landmark_id_count_ = _id;
-}
-
-inline bool LandmarkBase::isRemoving() const
-{
- return is_removing_;
-}
-
-inline unsigned int LandmarkBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
-inline FactorBasePtrList& LandmarkBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
-
-inline const std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
-{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
-}
-
-inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
-{
- state_block_vec_[_i] = _sb_ptr;
-}
-
-inline StateBlockPtr LandmarkBase::getP() const
-{
- return getStateBlock(0);
-}
-
-inline StateBlockPtr LandmarkBase::getO() const
-{
- return getStateBlock(1);
-}
-
-inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
-{
- setStateBlock(0, _st_ptr);
-}
-
-inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
-{
- setStateBlock(1, _st_ptr);
-}
-
-inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
-{
- descriptor_ = _descriptor;
-}
-
-inline Scalar LandmarkBase::getDescriptor(unsigned int _ii) const
-{
- assert(_ii < descriptor_.size() && "LandmarkBase::getDescriptor: bad index");
- return descriptor_(_ii);
-}
-
-inline const Eigen::VectorXs& LandmarkBase::getDescriptor() const
-{
- return descriptor_;
-}
-
-} // namespace wolf
-#endif
diff --git a/include/base/landmark/landmark_container.h b/include/base/landmark/landmark_container.h
deleted file mode 100644
index 65a2c5b710ceb0f42bb9984fee0e8d0a967348a4..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_container.h
+++ /dev/null
@@ -1,107 +0,0 @@
-
-#ifndef LANDMARK_CONTAINER_H_
-#define LANDMARK_CONTAINER_H_
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-#include "base/wolf.h"
-
-// Std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkContainer);
-
-//class LandmarkContainer
-class LandmarkContainer : public LandmarkBase
-{
- protected:
- Eigen::MatrixXs corners_;
-
- public:
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock pointer to the position
- * \param _o_ptr StateBlock pointer to the orientation
- * \param _witdh descriptor of the landmark: container width
- * \param _length descriptor of the landmark: container length
- *
- **/
- LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
-
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * The vertices are refered as A, B, C and D:
- *
- * B ---------------------------- A
- * | |
- * | <---+ |
- * | | |
- * | v |
- * C ---------------------------- D
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock pointer to the position
- * \param _o_ptr StateBlock pointer to the orientation
- * \param _corner_1_pose pose of corner 1
- * \param _corner_2_pose pose of corner 2
- * \param _corner_1_idx index of corner 1 (A = 0, B = 1, C = 2, D = 3)
- * \param _corner_2_idx index of corner 2 (A = 0, B = 1, C = 2, D = 3)
- * \param _witdh descriptor of the landmark: container width
- * \param _length descriptor of the landmark: container length
- *
- **/
- LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
-
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock pointer to the position
- * \param _o_ptr StateBlock pointer to the orientation
- * \param _corner_A_ptr LandmarkCorner2D pointer to one of its corners
- * \param _corner_B_ptr LandmarkCorner2D pointer to one of its corners
- * \param _corner_C_ptr LandmarkCorner2D pointer to one of its corners
- * \param _corner_D_ptr LandmarkCorner2D pointer to one of its corners
- * \param _witdh descriptor of the landmark: container width
- * \param _length descriptor of the landmark: container length
- *
- **/
- //LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
-
- virtual ~LandmarkContainer();
-
- /** \brief Returns the container width
- *
- * Returns the container width
- *
- **/
- Scalar getWidth() const;
-
- /** \brief Returns the container length
- *
- * Returns the container length
- *
- **/
- Scalar getLength() const;
-
- /** \brief Returns the container corners in container coordinates
- *
- * Returns the container corners in container coordinates
- *
- **/
- Eigen::MatrixXs getCorners() const;
-
- /** \brief Returns a corner in container coordinates
- *
- * Returns a corner in container coordinates
- * \param _id the index of the corner to be provided
- *
- **/
- Eigen::VectorXs getCorner(const unsigned int _id) const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/landmark/landmark_corner_2D.h b/include/base/landmark/landmark_corner_2D.h
deleted file mode 100644
index c1ce86fbdd1979a4e4e04150964207ab9e0ee994..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_corner_2D.h
+++ /dev/null
@@ -1,41 +0,0 @@
-
-#ifndef LANDMARK_CORNER_H_
-#define LANDMARK_CORNER_H_
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-
-// Std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkCorner2D);
-
-//class LandmarkCorner2D
-class LandmarkCorner2D : public LandmarkBase
-{
- public:
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock shared pointer to the position
- * \param _o_ptr StateBlock shared pointer to the orientation
- * \param _aperture descriptor of the landmark: aperture of the corner
- *
- **/
- LandmarkCorner2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _aperture=0);
-
- virtual ~LandmarkCorner2D();
-
- /** \brief Returns aperture
- *
- * Returns aperture
- *
- **/
- Scalar getAperture() const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/landmark/landmark_line_2D.h b/include/base/landmark/landmark_line_2D.h
deleted file mode 100644
index 3a0f84c340d12027a74bb5be7ff11f31288c06e2..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_line_2D.h
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#ifndef LANDMARK_LINE_2D_H_
-#define LANDMARK_LINE_2D_H_
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-#include "base/wolf.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkLine2D);
-
-//class LandmarkLine2D
-class LandmarkLine2D : public LandmarkBase
-{
- protected:
- //extreme points of the line segment
- Eigen::Vector3s point1_;
- Eigen::Vector3s point2_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- /** \brief Constructor with homogeneous parameters of the line
- *
- * \param _p_ptr homogeneous parameters of the line: (a,b,c) from ax+by+c=0, normalized according line/sqrt(a*a+b*b)
- * See http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
- * \param _point1 Extreme point 1 in homogeneous coordinates (3-vector)
- * \param _point2 Extreme point 2 in homogeneous coordinates (3-vector)
- *
- **/
- LandmarkLine2D(StateBlockPtr _p_ptr, Eigen::Vector3s & _point1, Eigen::Vector3s & _point2);
-
- virtual ~LandmarkLine2D();
-
- /** \brief Line segment update
- * Update extreme points according new inputs.
- * Assumes normalized points (x/z, y/z, 1)
- **/
- void updateExtremePoints(Eigen::Vector3s & _q1, Eigen::Vector3s & _q2);
-
- /** \brief Gets extreme point1
- **/
- const Eigen::Vector3s & point1() const;
-
- /** \brief Gets extreme point2
- **/
- const Eigen::Vector3s & point2() const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/landmark/landmark_match.h b/include/base/landmark/landmark_match.h
deleted file mode 100644
index 0c5d3a4915892a3680d634f24a6a40a73df6e26c..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_match.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef LANDMARK_MATCH_H_
-#define LANDMARK_MATCH_H_
-
-// Wolf includes
-#include "base/wolf.h"
-
-//wolf nampseace
-namespace wolf {
-
-// Map of Feature - Landmark matches
-WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatch);
-typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
-
-/** \brief Match between a feature and a landmark
- *
- * Match between a feature and a landmark
- *
- **/
-struct LandmarkMatch
-{
- LandmarkBasePtr landmark_ptr_;
- Scalar normalized_score_;
-
- LandmarkMatch() :
- landmark_ptr_(nullptr), normalized_score_(0)
- {
-
- }
- LandmarkMatch(LandmarkBasePtr _landmark_ptr, Scalar _normalized_score) :
- landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
- {
-
- }
-};
-
-}//end namespace
-
-#endif
diff --git a/include/base/landmark/landmark_match_polyline_2D.h b/include/base/landmark/landmark_match_polyline_2D.h
deleted file mode 100644
index 24d1eddf448d4e61577db954ffd839d4762d0b6a..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_match_polyline_2D.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef LANDMARK_MATCH_POLYLINE_2D_H_
-#define LANDMARK_MATCH_POLYLINE_2D_H_
-
-// Wolf includes
-#include "base/wolf.h"
-#include "base/landmark/landmark_match.h"
-
-//wolf nampseace
-namespace wolf {
-
-// Map of Feature - Landmark matches
-WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatchPolyline2D);
-typedef std::map<FeatureBasePtr, LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DMap;
-
-/** \brief Match between a feature_polyline_2D and a landmark_polyline_2D
- *
- * Match between a feature_polyline_2D and a landmark_polyline_2D
- *
- **/
-struct LandmarkMatchPolyline2D : public LandmarkMatch
-{
- int landmark_from_id_;
- int feature_from_id_;
- int landmark_to_id_;
- int feature_to_id_;
- int landmark_version_;
- Eigen::Matrix3s T_feature_landmark_;
-
- bool check() const;
-};
-
-}//end namespace
-
-#endif
diff --git a/include/base/landmark/landmark_point_3D.h b/include/base/landmark/landmark_point_3D.h
deleted file mode 100644
index 7ee1c0507b3f3f63d0015bf509fb7eb629db309c..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_point_3D.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef LANDMARK_POINT_3D_H
-#define LANDMARK_POINT_3D_H
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-
-// Vision Utils includes
-#include <vision_utils/vision_utils.h>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkPoint3D);
-
-//class
-class LandmarkPoint3D : public LandmarkBase
-{
- protected:
- cv::Mat descriptor_;
- Eigen::Vector3s position_;
- public:
- LandmarkPoint3D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, Eigen::Vector3s _position, cv::Mat _2D_descriptor);
-
- virtual ~LandmarkPoint3D();
-
- const Eigen::Vector3s& getPosition() const;
- void setPosition(const Eigen::Vector3s & _pos)
- {
- position_ = _pos;
- }
-
- const cv::Mat& getDescriptor() const;
- void setDescriptor(const cv::Mat& _descriptor)
- {
- descriptor_ = _descriptor;
- }
-};
-
-inline const Eigen::Vector3s& LandmarkPoint3D::getPosition() const
-{
- return position_;
-}
-
-inline const cv::Mat& LandmarkPoint3D::getDescriptor() const
-{
- return descriptor_;
-}
-
-} // namespace wolf
-
-#endif // LANDMARK_POINT_3D_H
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
deleted file mode 100644
index 0e7b541a40e0504de4b47c58a9cb0f9e7d4cb5a4..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_polyline_2D.h
+++ /dev/null
@@ -1,351 +0,0 @@
-/**
- * \file landmark_poliyline_2D.h
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#ifndef LANDMARK_POLYLINE_2D_H_
-#define LANDMARK_POLYLINE_2D_H_
-
-// Wolf
-#include "base/landmark/landmark_base.h"
-#include "base/processor/polyline_2D_utils.h"
-
-// STL
-#include <deque>
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-//landmark classification
-typedef enum
-{
- UNCLASSIFIED,
- CONTAINER, ///< A container 12.2 x 2.44 (m)
- SMALL_CONTAINER, ///< A small container 6.1 x 2.44 (m)
- PALLET, ///< A pallet box 0.9 x 1.2 (m)
- OTHER ///< To be used for custom rectangles
-} PolylineClassType;
-
-struct PolylineRectangularClass
-{
- PolylineClassType type; // name of the type
- Scalar L; // long side length
- Scalar W; // short side length
- Scalar D; // diagonal length
-
- PolylineRectangularClass() :
- type(UNCLASSIFIED),
- L(0),
- W(0),
- D(0)
- {
- };
-
- PolylineRectangularClass(const PolylineClassType& _type, const Scalar& _L, const Scalar& _W) :
- type(_type),
- L(_L),
- W(_W),
- D(sqrt(L*L+W*W))
- {
- };
-};
-struct PolylineClassContainer : public PolylineRectangularClass
-{
- PolylineClassContainer() : PolylineRectangularClass(CONTAINER, 12.2, 2.44){};
-};
-struct PolylineClassSmallContainer : public PolylineRectangularClass
-{
- PolylineClassSmallContainer() : PolylineRectangularClass(SMALL_CONTAINER, 6.1, 2.44){};
-};
-struct PolylineClassPallet : public PolylineRectangularClass
-{
- PolylineClassPallet() : PolylineRectangularClass(PALLET, 1.2, 0.9){};
-};
-
-WOLF_PTR_TYPEDEFS(LandmarkPolyline2D);
-WOLF_LIST_TYPEDEFS(LandmarkPolyline2D);
-
-//class
-class LandmarkPolyline2D : public LandmarkBase
-{
- protected:
- std::map<int,StateBlockPtr> point_state_ptr_map_; ///< polyline points state blocks
- bool first_defined_; ///< Wether the first point is an extreme of a line or the line may continue
- bool last_defined_; ///< Wether the last point is an extreme of a line or the line may continue
- int first_id_;
- int last_id_;
- bool closed_; ///< Wether the polyline is closed or not
- PolylineRectangularClass classification_; ///< The classification of the landmark
- int version_; ///< Integer increased each time a modification in landmark occurs (added points, merged points, closed, classified)
- LandmarkPolyline2DPtr merged_in_lmk_;
-
- private:
-
- StateBlockPtr& firstStateBlock();
- StateBlockPtr& lastStateBlock();
-
- public:
- LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0);
- LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class);
- virtual ~LandmarkPolyline2D();
-
- /** \brief Gets a const reference to the point state block pointer vector
- **/
- std::map<int,StateBlockPtr>& getPointStatePtrMap();
-
-
- /** \brief Gets wether the first/last points are defined or not
- **/
- bool isFirstDefined() const;
- bool isLastDefined() const;
-
- /** \brief Gets whether the polyline is closed or not
- **/
- bool isClosed() const;
-
- /** \brief Get the landmark pointer in which was merged (nullptr in case of no merged)
- **/
- LandmarkPolyline2DPtr getMergedInLandmark() const;
-
- /** \brief Set the landmark pointer in which was merged
- **/
- void setMergedInLandmark(const LandmarkPolyline2DPtr& lmk_ptr);
-
- /** \brief Gets whether the given state block point is defined or not (assumes the state block is in the landmark)
- **/
- bool isDefined(StateBlockPtr _state_block) const;
-
- /** \brief Sets the first/last extreme point
- **/
- void setFirst(const Eigen::VectorXs& _point, bool _defined);
- void setLast(const Eigen::VectorXs& _point, bool _defined);
-
- int getNPoints() const;
- int getNDefinedPoints() const;
- std::vector<int> getValidIds() const;
- int getFirstId() const;
- int getLastId() const;
- bool isValidId(const int& i) const;
- int getNextValidId(const int& i) const;
- int getPrevValidId(const int& i) const;
- int getVersion() const;
-
- const Eigen::VectorXs getPointVector(int _i) const;
-
- Eigen::MatrixXs computePointsGlobal() const;
- Eigen::MatrixXs computePointsCovGlobal() const;
-
- StateBlockPtr getPointStateBlock(int _i);
-
- /** \brief Gets a vector of all state blocks pointers
- **/
- virtual std::vector<StateBlockPtr> getPointsStateBlockVector() const;
-
- /** \brief Adds a new point to the landmark
- * \param _point: the point to be added
- * \param _extreme: if its extreme or not
- * \param _back: if it have to be added in the back (true) or in the front (false)
- **/
- void addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back);
-
- /** \brief Adds new points to the landmark
- * \param _points: a matrix containing points, some of them to be added
- * \param _idx: from wich position of the first point to be added
- * \param _extreme: if last point to be added is extreme or not
- * \param _back: if the points have to be added in the back (true) or in the front (false)
- **/
- void addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined, const bool& _back);
-
- /** \brief define extreme point
- **/
- virtual void defineExtreme(const bool _back);
-
- /** \brief define first point
- **/
- virtual void defineFirst();
-
- /** \brief define last point
- **/
- virtual void defineLast();
-
- /** \brief Try to close the polyline (if it has overlapped points)
- **/
- virtual bool tryClose(const Scalar& _dist_tol);
-
- /** \brief Set the polyline as closed
- **/
- virtual void setClosed();
-
- /** \brief merge points
- **/
- void mergePoints(int _remove_id, int _remain_id);
-
- /** \brief Classify as a known object
- **/
- void classify(PolylineRectangularClass _class);
-
- /** \brief Try to classify polyline as a known object
- **/
- bool tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes);
-
- /** \brief Try to classify polyline as a known object
- **/
- void mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, const int& _from, const int& _to, const int& _merged_from, const int& _merged_to);
-
- /** \brief get classification
- **/
- PolylineRectangularClass getClassification() const;
-
- /** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
- **/
- virtual void registerNewStateBlocks();
- virtual void removeStateBlocks();
-
- /** Factory method to create new landmarks from YAML nodes
- */
- static LandmarkBasePtr create(const YAML::Node& _lmk_node);
-
- YAML::Node saveToYaml() const;
-
- static void tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol);
-};
-
-inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock()
-{
- return point_state_ptr_map_.begin()->second;
-}
-
-inline StateBlockPtr& LandmarkPolyline2D::lastStateBlock()
-{
- return point_state_ptr_map_.rbegin()->second;
-}
-
-inline std::map<int,StateBlockPtr>& LandmarkPolyline2D::getPointStatePtrMap()
-{
- return point_state_ptr_map_;
-}
-
-inline bool LandmarkPolyline2D::isFirstDefined() const
-{
- return first_defined_;
-}
-
-inline bool LandmarkPolyline2D::isLastDefined() const
-{
- return last_defined_;
-}
-
-inline bool LandmarkPolyline2D::isValidId(const int& i) const
-{
- return point_state_ptr_map_.find(i) != point_state_ptr_map_.end();
-};
-
-inline bool LandmarkPolyline2D::isClosed() const
-{
- return closed_;
-}
-
-inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const
-{
- // recursive call
- if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
- return merged_in_lmk_->getMergedInLandmark();
- return merged_in_lmk_;
-}
-
-inline bool LandmarkPolyline2D::isDefined(StateBlockPtr _state_block) const
-{
- if (_state_block == point_state_ptr_map_.begin()->second)
- return first_defined_;
-
- if (_state_block == point_state_ptr_map_.rbegin()->second)
- return last_defined_;
-
- return true;
-}
-
-inline int LandmarkPolyline2D::getNPoints() const
-{
- return (int)point_state_ptr_map_.size();
-}
-
-inline int LandmarkPolyline2D::getNDefinedPoints() const
-{
- return (int)point_state_ptr_map_.size() - (first_defined_ ? 0 : 1) - (last_defined_ ? 0 : 1);
-}
-
-inline std::vector<int> LandmarkPolyline2D::getValidIds() const
-{
- std::vector<int> valid_ids;
- for (auto st_pair : point_state_ptr_map_)
- valid_ids.push_back(st_pair.first);
-
- return valid_ids;
-}
-
-inline int LandmarkPolyline2D::getFirstId() const {
- return point_state_ptr_map_.begin()->first;
-}
-
-inline int LandmarkPolyline2D::getLastId() const {
- return point_state_ptr_map_.rbegin()->first;
-}
-
-
-inline int LandmarkPolyline2D::getNextValidId(const int& i) const
-{
- assert(!(i == last_id_ && !closed_) && "Calling getNextValidId of last_id in an open landmark");
- assert(isValidId(i) && "Calling getNextValidId of an invalid id");
-
- if (i == last_id_ && closed_)
- return first_id_;
- return std::next(point_state_ptr_map_.find(i))->first;
-}
-
-inline int LandmarkPolyline2D::getPrevValidId(const int& i) const
-{
- assert(!(i == first_id_ && !closed_) && "Calling getPrevValidId of first_id in an open landmark");
- assert(isValidId(i) && "Calling getPrevValidId of an invalid id");
-
- if (i == first_id_ && closed_)
- return last_id_;
- return std::prev(point_state_ptr_map_.find(i))->first;
-}
-
-inline int LandmarkPolyline2D::getVersion() const
-{
- return version_;
-}
-
-inline std::vector<StateBlockPtr> LandmarkPolyline2D::getPointsStateBlockVector() const
-{
- std::vector<StateBlockPtr> state_block_vec;
- for (auto st_pair : point_state_ptr_map_)
- state_block_vec.push_back(st_pair.second);
-
- return state_block_vec;
-}
-
-inline void LandmarkPolyline2D::classify(PolylineRectangularClass _class)
-{
- classification_ = _class;
-}
-
-inline PolylineRectangularClass LandmarkPolyline2D::getClassification() const
-{
- return classification_;
-}
-
-inline void LandmarkPolyline2D::setMergedInLandmark(const LandmarkPolyline2DPtr& lmk_ptr)
-{
- merged_in_lmk_ = lmk_ptr;
-}
-
-} /* namespace wolf */
-
-#endif /* LANDMARK_POLYLINE_2D_H_ */
diff --git a/include/base/local_parametrization_angle.h b/include/base/local_parametrization_angle.h
deleted file mode 100644
index 912237c07a8b4a7aa4d711c39c68f3c9cccf8d24..0000000000000000000000000000000000000000
--- a/include/base/local_parametrization_angle.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/**
- * \file local_parametrization_angle.h
- *
- * Created on: Apr 4, 2017
- * \author: jsola
- */
-
-#ifndef LOCAL_PARAMETRIZATION_ANGLE_H_
-#define LOCAL_PARAMETRIZATION_ANGLE_H_
-
-#include "base/local_parametrization_base.h"
-#include "base/rotations.h"
-
-namespace wolf
-{
-
-class LocalParametrizationAngle : public LocalParametrizationBase
-{
- public:
- LocalParametrizationAngle();
- virtual ~LocalParametrizationAngle();
-
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _h, Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _x1,
- Eigen::Map<const Eigen::VectorXs>& _x2,
- Eigen::Map<Eigen::VectorXs>& _x2_minus_x1);
-
-};
-
-inline LocalParametrizationAngle::LocalParametrizationAngle() :
- LocalParametrizationBase(1,1)
-{
- //
-}
-
-inline LocalParametrizationAngle::~LocalParametrizationAngle()
-{
- //
-}
-
-inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const
-{
- _h_plus_delta(0) = pi2pi(_h(0) + _delta(0));
- return true;
-}
-
-inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
-{
- _jacobian(0) = 1.0;
- return true;
-}
-
-inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXs>& _x1,
- Eigen::Map<const Eigen::VectorXs>& _x2,
- Eigen::Map<Eigen::VectorXs>& _x2_minus_x1)
-{
- _x2_minus_x1(0) = pi2pi(_x2(0)-_x1(0));
- return true;
-}
-
-} /* namespace wolf */
-
-#endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */
diff --git a/include/base/local_parametrization_base.h b/include/base/local_parametrization_base.h
deleted file mode 100644
index 3b7dcc38dc698936a707101aa40cad076adc910f..0000000000000000000000000000000000000000
--- a/include/base/local_parametrization_base.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * \file local_parametrization_base.h
- *
- * Created on: Feb 17, 2016
- * \author: jsola
- */
-
-#ifndef LOCAL_PARAMETRIZATION_BASE_H_
-#define LOCAL_PARAMETRIZATION_BASE_H_
-
-#include "base/wolf.h"
-
-namespace wolf {
-
-//WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
-
-class LocalParametrizationBase{
- protected:
- unsigned int global_size_;
- unsigned int local_size_;
- public:
- LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
- virtual ~LocalParametrizationBase();
-
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _x,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _x_plus_delta) const = 0;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _x, Eigen::Map<Eigen::MatrixXs>& _jacobian) const = 0;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _x1,
- Eigen::Map<const Eigen::VectorXs>& _x2,
- Eigen::Map<Eigen::VectorXs>& _x2_minus_x1) = 0;
-
- unsigned int getLocalSize() const;
- unsigned int getGlobalSize() const;
-};
-
-} // namespace wolf
-
-#endif /* LOCAL_PARAMETRIZATION_BASE_H_ */
diff --git a/include/base/local_parametrization_homogeneous.h b/include/base/local_parametrization_homogeneous.h
deleted file mode 100644
index f06dd2fb150e5f34eede487e3a685fb88f00d130..0000000000000000000000000000000000000000
--- a/include/base/local_parametrization_homogeneous.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * \file local_parametrization_homogeneous.h
- *
- * Created on: 24/02/2016
- * Author: jsola
- */
-
-#ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_
-#define LOCALPARAMETRIZATIONHOMOGENEOUS_H_
-
-#include "base/local_parametrization_base.h"
-
-namespace wolf {
-
-/**
- * \brief Local parametrization for homogeneous vectors.
- *
- * The composition x_plus_dx = plus(x,dx), written in math mode as \f${\bf x}^+={\bf x}\oplus d {\bf x}\f$,
- * is done orthogonally to \f${\bf x}\f$,
- * so that the result remains in the 4-sphere defined by the norm of \f${\bf x}\f$.
- * Because this is exactly what we do with quaternions,
- * we use quaternion algebra to achieve this effect, with
- *
- * \f[{\bf x}^+ = {\bf q}( d {\bf x}) \otimes {\bf x}\f]
- *
- * where \f$\otimes\f$ is the quaternion product, and \f$d{\bf q} = {\bf q}(d {\bf x})\f$ is a unit delta_quaternion
- * equivalent to a rotation \f$d{\bf x}\f$, obtained with
- *
- * \f[{\bf q}(d{\bf x}) = \left[\begin{array}{c} \frac{ d {\bf x}}{ |d{\bf x}|} \sin(|d{\bf x}|) \\ \cos(|d{\bf x}|) \end{array}\right]\f]
- *
- * Contrary to the case of quaternions, it is not required that \f${\bf x}\f$ be a unit homogeneous vector.
- * In this case, the updated \f${\bf x}^+={\bf x}\oplus d {\bf x}\f$ has the same norm as \f${\bf x}\f$.
- * It is however a good practice to have unit or close-to-unit
- * homogeneous vectors for the sake of numerical stability.
- *
- */
-class LocalParametrizationHomogeneous : public LocalParametrizationBase
-{
- public:
- LocalParametrizationHomogeneous();
- virtual ~LocalParametrizationHomogeneous();
-
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _h1,
- Eigen::Map<const Eigen::VectorXs>& _h2,
- Eigen::Map<Eigen::VectorXs>& _h2_minus_h1);
-};
-
-} // namespace wolf
-
-#endif /* LOCALPARAMETRIZATIONHOMOGENEOUS_H_ */
diff --git a/include/base/local_parametrization_polyline_extreme.h b/include/base/local_parametrization_polyline_extreme.h
deleted file mode 100644
index 3a75a8a0ff4eede5a3e865810555852653494713..0000000000000000000000000000000000000000
--- a/include/base/local_parametrization_polyline_extreme.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * \file local_parametrization_polyline_extreme.h
- *
- * Created on: Jun 10, 2016
- * \author: jvallve
- */
-
-#ifndef LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
-#define LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
-
-#include "base/local_parametrization_base.h"
-
-namespace wolf {
-
-/**
- * \brief Local parametrization for polylines not defined extreme points
- *
- */
-class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
-{
- private:
- StateBlockPtr reference_point_;
- public:
- LocalParametrizationPolylineExtreme(StateBlockPtr _reference_point);
- virtual ~LocalParametrizationPolylineExtreme();
-
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta);
-};
-
-} // namespace wolf
-
-#endif /* LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_ */
diff --git a/include/base/local_parametrization_quaternion.h b/include/base/local_parametrization_quaternion.h
deleted file mode 100644
index f92676558d4cbd8da107852779241a20836c2fe0..0000000000000000000000000000000000000000
--- a/include/base/local_parametrization_quaternion.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * \file local_parametrization_quaternion.h
- *
- * Created on: Feb 18, 2016
- * \author: jsola
- */
-
-#ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_
-#define LOCAL_PARAMETRIZATION_QUATERNION_H_
-
-#include "base/local_parametrization_base.h"
-
-namespace wolf {
-
-/**
- * \brief Local or global orientation error
- *
- * Local or global orientation error.
- *
- * This enum controls whether the orientation error is composed locally or globally to an orientation specification.
- *
- * See LocalParametrizationQuaternion for more information.
- */
-typedef enum {
- DQ_LOCAL,
- DQ_GLOBAL
-} QuaternionDeltaReference;
-
-/**
- * \brief Local parametrization for quaternions
- *
- * This class implements two possible local parametrizations for quaternions:
- *
- * - With a local delta, so that \f${\bf q}^+ = {\bf q} \otimes {\bf q}(d\theta)\f$.
- * - With a global delta, so that \f${\bf q}^+ = {\bf q}(d\theta) \otimes {\bf q}\f$.
- *
- * The choice is templated, through an enum QuaternionDeltaReference.
- *
- * In either case, the incremental quaternion \f${\bf q}(d\theta)\f$ is computed from the delta_theta
- * variable, here named \f$d\theta\f$, using
- *
- * \f[{\bf q}(d\theta) =
- * \left[\begin{array}{c}
- * \frac{d\theta}{|d\theta|} \sin(|d\theta|) \\
- * \cos(|d\theta|)
- * \end{array}\right]
- * \f]
- *
- */
-template <QuaternionDeltaReference DeltaReference = DQ_LOCAL>
-class LocalParametrizationQuaternion : public LocalParametrizationBase
-{
-
-public:
-
- LocalParametrizationQuaternion() :
- LocalParametrizationBase(4, 3)
- {
- //
- }
-
- virtual ~LocalParametrizationQuaternion()
- {
- //
- }
-
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const;
-
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _q1,
- Eigen::Map<const Eigen::VectorXs>& _q2,
- Eigen::Map<Eigen::VectorXs>& _q2_minus_q1);
-};
-
-typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
-typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionLocal;
-
-} // namespace wolf
-
-#endif /* LOCAL_PARAMETRIZATION_QUATERNION_H_ */
diff --git a/include/base/logging.h b/include/base/logging.h
deleted file mode 100644
index 3593fc718de7b717db2d19f588fe8e97a1e814ce..0000000000000000000000000000000000000000
--- a/include/base/logging.h
+++ /dev/null
@@ -1,333 +0,0 @@
-/**
- * \file logging.h
- *
- * Created on: Oct 27, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef WOLF_LOGGING_H_
-#define WOLF_LOGGING_H_
-
-// third_party include
-#include <map>
-
-// spdlog include
-#include "spdlog/spdlog.h"
-// enable the use of ostream operator<<
-#include "spdlog/fmt/bundled/ostream.h"
-
-// Wolf includes
-#include "base/singleton.h"
-
-namespace wolf {
-namespace internal {
-namespace do_not_enter_where_the_wolf_lives {
-
-#define __INTERNAL_WOLF_MAIN_LOGGER_NAME_ "wolf_main_console"
-
-static const auto repeated_brace = std::make_tuple("{}",
- "{}{}",
- "{}{}{}",
- "{}{}{}{}",
- "{}{}{}{}{}",
- "{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}"); // up to 25 args. Should be fine
-class Logger
-{
-public:
-
- Logger(const std::string& name);
-
- Logger(std::string&& name);
-
- ~Logger();
-
- // Not copyable/movable
- Logger(Logger&) = delete;
- void operator=(Logger&) = delete;
- Logger(Logger&&) = delete;
- void operator=(Logger&&) = delete;
-
- template<typename... Args>
- void info(Args&&... args) const;
-
- template<typename... Args>
- void warn(Args&&... args) const;
-
- template<typename... Args>
- void error(Args&&... args) const;
-
- template<typename... Args>
- void debug(Args&&... args) const;
-
- template<typename... Args>
- void trace(Args&&... args) const;
-
- bool set_async_queue(const std::size_t q_size);
-
- void set_pattern(const std::string& p);
-
-protected:
-
- const std::string log_name_;
-
- std::shared_ptr<spdlog::logger> console_;
-};
-
-inline Logger::Logger(const std::string& name) :
- log_name_(name)
-{
- // Create main logger
- console_ = spdlog::stdout_color_mt(log_name_);
-
-#ifdef _WOLF_TRACE
- console_->set_level(spdlog::level::trace);
-#endif
-
- // Enable asynchronous logging
- // Queue size must be a power of 2
- spdlog::set_async_mode(4096);
-
- if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_)
- // Logging pattern is :
- // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
-// set_pattern("[%l][%x - %H:%M:%S.%F] %v");
- set_pattern("[%l][%H:%M:%S] %v");
- else
- // Logging pattern is :
- // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content
- set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v");
-}
-
-inline Logger::Logger(std::string&& name) :
- log_name_(std::forward<std::string>(name))
-{
- // Create main logger
- console_ = spdlog::stdout_color_mt(log_name_);
-
-#ifdef _WOLF_TRACE
- console_->set_level(spdlog::level::trace);
-#endif
-
- // Enable asynchronous logging
- // Queue size must be a power of 2
- spdlog::set_async_mode(4096);
-
- if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_)
- // Logging pattern is :
- // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
-// set_pattern("[%l][%x - %H:%M:%S.%F] %v");
- set_pattern("[%l][%H:%M:%S] %v");
- else
- // Logging pattern is :
- // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content
- set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v");
-}
-
-inline Logger::~Logger()
-{
- spdlog::drop(log_name_);
-}
-
-template<typename... Args>
-void Logger::info(Args&&... args) const
-{
- console_->info(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
-}
-
-template<typename... Args>
-void Logger::warn(Args&&... args) const
-{
- console_->warn(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
-}
-
-template<typename... Args>
-void Logger::error(Args&&... args) const
-{
- console_->error(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
-}
-
-template<typename... Args>
-void Logger::debug(Args&&... args) const
-{
- console_->debug(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
-}
-
-template<typename... Args>
-void Logger::trace(Args&&... args) const
-{
- console_->trace(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
-}
-
-inline bool Logger::set_async_queue(const std::size_t q_size)
-{
- bool p2 = q_size%2 == 0;
-
- if (p2) spdlog::set_async_mode(q_size);
-
- return q_size;
-}
-
-inline void Logger::set_pattern(const std::string& p)
-{
- console_->set_pattern(p);
-}
-
-using LoggerPtr = std::unique_ptr<Logger>;
-
-/// dummy namespace to avoid colision with c++14
-/// @todo use std version once we move to cxx14
-namespace not_std {
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-} /* namespace not_std */
-
-class LoggerManager
-{
-public:
-
- LoggerManager() = default;
- ~LoggerManager() = default;
-
- bool exists(const std::string& name) const
- {
- std::lock_guard<std::mutex> lock(mut_);
- return existsImpl(name);
- }
-
- const Logger& getLogger(const std::string& name) /*const*/
- {
- std::lock_guard<std::mutex> lock(mut_);
-
- if (!existsImpl(name)) addLogger(name);
-
- return *(logger_map_.at(name));
- }
-
-protected:
-
- mutable std::mutex mut_;
-
- std::map<const std::string, const LoggerPtr> logger_map_;
-
- bool addLogger(const std::string& name)
- {
- /// @note would be easier with cpp17 map.try_emplace...
- const bool created = existsImpl(name) ?
- false :
- logger_map_.emplace(name, not_std::make_unique<Logger>(name)).second;
- return created;
- }
-
- bool existsImpl(const std::string& name) const
- {
- return (logger_map_.find(name) != logger_map_.end());
- //return (spdlog::get(name) != nullptr);
- }
-};
-
-} /* namespace do_not_enter_where_the_wolf_lives */
-
-using WolfLogger = Singleton<do_not_enter_where_the_wolf_lives::Logger>;
-using WolfLoggerManager = Singleton<do_not_enter_where_the_wolf_lives::LoggerManager>;
-
-} /* namespace internal */
-} /* namespace wolf */
-
-#define WOLF_STRINGIFY(x) #x
-#define WOLF_STR_HELPER(x) WOLF_STRINGIFY(x)
-
-/// @brief NAMED LOGGING
-
-#define WOLF_ASYNC_QUEUE_LOG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).set_async_queue(x);
-
-#define WOLF_INFO_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).info(__VA_ARGS__);
-#define WOLF_INFO_NAMED_COND(name, cond, ...) if (cond) WOLF_INFO_NAMED(name, __VA_ARGS__);
-
-#define WOLF_WARN_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).warn(__VA_ARGS__);
-#define WOLF_WARN_NAMED_COND(name, cond, ...) if (cond) WOLF_WARN_NAMED(name, __VA_ARGS__);
-
-#define WOLF_ERROR_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).error(__VA_ARGS__);
-#define WOLF_ERROR_NAMED_COND(name, cond, ...) if (cond) WOLF_ERROR_NAMED(name, __VA_ARGS__);
-
-#ifdef _WOLF_DEBUG
- #define WOLF_DEBUG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).debug(__VA_ARGS__);
- #define WOLF_DEBUG_NAMED_COND(name, cond, ...) if (cond) WOLF_DEBUG_NAMED(name, __VA_ARGS__);
-#else
- #define WOLF_DEBUG_NAMED(name, ...)
- #define WOLF_DEBUG_NAMED_COND(cond, name, ...)
-#endif
-
-#ifdef _WOLF_TRACE
- #define WOLF_TRACE_NAMED(name, ...) \
- {char this_file[] = __FILE__;\
- wolf::internal::WolfLoggerManager::get().getLogger(name).trace("[", basename(this_file), " L", __LINE__, \
- " : ", __FUNCTION__, "] ", __VA_ARGS__);}
- #define WOLF_TRACE_NAMED_COND(name, cond, ...) if (cond) WOLF_TRACE_NAMED_COND(name, __VA_ARGS__);
-#else
- #define WOLF_TRACE_NAMED(...)
- #define WOLF_TRACE_NAMED_cond(name, cond, ...)
-#endif
-
-/// @brief MAIN LOGGING
-
-#define WOLF_ASYNC_QUEUE_LOG(x) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).set_async_queue(x);
-
-#define WOLF_INFO(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).info(__VA_ARGS__);
-#define WOLF_INFO_COND(cond, ...) if (cond) WOLF_INFO(__VA_ARGS__);
-
-#define WOLF_WARN(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).warn(__VA_ARGS__);
-#define WOLF_WARN_COND(cond, ...) if (cond) WOLF_WARN(__VA_ARGS__);
-
-#define WOLF_ERROR(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).error(__VA_ARGS__);
-#define WOLF_ERROR_COND(cond, ...) if (cond) WOLF_ERROR(__VA_ARGS__);
-
-#ifdef _WOLF_DEBUG
- #define WOLF_DEBUG(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).debug(__VA_ARGS__);
- #define WOLF_DEBUG_COND(cond, ...) if (cond) WOLF_DEBUG(__VA_ARGS__);
-#else
- #define WOLF_DEBUG(...)
- #define WOLF_DEBUG_COND(cond, ...)
-#endif
-
-#ifdef _WOLF_TRACE
- #define WOLF_TRACE(...) \
- {char this_file[] = __FILE__;\
- wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).trace("[", basename(this_file), " L", __LINE__, \
- " : ", __FUNCTION__, "] ", __VA_ARGS__);}
- #define WOLF_TRACE_COND(cond, ...) if (cond) WOLF_TRACE(__VA_ARGS__);
-#else
- #define WOLF_TRACE(...)
- #define WOLF_TRACE_COND(cond, ...)
-#endif
-
-#endif /* WOLF_LOGGING_H_ */
diff --git a/include/base/make_unique.h b/include/base/make_unique.h
deleted file mode 100644
index f336eb4f7ff52eae0e7014505180bb060f48ee70..0000000000000000000000000000000000000000
--- a/include/base/make_unique.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/**
- * \file make_unique.h
- *
- * Created on: Oct 11, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_MAKE_UNIQUE_H_
-#define _WOLF_MAKE_UNIQUE_H_
-
-#include <memory>
-
-namespace wolf {
-
-/// @note this appears only in cpp14
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-
-}
-
-#endif /* _WOLF_MAKE_UNIQUE_H_ */
diff --git a/include/base/map_base.h b/include/base/map_base.h
deleted file mode 100644
index a8b447905d59c8ffb30be66cd1b91a6b36dc9ae6..0000000000000000000000000000000000000000
--- a/include/base/map_base.h
+++ /dev/null
@@ -1,47 +0,0 @@
-
-#ifndef MAP_BASE_H_
-#define MAP_BASE_H_
-
-// Fwd refs
-namespace wolf{
-class Problem;
-class LandmarkBase;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-
-//std includes
-
-namespace wolf {
-
-//class MapBase
-class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
-{
- private:
- LandmarkBasePtrList landmark_list_;
-
- public:
- MapBase();
- ~MapBase();
-
- virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
- virtual void addLandmarkList(LandmarkBasePtrList& _landmark_list);
- LandmarkBasePtrList& getLandmarkList();
-
- void load(const std::string& _map_file_yaml);
- void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
-
- private:
- std::string dateTimeNow();
-};
-
-inline LandmarkBasePtrList& MapBase::getLandmarkList()
-{
- return landmark_list_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/motion_buffer.h b/include/base/motion_buffer.h
deleted file mode 100644
index f8d0ad5be8e854eaf8badca494218b637795d16e..0000000000000000000000000000000000000000
--- a/include/base/motion_buffer.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/**
- * \file motion_buffer.h
- *
- * Created on: Apr 14, 2016
- * \author: jsola
- */
-
-#ifndef SRC_MOTIONBUFFER_H_
-#define SRC_MOTIONBUFFER_H_
-
-#include "base/wolf.h"
-#include "base/time_stamp.h"
-
-#include <list>
-#include <algorithm>
-
-namespace wolf {
-
-using namespace Eigen;
-
-struct Motion
-{
- public:
- SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
- TimeStamp ts_; ///< Time stamp
- Eigen::VectorXs data_; ///< instantaneous motion data
- Eigen::MatrixXs data_cov_; ///< covariance of the instantaneous data
- Eigen::VectorXs delta_; ///< instantaneous motion delta
- Eigen::MatrixXs delta_cov_; ///< covariance of the instantaneous delta
- Eigen::VectorXs delta_integr_; ///< the integrated motion or delta-integral
- Eigen::MatrixXs delta_integr_cov_; ///< covariance of the integrated delta
- Eigen::MatrixXs jacobian_delta_; ///< Jacobian of the integration wrt delta_
- Eigen::MatrixXs jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
- Eigen::MatrixXs jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
- public:
- Motion() = delete; // completely delete unpredictable stuff like this
- Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size);
- Motion(const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _delta,
- const MatrixXs& _delta_cov,
- const VectorXs& _delta_int,
- const MatrixXs& _delta_integr_cov,
- const MatrixXs& _jac_delta,
- const MatrixXs& _jac_delta_int,
- const MatrixXs& _jacobian_calib);// = MatrixXs::Zero(0,0));
- ~Motion();
- private:
- void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
-
-}; ///< One instance of the buffered data, corresponding to a particular time stamp.
-
-/** \brief class for motion buffers.
- *
- * It consists of a buffer of pre-integrated motions (aka. delta-integrals) that is being filled
- * by the motion processors (deriving from ProcessorMotion).
- * Each delta-integral is accompanied by a time stamp, a Jacobian and a covariances matrix.
- *
- * This buffer contains the time stamp and delta-integrals:
- * - since the last key-Frame
- * - until the frame of this capture.
- *
- * The buffer can be queried for motion-integrals and delta-integrals corresponding to a provided time stamp,
- * with the following rules:
- * - If the query time stamp is later than the last one in the buffer,
- * the last motion-integral or delta-integral is returned.
- * - If the query time stamp is earlier than the beginning of the buffer,
- * the earliest Motion or Delta is returned.
- * - If the query time stamp matches one time stamp in the buffer exactly,
- * the returned motion-integral or delta-integral is the one of the queried time stamp.
- * - If the query time stamp does not match any time stamp in the buffer,
- * the returned motion-integral or delta-integral is the one immediately before the query time stamp.
- */
-class MotionBuffer{
- public:
- SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
- MotionBuffer() = delete;
- MotionBuffer(SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size);
- std::list<Motion>& get();
- const std::list<Motion>& get() const;
- const VectorXs& getCalibrationPreint() const;
- void setCalibrationPreint(const VectorXs& _calib_preint);
- const Motion& getMotion(const TimeStamp& _ts) const;
- void getMotion(const TimeStamp& _ts, Motion& _motion) const;
- void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
-// MatrixXs integrateCovariance() const; // Integrate all buffer
-// MatrixXs integrateCovariance(const TimeStamp& _ts) const; // Integrate up to time stamp (included)
-// MatrixXs integrateCovariance(const TimeStamp& _ts_1, const TimeStamp _ts_2) const; // integrate between time stamps (both included)
- void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0);
-
- private:
- VectorXs calib_preint_; ///< value of the calibration params during pre-integration
- std::list<Motion> container_;
-};
-
-inline std::list<Motion>& MotionBuffer::get()
-{
- return container_;
-}
-
-inline const std::list<Motion>& MotionBuffer::get() const
-{
- return container_;
-}
-
-inline const VectorXs& MotionBuffer::getCalibrationPreint() const
-{
- return calib_preint_;
-}
-
-inline void MotionBuffer::setCalibrationPreint(const VectorXs& _calib_preint)
-{
- assert(_calib_preint.size() == calib_size_ && "Wrong size of calibration parameters");
-
- calib_preint_ = _calib_preint;
-}
-
-} // namespace wolf
-
-#endif /* SRC_MOTIONBUFFER_H_ */
diff --git a/include/base/node_base.h b/include/base/node_base.h
deleted file mode 100644
index ebbc640825d3d6775da395b2fe1d02c03abf8073..0000000000000000000000000000000000000000
--- a/include/base/node_base.h
+++ /dev/null
@@ -1,153 +0,0 @@
-#ifndef NODE_BASE_H_
-#define NODE_BASE_H_
-
-// Wolf includes
-#include "base/wolf.h"
-
-namespace wolf {
-
-/** \brief Base class for Nodes
- *
- * Base class for all Nodes in the Wolf tree. Each node has
- *
- * - A unique ID. The class implements the ID factory.
- *
- * - A unique category name, strictly within this range of possibilities:
- * - "BASE" -- should not be used
- * - "PROBLEM"
- * - "HARDWARE"
- * - "SENSOR"
- * - "PROCESSOR"
- * - "TRAJECTORY"
- * - "FRAME"
- * - "CAPTURE"
- * - "FEATURE"
- * - "FACTOR"
- * - "MAP"
- * - "LANDMARK"
- *
- * - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples follow:
- * - "Camera"
- * - "LIDAR 2D"
- * - "Point 3D"
- * - "Lidar 2D processor"
- *
- * please refer to each base class derived from NodeLinked for better examples of their types.
- *
- * - A name, defined in each application, which is specific of each object. Again, just some examples:
- * - "Front-left camera"
- * - "Main wheel odometer"
- * - "IMU delta pre-integrator processor"
- *
- * These names are only required for objects being defined by the user at configuration time, such as:
- * - Sensors
- * - Processors
- *
- * Additionally, but not necessarily, one could also give names to:
- * - Pre-defined maps or landmarks (provided to the problem)
- * - Each of the generated captures --> these names could, for example, be a copy of the sensor name.
- *
- * Finally, all the other node names in the Wolf Tree are not required, and in fact they are not used for anything.
- *
- **/
-
-class NodeBase
-{
- private:
- static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
-
- struct Serializer;
-
- protected:
- ProblemWPtr problem_ptr_;
-
- unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
- std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc)
- std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2D", etc)
- std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc)
-
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
-
- public:
-
- NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
- virtual ~NodeBase() = default;
-
- unsigned int nodeId() const;
- std::string getCategory() const;
- std::string getType() const;
- std::string getName() const;
- bool isRemoving() const;
-
- void setType(const std::string& _type);
- void setName(const std::string& _name);
-
- ProblemPtr getProblem() const;
- virtual void setProblem(ProblemPtr _prob_ptr);
-};
-
-} // namespace wolf
-
-#include <iostream>
-
-namespace wolf{
-
-inline NodeBase::NodeBase(const std::string& _category, const std::string& _type, const std::string& _name) :
- problem_ptr_(), // nullptr
- node_id_(++node_id_count_),
- node_category_(_category),
- node_type_(_type),
- node_name_(_name),
- is_removing_(false)
-{
- //
-}
-
-inline unsigned int NodeBase::nodeId() const
-{
- return node_id_;
-}
-
-inline std::string NodeBase::getCategory() const
-{
- return node_category_;
-}
-
-inline std::string NodeBase::getType() const
-{
- return node_type_;
-}
-
-inline std::string NodeBase::getName() const
-{
- return node_name_;
-}
-
-inline bool NodeBase::isRemoving() const
-{
- return is_removing_;
-}
-
-inline void NodeBase::setType(const std::string& _type)
-{
- node_type_ = _type;
-}
-
-inline void NodeBase::setName(const std::string& _name)
-{
- node_name_ = _name;
-}
-
-inline ProblemPtr NodeBase::getProblem() const
-{
- return problem_ptr_.lock();
-}
-
-inline void NodeBase::setProblem(ProblemPtr _prob_ptr)
-{
- problem_ptr_ = _prob_ptr;
-}
-
-} // namespace wolf
-
-#endif /* NODE_BASE_H_ */
diff --git a/include/base/pinhole_tools.h b/include/base/pinhole_tools.h
deleted file mode 100644
index 46cb1fb9dd1f12dd18d3a520cc574488e7c9f170..0000000000000000000000000000000000000000
--- a/include/base/pinhole_tools.h
+++ /dev/null
@@ -1,770 +0,0 @@
-#ifndef PINHOLETOOLS_H
-#define PINHOLETOOLS_H
-
-/**
- * \file pinhole_tools.h
- *
- * \date 06/04/2010
- * \author jsola
- *
- * ## Add a description here ##
- *
- */
-
-#include "base/wolf.h"
-
-#include <iostream>
-
-namespace wolf {
-/**
- * Namespace for operations related to the pin-hole model of a camera.
- *
- * The model incorporates radial distortion.
- *
- */
-namespace pinhole {
-
-using Eigen::MatrixBase;
-using Eigen::Matrix;
-
-/**
- * Pin-hole canonical projection.
- * \param _v a 3D point to project
- * \param _up the projected point in the normalized 2D plane
- */
-template<typename Derived1, typename Derived2>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
-
- _up(0) = _v(0) / _v(2);
- _up(1) = _v(1) / _v(2);
-}
-
-/**
- * Pin-hole canonical projection.
- * \return the projected point in the normalized 2D plane
- * \param _v a 3D point
- */
-template<typename Derived>
-inline Matrix<typename Derived::Scalar, 2, 1>
-projectPointToNormalizedPlane(const MatrixBase<Derived>& _v)
-{
- MatrixSizeCheck<3, 1>::check(_v);
-
- typedef typename Derived::Scalar T;
- Matrix<T, 2, 1> up;
-
- projectPointToNormalizedPlane(_v, up);
-
- return up;
-}
-
-/**
- * Pin-hole canonical projection, return also distance (not depth!).
- * \param _v a 3D point
- * \param _up the projected point in the normalized 2D plane
- * \param _dist the distance from the camera to the point
- */
-template<typename Derived1, typename Derived2>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up,
- typename Derived2::Scalar& _dist)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
-
- projectPointToNormalizedPlane(_v, _up);
- _dist = _v.norm();
-}
-
-/**
- * Pin-hole canonical projection, with jacobian
- * \param _v the 3D point to project
- * \param _up the projected 2D point
- * \param _UP_v the Jacibian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up,
- MatrixBase<Derived3>& _UP_v)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
- MatrixSizeCheck<2, 3>::check(_UP_v);
-
- projectPointToNormalizedPlane(_v, _up);
-
- _UP_v(0, 0) = 1.0 / _v(2);
- _UP_v(0, 1) = 0.0;
- _UP_v(0, 2) = -_v(0) / (_v(2) * _v(2));
- _UP_v(1, 0) = 0.0;
- _UP_v(1, 1) = 1.0 / _v(2);
- _UP_v(1, 2) = -_v(1) / (_v(2) * _v(2));
-}
-
-/**
- * Pin-hole canonical projection, with distance (not depth!) and jacobian
- * \param _v the 3D point to project
- * \param _up the projected 2D point
- * \param _UP_v the Jacibian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up,
- typename Derived1::Scalar& _dist,
- MatrixBase<Derived3>& _UP_v)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
- MatrixSizeCheck<2, 3>::check(_UP_v);
-
- projectPointToNormalizedPlane(_v, _up, _UP_v);
-
- _dist = _v.norm();
-}
-
-/**
- * Canonical back-projection.
- * \param _u the 2D point in the image plane
- * \param _depth point's depth orthogonal to image plane. Defaults to 1.0
- * \return the back-projected 3D point at the given depth
- */
-template<typename Derived>
-Matrix<typename Derived::Scalar, 3, 1>
-backprojectPointFromNormalizedPlane(const MatrixBase<Derived> & _u,
- const typename Derived::Scalar _depth = 1)
-{
- MatrixSizeCheck<2,1>::check(_u);
-
- Matrix<typename Derived::Scalar, 3, 1> p;
-
- p << _depth*_u ,
- _depth;
-
- return p;
-}
-
-/**
- * Canonical back-projection.
- * \param u the 2D point in the image plane.
- * \param depth point's depth orthogonal to image plane.
- * \param p the 3D point.
- * \param P_u Jacobian of p wrt u.
- * \param P_depth Jacobian of p wrt depth.
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void backprojectPointFromNormalizedPlane(const MatrixBase<Derived1> & u,
- const typename Derived2::Scalar depth,
- MatrixBase<Derived2>& p,
- MatrixBase<Derived3>& P_u,
- MatrixBase<Derived4>& P_depth )
-{
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<3,1>::check(p);
- MatrixSizeCheck<3,2>::check(P_u);
- MatrixSizeCheck<3,1>::check(P_depth);
-
- p = backprojectPointFromNormalizedPlane(u, depth);
-
- P_u(0, 0) = depth;
- P_u(0, 1) = 0.0;
- P_u(1, 0) = 0.0;
- P_u(1, 1) = depth;
- P_u(2, 0) = 0.0;
- P_u(2, 1) = 0.0;
-
- P_depth(0, 0) = u(0);
- P_depth(1, 0) = u(1);
- P_depth(2, 0) = 1.0;
-}
-
-/**
- * Distortion factor for the model s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
- * \param d the distortion parameters vector
- * \param r2 the square of the radius to evaluate, r2 = r^2.
- * \return the distortion factor so that rd = s*r
- */
-template<typename Derived, typename T>
-T distortionFactor(const MatrixBase<Derived> & d,
- T r2)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-
- if (d.size() == 0)
- return (T)1.0;
- T s = (T)1.0;
- T r2i = (T)1.0;
- for (SizeEigen i = 0; i < d.size(); i++)
- { // here we are doing:
- r2i = r2i * r2; // r2i = r^(2*(i+1))
- s += d(i) * r2i; // s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
- }
- /*
- The model is not valid out of the image, and it can bring back landmarks very quickly after they got out.
- We should compute the point where the monotony changes by solving d/du u.f(u,v) = 0, but it would require a lot
- more computations to check the validity of the transform than to compute the distortion... and it should be done
- manually for every size of d (fortunately only 2 or 3 usually).
- So we just make a rough test on s, no camera should have a distortion greater than this, and anyway it will just
- prevent from observing some points on the borders of the image. In that case the landmark will seem to suddenly
- jump outside of the image.
- */
- if (s < 0.6)
- s = (T)1.0;
- return s;
-}
-
-/**
- * Correction factor for the model s = 1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...
- * \param c the correction parameters vector
- * \param r2 the square of the radius to evaluate, r2 = r^2.
- * \return the correction factor so that rc = s*r
- */
-template<typename Derived, typename T>
-T correctionFactor(const MatrixBase<Derived> & c,
- T r2)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-
- /*
- * Since we use the same polynomial kernel as for the distortion factor, we just call distortionFactor()
- */
- return distortionFactor(c, r2);
-}
-
-/**
- * Radial distortion: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + etc) * u
- * \param d the distortion parameters vector
- * \param up the point to distort
- * \return the distorted point
- */
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> distortPoint(const MatrixBase<Derived1> & d,
- const MatrixBase<Derived2> & up)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(up);
-
- SizeEigen n = d.size();
- if (n == 0)
- return up;
- else {
- typename Derived2::Scalar r2 = up(0) * up(0) + up(1) * up(1); // this is the norm squared: r2 = ||u||^2
- return distortionFactor(d, r2) * up;
- }
-}
-
-/**
- * Radial distortion: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + etc) * u, with jacobians
- * \param d the radial distortion parameters vector
- * \param up the point to distort
- * \param ud the distorted point
- * \param UD_up the Jacobian of \a ud wrt \a up
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void distortPoint(const MatrixBase<Derived1> & d,
- const MatrixBase<Derived2> & up,
- MatrixBase<Derived3> & ud,
- MatrixBase<Derived4> & UD_up)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(up);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,2>::check(UD_up);
-
- typedef typename Derived2::Scalar T;
-
- Matrix<T, 2, 1> R2_up;
- Matrix<T, 2, 1> S_up;
-
- SizeEigen n = d.size();
- if (n == 0) {
- ud = up;
- UD_up.setIdentity();
- }
-
- else {
- T r2 = up(0) * up(0) + up(1) * up(1); // this is the norm squared: r2 = ||u||^2
- T s = (T) 1.0;
- T r2i = (T) 1.0;
- T r2im1 = (T) 1.0; //r2*(i-1)
- T S_r2 = (T) 0.0;
-
- for (SizeEigen i = 0; i < n; i++) { //.. here we are doing:
- r2i = r2i * r2; //................. r2i = r^(2*(i+1))
- s += d(i) * r2i; //................ s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
-
- S_r2 = S_r2 + (i + 1) * d(i) * r2im1; //jacobian of s wrt r2 : S_r2 = d_0 + 2 * d1 * r^2 + 3 * d_2 * r^4 + ...
- r2im1 = r2im1 * r2;
- }
-
- if (s < (T)0.6) s = (T)1.0; // because the model is not valid too much out of the image, avoid to wrongly bring them back in the field of view
- // see extensive note in distortionFactor()
- ud = s * up; // finally ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...) * u;
-
- R2_up(0) = 2 * up(0);
- R2_up(1) = 2 * up(1);
-
- S_up(0) = R2_up(0) * S_r2;
- S_up(1) = R2_up(1) * S_r2;
-
- UD_up(0, 0) = S_up(0) * up(0) + s;
- UD_up(0, 1) = S_up(1) * up(0);
- UD_up(1, 0) = S_up(0) * up(1);
- UD_up(1, 1) = S_up(1) * up(1) + s;
- }
-
-}
-
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> undistortPoint(const MatrixBase<Derived1>& c,
- const MatrixBase<Derived2>& ud)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(ud);
-
- SizeEigen n = c.size();
- if (n == 0)
- return ud;
- else {
- typename Derived2::Scalar r2 = ud(0) * ud(0) + ud(1) * ud(1); // this is the norm squared: r2 = ||u||^2
- return correctionFactor(c, r2) * ud;
- }
-}
-
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void undistortPoint(const MatrixBase<Derived1>& c,
- const MatrixBase<Derived2>& ud,
- MatrixBase<Derived3>& up,
- MatrixBase<Derived4>& UP_ud)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,1>::check(up);
- MatrixSizeCheck<2,2>::check(UP_ud);
-
- typedef typename Derived1::Scalar T;
-
- SizeEigen n = c.size();
- Matrix<typename Derived4::Scalar, 2, 1> R2_ud;
- Matrix<typename Derived4::Scalar, 2, 1> S_ud;
-
- if (n == 0)
- {
- up = ud;
- UP_ud.setIdentity();
- }
-
- else
- {
- T r2 = ud(0) * ud(0) + ud(1) * ud(1); // this is the norm squared: r2 = ||u||^2
- T s = (T)1.0;
- T r2i = (T)1.0;
- T r2im1 = (T)1.0; //r2*(i-1)
- T S_r2 = (T)0.0;
-
- for (SizeEigen i = 0; i < n; i++)
- { //.. here we are doing:
- r2i = r2i * r2; //................. r2i = r^(2*(i+1))
- s += c(i) * r2i; //................ s = 1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...
-
- S_r2 = S_r2 + (i + 1) * c(i) * r2im1; //jacobian of s wrt r2 : S_r2 = c_0 + 2 * d1 * r^2 + 3 * c_2 * r^4 + ...
- r2im1 = r2im1 * r2;
- }
- if (s < (T)0.6) s = (T)1.0; // because the model is not valid too much out of the image, avoid to wrongly bring them back in the field of view
- // see extensive note in distortionFactor()
-
- up = s * ud; // finally up = (1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...) * u;
-
- R2_ud(0) = 2 * ud(0);
- R2_ud(1) = 2 * ud(1);
-
- S_ud(0) = R2_ud(0) * S_r2;
- S_ud(1) = R2_ud(1) * S_r2;
-
- UP_ud(0, 0) = S_ud(0) * ud(0) + s;
- UP_ud(0, 1) = S_ud(1) * ud(0);
- UP_ud(1, 0) = S_ud(0) * ud(1);
- UP_ud(1, 1) = S_ud(1) * ud(1) + s;
- }
-}
-
-/**
- * Pixellization from k = [u_0, v_0, a_u, a_v]
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param ud the point to pixellize, adimensional
- * \return the point in pixels coordinates
- */
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> pixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& ud)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(ud);
-
- typedef typename Derived2::Scalar T;
-
- T u_0 = k(0);
- T v_0 = k(1);
- T a_u = k(2);
- T a_v = k(3);
- Matrix<T, 2, 1> u;
-
- u(0) = u_0 + a_u * ud(0);
- u(1) = v_0 + a_v * ud(1);
-
- return u;
-}
-
-/**
- * Pixellization from k = [u_0, v_0, a_u, a_v] with jacobians
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param ud the point to pixellize, adimensional
- * \param u the pixellized point
- * \param U_ud the Jacobian of \a u wrt \a ud
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void pixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& ud,
- MatrixBase<Derived3>& u,
- MatrixBase<Derived4>& U_ud)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,2>::check(U_ud);
-
- typedef typename Derived1::Scalar T;
-
- u = pixellizePoint(k, ud);
-
- T a_u = k(2);
- T a_v = k(3);
-
- U_ud(0, 0) = a_u;
- U_ud(0, 1) = 0;
- U_ud(1, 0) = 0;
- U_ud(1, 1) = a_v;
-}
-
-/**
- * Depixellization from k = [u_0, v_0, a_u, a_v]
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param u the point to depixellize, in pixels
- * \return the depixellized point, adimensional
- */
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> depixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& u)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
-
- typedef typename Derived1::Scalar T;
-
- T u_0 = k(0);
- T v_0 = k(1);
- T a_u = k(2);
- T a_v = k(3);
- Matrix<typename Derived2::Scalar, 2, 1> ud;
-
- ud(0) = (u(0) - u_0) / a_u;
- ud(1) = (u(1) - v_0) / a_v;
-
- return ud;
-}
-
-/**
- * Depixellization from k = [u_0, v_0, a_u, a_v], with Jacobians
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param u the point to depixellize, in pixels
- * \param ud the depixellized point
- * \param UD_u the Jacobian of \a ud wrt \a u
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void depixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& u,
- MatrixBase<Derived3>& ud,
- MatrixBase<Derived4>& UD_u)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,2>::check(UD_u);
-
- typedef typename Derived1::Scalar T;
- ud = depixellizePoint(k, u);
-
- T a_u = k(2);
- T a_v = k(3);
-
- UD_u(0, 0) = 1.0 / a_u;
- UD_u(0, 1) = 0.0;
- UD_u(1, 0) = 0.0;
- UD_u(1, 1) = 1.0 / a_v;
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \return the projected and distorted point
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-Matrix<typename Derived3::Scalar, 2, 1> projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v)
-{
- MatrixSizeCheck<4,1>::check(k);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
-
- return pixellizePoint( k, distortPoint( d, projectPointToNormalizedPlane( v )));
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \param u the projected and distorted point
- * \param U_v the Jacobian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>
-void projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v,
- MatrixBase<Derived4>& u,
- MatrixBase<Derived5>& U_v)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,3>::check(U_v);
-
- Matrix<typename Derived4::Scalar, 2, 1> up, ud;
- Matrix<typename Derived5::Scalar, 2, 3> UP_v; /// Check this one -> mat23
- Matrix<typename Derived5::Scalar, 2, 2> UD_up, U_ud;
-
- projectPointToNormalizedPlane (v, up, UP_v);
- distortPoint (d, up, ud, UD_up);
- pixellizePoint (k, ud, u, U_ud);
-
- U_v.noalias() = U_ud * UD_up * UP_v;
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion.
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \param u the projected and distorted point
- * \param dist distance from the optical center to the 3D point
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>//, typename T>
-void projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v,
- MatrixBase<Derived4>& u,
- typename Derived3::Scalar& dist)
-{
- MatrixSizeCheck<4,1>::check(k);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
- MatrixSizeCheck<2,1>::check(u);
-
- Matrix<typename Derived4::Scalar, 2, 1> up;
- projectPointToNormalizedPlane(v, up, dist);
- u = pixellizePoint( k, distortPoint( d, up ));
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \param u the projected and distorted point
- * \param dist the distance from the camera to the point
- * \param U_v the Jacobian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>
-void projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v,
- MatrixBase<Derived4>& u,
- typename Derived3::Scalar& dist,
- MatrixBase<Derived5>& U_v)
-{
- MatrixSizeCheck<4,1>::check(k);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,3>::check(U_v);
-
- Matrix<typename Derived4::Scalar, 2, 1> up, ud;
- Matrix<typename Derived5::Scalar, 2, 3> UP_v;
- Matrix<typename Derived5::Scalar, 2, 2> UD_up, U_ud;
-
- projectPointToNormalizedPlane (v, up, dist, UP_v);
- distortPoint (d, up, ud, UD_up);
- pixellizePoint (k, ud, u, U_ud);
-
- U_v.noalias() = U_ud * UD_up * UP_v;
-}
-
-/**
- * Back-Project a point from a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param c the radial undistortion parameters vector
- * \param u the 2D pixel
- * \param depth the depth prior
- * \return the back-projected 3D point
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-Matrix<typename Derived3::Scalar, 3, 1> backprojectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& c,
- const MatrixBase<Derived3>& u,
- const typename Derived3::Scalar& depth = 1.0)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
-
- return backprojectPointFromNormalizedPlane(undistortPoint(c, depixellizePoint(k, u)), depth);
-}
-
-/**
- * Back-Project a point from a pin-hole camera with radial distortion; give Jacobians
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param c the radial undistortion parameters vector
- * \param u the 2D pixel
- * \param depth the depth prior
- * \param p the back-projected 3D point
- * \param P_u Jacobian of p wrt u
- * \param P_depth Jacobian of p wrt depth
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6>
-void backprojectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& c,
- const MatrixBase<Derived3>& u,
- const typename Derived4::Scalar& depth,
- MatrixBase<Derived4>& p,
- MatrixBase<Derived5>& P_u,
- MatrixBase<Derived6>& P_depth)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<3,1>::check(p);
- MatrixSizeCheck<3,2>::check(P_u);
- MatrixSizeCheck<3,1>::check(P_depth);
-
- Matrix<typename Derived3::Scalar, 2, 1> up, ud;
- Matrix<typename Derived5::Scalar, 3, 2> P_up;
- Matrix<typename Derived5::Scalar, 2, 2> UP_ud, UD_u;
- depixellizePoint(k, u, ud, UD_u);
- undistortPoint(c, ud, up, UP_ud);
- backprojectPointFromNormalizedPlane(up, depth, p, P_up, P_depth);
-
- P_u.noalias() = P_up * UP_ud * UD_u;
-}
-
-/**
- * Determine if a pixel is inside the region of interest
- * \param pix the pixel to test
- * \param x the region of interest, top-left x
- * \param y the region of interest, top-left y
- * \param width the region of interest width
- * \param height the region of interest height
- */
-template<typename VPix>
-bool isInRoi(const MatrixBase<VPix> & pix, const int x, const int y, const int width, const int height) {
- return ((pix(0) >= x) && (pix(0) <= x + width - 1) && (pix(1) >= y) && (pix(1) <= y + height - 1));
-}
-
-/**
- * Determine if a pixel is inside the image
- * \param pix the pixel to test
- * \param width the image width, in pixels
- * \param height the image height, in pixels
- */
-template<typename VPix>
-bool isInImage(const MatrixBase<VPix> & pix, const int & width, const int & height) {
- return isInRoi(pix, 0, 0, width, height);
-}
-
-/**
- * Compute distortion correction parameters.
- *
- * This method follows the one in Joan Sola's thesis [1], pag 46--49.
- * \param k the intrinsic parameters vector
- * \param d the distortion parameters vector
- * \param c the correction parameters vector. Provide it with the desired size.
- */
-template<class Vk, class Vd, class Vc>
-void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
-
-{
- SizeEigen size = c.size();
-
- if (size != 0)
- {
-
- Scalar r_max = sqrt(k(0) * k(0) / (k(2) * k(2)) + k(1) * k(1) / (k(3) * k(3)));
- Scalar rd_max = 1.2 * r_max;
-
- SizeEigen N_samples = 200; // number of samples
- Scalar iN_samples = 1 / (Scalar)N_samples;
- Scalar rd_n, rc_2, rd_2;
- Eigen::VectorXs rd(N_samples + 1), rc(N_samples + 1);
- Eigen::MatrixXs Rd(N_samples + 1, size);
-
- for (SizeEigen sample = 0; sample <= N_samples; sample++)
- {
-
- rc(sample) = sample * rd_max * iN_samples; // sample * rd_max / N_samples
- rc_2 = rc(sample) * rc(sample);
- rd(sample) = distortionFactor(d, rc_2) * rc(sample);
- rd_2 = rd(sample) * rd(sample);
-
- rd_n = rd(sample); // start with rd
-
- for (SizeEigen order = 0; order < size; order++)
- {
- rd_n *= rd_2; // increment:
- Rd(sample, order) = rd_n; // we have : rd^3, rd^5, rd^7, ...
- }
- }
-
- // solve Rd*c = (rc-rd) for c, with least-squares SVD method:
- // the result is c = pseudo_inv(Rd)*(rc-rd)
- // with pseudo_inv(Rd) = (Rd'*Rd)^-1 * Rd'
-
- // this does not work:
- // jmath::LinearSolvers::solve_Cholesky(Rd, (rc - rd), c);
-
- // therefore we solve manually the pseudo-inverse:
- Eigen::MatrixXs RdtRd(size, size);
- RdtRd = Rd.transpose() * Rd;
- Eigen::MatrixXs iRdtRd(size, size);
- //jmath::ublasExtra::inv(RdtRd, iRdtRd);
- // I understood that iRdtRd is the inverse of RdtRd)
- iRdtRd = RdtRd.inverse();
- Eigen::MatrixXs iRd = iRdtRd * Rd.transpose();
-
- c = iRd * (rc - rd);
- }
-}
-
-} // namespace pinhole
-
-} // namespace wolf
-
-#endif // PINHOLETOOLS_H
diff --git a/include/base/problem.h b/include/base/problem.h
deleted file mode 100644
index a9f8e5e4a28c857a4061ec2296cb81cf31858dfa..0000000000000000000000000000000000000000
--- a/include/base/problem.h
+++ /dev/null
@@ -1,321 +0,0 @@
-#ifndef PROBLEM_H_
-#define PROBLEM_H_
-
-// Fwd refs
-namespace wolf{
-class HardwareBase;
-class TrajectoryBase;
-class MapBase;
-class ProcessorMotion;
-class StateBlock;
-class TimeStamp;
-struct IntrinsicsBase;
-struct ProcessorParamsBase;
-}
-
-//wolf includes
-#include "base/wolf.h"
-#include "base/frame_base.h"
-#include "base/state_block.h"
-
-// std includes
-
-namespace wolf {
-
-enum Notification
-{
- ADD,
- REMOVE
-};
-
-/** \brief Wolf problem node element in the Wolf Tree
- */
-class Problem : public std::enable_shared_from_this<Problem>
-{
-
- protected:
- HardwareBasePtr hardware_ptr_;
- TrajectoryBasePtr trajectory_ptr_;
- MapBasePtr map_ptr_;
- ProcessorMotionPtr processor_motion_ptr_;
- StateBlockPtrList state_block_list_;
- std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
- SizeEigen state_size_, state_cov_size_, dim_;
- std::map<FactorBasePtr, Notification> factor_notification_map_;
- std::map<StateBlockPtr, Notification> state_block_notification_map_;
- bool prior_is_set_;
-
- private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(const std::string& _frame_structure); // USE create() below !!
- void setup();
-
- public:
- static ProblemPtr create(const std::string& _frame_structure); // USE THIS AS A CONSTRUCTOR!
- virtual ~Problem();
-
- // Properties -----------------------------------------
- SizeEigen getFrameStructureSize() const;
- void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
- SizeEigen getDim() const;
-
- // Hardware branch ------------------------------------
- HardwareBasePtr getHardware();
- void addSensor(SensorBasePtr _sen_ptr);
-
- /** \brief Factory method to install (create and add) sensors only from its properties
- * \param _sen_type type of sensor
- * \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
- * \param _extrinsics a vector of extrinsic parameters: size 2 for 2D position, 3 for 2D pose, 3 for 3D position, 7 for 3D pose.
- * \param _intrinsics a base-pointer to a derived struct defining the intrinsic parameters.
- */
- SensorBasePtr installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
- IntrinsicsBasePtr _intrinsics = nullptr);
-
- /** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file
- * \param _sen_type type of sensor
- * \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
- * \param _extrinsics a vector of extrinsic parameters: size 2 for 2D position, 3 for 2D pose, 3 for 3D position, 7 for 3D pose.
- * \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in IntrinsicsFactory under the key _sen_type.
- */
- SensorBasePtr installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
- const std::string& _intrinsics_filename);
-
- /** \brief get a sensor pointer by its name
- * \param _sensor_name The sensor name, as it was installed with installSensor()
- */
- SensorBasePtr getSensor(const std::string& _sensor_name);
-
- /** \brief Factory method to install (create, and add to sensor) processors only from its properties
- *
- * This method creates a Processor, and adds it to the specified sensor's list of processors
- * \param _prc_type type of processor
- * \param _unique_processor_name unique processor name, used to identify the particular instance of the processor
- * \param _corresponding_sensor_ptr pointer to the sensor where the processor will be installed.
- * \param _prc_params a base-pointer to a derived struct defining the processor parameters.
- */
- ProcessorBasePtr installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- SensorBasePtr _corresponding_sensor_ptr, //
- ProcessorParamsBasePtr _prc_params = nullptr);
-
- /** \brief Factory method to install (create, and add to sensor) processors only from its properties
- *
- * This method creates a Processor, and adds it to the specified sensor's list of processors
- *
- * This method is a helper wrapper around the version accepting a sensor pointer instead of a sensor name.
- * \param _prc_type type of processor
- * \param _unique_processor_name unique processor name, used to identify the particular instance of the processor
- * \param _corresponding_sensor_name corresponding sensor name, used to bind the processor to the particular instance of the sensor
- * \param _params_filename name of formatted file (xml, yaml, etc) defining the processor parameters.
- */
- ProcessorBasePtr installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- const std::string& _corresponding_sensor_name, //
- const std::string& _params_filename = "");
-
- /** \brief Set the processor motion
- *
- * Set the processor motion.
- */
- void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
- ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
- void clearProcessorMotion();
- ProcessorMotionPtr& getProcessorMotion();
-
- // Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectory();
- virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
- const Eigen::MatrixXs& _prior_cov, //
- const TimeStamp& _ts,
- const Scalar _time_tolerance);
-
- /** \brief Emplace frame from string frame_structure
- * \param _frame_structure String indicating the frame structure.
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
- * \param _time_stamp Time stamp of the frame
- *
- * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
- * - Create a Frame
- * - Add it to Trajectory
- * - If it is key-frame, update state-block lists in Problem
- */
- FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
- FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
- const TimeStamp& _time_stamp);
-
- /** \brief Emplace frame from string frame_structure without state
- * \param _frame_structure String indicating the frame structure.
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
- * \param _time_stamp Time stamp of the frame
- *
- * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
- * - Create a Frame
- * - Add it to Trajectory
- * - If it is key-frame, update state-block lists in Problem
- */
- FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
- FrameType _frame_key_type, //
- const TimeStamp& _time_stamp);
-
- /** \brief Emplace frame from string frame_structure without structure
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
- * \param _time_stamp Time stamp of the frame
- *
- * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
- * - Create a Frame
- * - Add it to Trajectory
- * - If it is key-frame, update state-block lists in Problem
- */
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
- const TimeStamp& _time_stamp);
-
- /** \brief Emplace frame from string frame_structure without structure nor state
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
- * \param _time_stamp Time stamp of the frame
- *
- * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
- * - Create a Frame
- * - Add it to Trajectory
- * - If it is key-frame, update state-block lists in Problem
- */
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp);
-
- // Frame getters
- FrameBasePtr getLastFrame ( );
- FrameBasePtr getLastKeyFrame ( );
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
-
- /** \brief Give the permission to a processor to create a new keyFrame
- *
- * This should implement a black list of processors that have forbidden keyframe creation.
- * - This decision is made at problem level, not at processor configuration level.
- * - Therefore, if you want to permanently configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
- */
- bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
-
- /** \brief New key frame callback
- *
- * New key frame callback: It should be called by any processor that creates a new keyframe. It calls the keyFrameCallback of the rest of processors.
- */
- void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
- ProcessorBasePtr _processor_ptr, //
- const Scalar& _time_tolerance);
-
- // State getters
- Eigen::VectorXs getCurrentState ( );
- void getCurrentState (Eigen::VectorXs& state);
- void getCurrentStateAndStamp (Eigen::VectorXs& state, TimeStamp& _ts);
- Eigen::VectorXs getState (const TimeStamp& _ts);
- void getState (const TimeStamp& _ts, Eigen::VectorXs& state);
- // Zero state provider
- Eigen::VectorXs zeroState ( );
- bool priorIsSet() const;
-
- // Map branch -----------------------------------------
- MapBasePtr getMap();
- LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
- void addLandmarkList(LandmarkBasePtrList& _lmk_list);
- void loadMap(const std::string& _filename_dot_yaml);
- void saveMap(const std::string& _filename_dot_yaml, //
- const std::string& _map_name = "Map automatically saved by Wolf");
-
- // Covariances -------------------------------------------
- void clearCovariance();
- void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov);
- void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov);
- bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0, const int _col=0);
- bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
- bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
- bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
- bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
- bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
-
- // Solver management ----------------------------------------
-
- /** \brief Gets a reference to the state blocks list
- */
- StateBlockPtrList& getStateBlockPtrList();
-
- /** \brief Notifies a new state block to be added to the solver manager
- */
- StateBlockPtr addStateBlock(StateBlockPtr _state_ptr);
-
- /** \brief Notifies a state block to be removed from the solver manager
- */
- void removeStateBlock(StateBlockPtr _state_ptr);
-
- /** \brief Gets a map of factor notification to be handled by the solver
- */
- std::map<StateBlockPtr,Notification>& getStateBlockNotificationMap();
-
- /** \brief Notifies a new factor to be added to the solver manager
- */
- FactorBasePtr addFactor(FactorBasePtr _factor_ptr);
-
- /** \brief Notifies a factor to be removed from the solver manager
- */
- void removeFactor(FactorBasePtr _factor_ptr);
-
- /** \brief Gets a map of factor notification to be handled by the solver
- */
- std::map<FactorBasePtr, Notification>& getFactorNotificationMap();
-
- // Print and check ---------------------------------------
- /**
- * \brief print wolf tree
- * \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: 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 metric : show metric info (status, time stamps, state vectors, measurements)
- * \param state_blocks : show state blocks
- */
- void print(int depth = 4, //
- bool constr_by = false, //
- bool metric = true, //
- bool state_blocks = false);
- void print(const std::string& depth, //
- bool constr_by = false, //
- bool metric = true, //
- bool state_blocks = false);
- bool check(int verbose_level = 0);
-
-};
-
-} // namespace wolf
-
-// IMPLEMENTATION
-
-namespace wolf
-{
-
-inline bool Problem::priorIsSet() const
-{
- return prior_is_set_;
-}
-
-inline ProcessorMotionPtr& Problem::getProcessorMotion()
-{
- return processor_motion_ptr_;
-}
-
-inline std::map<StateBlockPtr,Notification>& Problem::getStateBlockNotificationMap()
-{
- return state_block_notification_map_;
-}
-
-inline std::map<FactorBasePtr,Notification>& Problem::getFactorNotificationMap()
-{
- return factor_notification_map_;
-}
-
-} // namespace wolf
-
-#endif // PROBLEM_H
diff --git a/include/base/processor/polyline_2D_utils.h b/include/base/processor/polyline_2D_utils.h
deleted file mode 100644
index 78d6b960ac3a363d9f135c6da2279a32472fc81f..0000000000000000000000000000000000000000
--- a/include/base/processor/polyline_2D_utils.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * polyline_2D_utils.h
- *
- * Created on: Mar 14, 2019
- * Author: jvallve
- */
-
-#ifndef POLYLINE_2D_UTILS_H_
-#define POLYLINE_2D_UTILS_H_
-
-#include "base/wolf.h"
-#include "base/landmark/landmark_match_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/feature/feature_match_polyline_2D.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/rotations.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(MatchPolyline2D);
-typedef std::map<Scalar,MatchPolyline2DPtr> MatchPolyline2DMap;
-typedef std::list<MatchPolyline2DPtr> MatchPolyline2DList;
-
-/** \brief Match between a two polylines (not specialized to landmark nor feature)
- *
- * Match between a two polylines (not specialized to landmark nor feature)
- *
- **/
-struct MatchPolyline2D
-{
- int from_A_id_;
- int to_A_id_;
- int from_B_id_;
- int to_B_id_;
- int normalized_score_;
- Eigen::Matrix3s T_A_B_;
-};
-
-Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta);
-Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose);
-Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T);
-
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last, const bool& first_defined = true, const bool& last_defined=true);
-
-/** \brief Computes the squared distance from a point to a segment defined by two points
- **/
-Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b);
-
-Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict=false);
-
-MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error );
-
-MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error );
-
-bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B);
-
-Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
-
-} // namespace wolf
-
-#endif /* POLYLINE_2D_UTILS_H_ */
diff --git a/include/base/processor/processor_GPS.h b/include/base/processor/processor_GPS.h
deleted file mode 100644
index 35658410c4a0565c7da75976d72766b377e1712e..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_GPS.h
+++ /dev/null
@@ -1,41 +0,0 @@
-//
-// Created by ptirindelli on 16/12/15.
-//
-
-#ifndef WOLF_PROCESSOR_GPS_H
-#define WOLF_PROCESSOR_GPS_H
-
-// Wolf includes
-#include "base/processor/processor_base.h"
-#include "base/capture/capture_GPS.h"
-
-// Std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorGPS);
-
-//class
-class ProcessorGPS : public ProcessorBase
-{
- protected:
- CaptureGPSPtr capture_gps_ptr_;
- Scalar gps_covariance_;
-
- public:
- ProcessorGPS(ProcessorParamsBasePtr _params);
- virtual ~ProcessorGPS();
- virtual void configure(SensorBasePtr _sensor) { };
- virtual void init(CaptureBasePtr _capture_ptr);
- virtual void process(CaptureBasePtr _capture_ptr);
- virtual bool voteForKeyFrame();
- virtual void keyFrameCallback(FrameBasePtr, const Scalar& _time_tol);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-
-};
-
-} // namespace wolf
-
-#endif //WOLF_PROCESSOR_GPS_H
diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h
deleted file mode 100644
index 53204c6a00bce51200b555c2e2fff61f189c4599..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_IMU.h
+++ /dev/null
@@ -1,85 +0,0 @@
-#ifndef PROCESSOR_IMU_H
-#define PROCESSOR_IMU_H
-
-// Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/feature/feature_IMU.h"
-#include "base/processor/processor_motion.h"
-
-namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
-
-struct ProcessorParamsIMU : public ProcessorParamsMotion
-{
- //
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorIMU);
-
-//class
-class ProcessorIMU : public ProcessorMotion{
- public:
- ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU);
- virtual ~ProcessorIMU();
- virtual void configure(SensorBasePtr _sensor) override { };
-
- protected:
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta) override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta,
- Eigen::MatrixXs& _jacobian_delta_preint,
- Eigen::MatrixXs& _jacobian_delta) override;
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta ) override;
- virtual Eigen::VectorXs deltaZero() const override;
- virtual Motion interpolate(const Motion& _motion_ref,
- Motion& _motion,
- TimeStamp& _ts) override;
- virtual bool voteForKeyFrame() override;
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) override;
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
-
- protected:
- ProcessorParamsIMUPtr params_motion_IMU_;
-
- public:
- //for factory
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-}
-
-/////////////////////////////////////////////////////////
-// IMPLEMENTATION. Put your implementation includes here
-/////////////////////////////////////////////////////////
-
-namespace wolf{
-
-inline Eigen::VectorXs ProcessorIMU::deltaZero() const
-{
- return (Eigen::VectorXs(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
-}
-
-} // namespace wolf
-
-#endif // PROCESSOR_IMU_H
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
deleted file mode 100644
index 84fee5343c63ec3c01b28de1dfbc46c37125f337..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_base.h
+++ /dev/null
@@ -1,267 +0,0 @@
-#ifndef PROCESSOR_BASE_H_
-#define PROCESSOR_BASE_H_
-
-// Fwd refs
-namespace wolf{
-class SensorBase;
-}
-
-// Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
-#include "base/frame_base.h"
-
-// std
-#include <memory>
-#include <map>
-
-namespace wolf {
-
-/** \brief Key frame class pack
- *
- * To store a key_frame with an associated time tolerance.
- *
- * Used in keyframe callbacks as the minimal pack of information needed by the processor receiving the callback.
- */
-class PackKeyFrame
-{
- public:
- PackKeyFrame(const FrameBasePtr _key_frame, const Scalar _time_tolerance) : key_frame(_key_frame), time_tolerance(_time_tolerance) {};
- ~PackKeyFrame(){};
- FrameBasePtr key_frame;
- Scalar time_tolerance;
-};
-
-WOLF_PTR_TYPEDEFS(PackKeyFrame);
-
-/** \brief Buffer of Key frame class objects
- *
- * Object and functions to manage a buffer of KFPack objects.
- */
-class PackKeyFrameBuffer
-{
- public:
-
- typedef std::map<TimeStamp,PackKeyFramePtr>::iterator Iterator; // buffer iterator
-
- PackKeyFrameBuffer(void);
- ~PackKeyFrameBuffer(void);
-
- /**\brief Select a Pack from the buffer
- *
- * Select from the buffer the closest pack (w.r.t. time stamp),
- * respecting a defined time tolerances
- */
- PackKeyFramePtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
- PackKeyFramePtr selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
-
- PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
- PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
-
- /**\brief Buffer size
- *
- */
- SizeStd size(void);
-
- /**\brief Add a pack to the buffer
- *
- */
- void add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance);
-
- /**\brief Remove all packs in the buffer with a time stamp older than the specified
- *
- */
- void removeUpTo(const TimeStamp& _time_stamp);
-
- /**\brief Check time tolerance
- *
- * Check if the time distance between two time stamps is smaller than
- * the minimum time tolerance of the two frames.
- */
- bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
-
- /**\brief Clear the buffer
- *
- */
- void clear();
-
- /**\brief Empty the buffer
- *
- */
- bool empty();
-
- /**\brief Print buffer information
- *
- */
- void print();
-
- private:
-
- std::map<TimeStamp,PackKeyFramePtr> container_; // Main buffer container
-};
-
-/** \brief base struct for processor parameters
- *
- * Derive from this struct to create structs of processor parameters.
- */
-struct ProcessorParamsBase
-{
- ProcessorParamsBase() = default;
-
- ProcessorParamsBase(bool _voting_active,
- Scalar _time_tolerance)
- : voting_active(_voting_active)
- , time_tolerance(_time_tolerance)
- {
- //
- }
-
- virtual ~ProcessorParamsBase() = default;
-
- bool voting_active = false;
-
- ///< maximum time difference between a Keyframe time stamp and
- /// a particular Capture of this processor to allow assigning
- /// this Capture to the Keyframe.
- Scalar time_tolerance = Scalar(0);
-};
-
-//class ProcessorBase
-class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
-{
- protected:
- unsigned int processor_id_;
- ProcessorParamsBasePtr params_;
- PackKeyFrameBuffer kf_pack_buffer_;
-
- private:
- SensorBaseWPtr sensor_ptr_;
-
- static unsigned int processor_id_count_;
-
- public:
- ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params);
- virtual ~ProcessorBase();
- virtual void configure(SensorBasePtr _sensor) = 0;
- virtual void remove();
-
- unsigned int id();
-
- virtual void process(CaptureBasePtr _capture_ptr) = 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!
- */
- virtual bool voteForKeyFrame() = 0;
-
- virtual bool permittedKeyFrame() final;
-
- /**\brief make a Frame with the provided Capture
- *
- * Provide the following functionality:
- * - Construct a Frame,
- * - Put it in the Trajectory, and
- * - Add the provided capture on it.
- */
- FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr);
-
- /**\brief make a Frame with the provided Capture
- *
- * Provide the following functionality:
- * - Construct a Frame,
- * - Set its state vector
- * - Put it in the Trajectory, and
- * - Add the provided capture on it.
- */
- FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state);
-
- virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
-
- SensorBasePtr getSensor();
- const SensorBasePtr getSensor() const;
- void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
-
- virtual bool isMotion();
-
- void setTimeTolerance(Scalar _time_tolerance);
-
- bool isVotingActive() const;
-
- void setVotingActive(bool _voting_active = true);
-};
-
-inline bool ProcessorBase::isVotingActive() const
-{
- return params_->voting_active;
-}
-
-inline void ProcessorBase::setVotingActive(bool _voting_active)
-{
- params_->voting_active = _voting_active;
-}
-
-}
-
-#include "base/sensor/sensor_base.h"
-#include "base/factor/factor_base.h"
-
-namespace wolf {
-
-inline bool ProcessorBase::isMotion()
-{
- return false;
-}
-
-inline unsigned int ProcessorBase::id()
-{
- return processor_id_;
-}
-
-inline SensorBasePtr ProcessorBase::getSensor()
-{
- return sensor_ptr_.lock();
-}
-
-inline const SensorBasePtr ProcessorBase::getSensor() const
-{
- return sensor_ptr_.lock();
-}
-
-inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
-{
- params_->time_tolerance = _time_tolerance;
-}
-
-inline PackKeyFrameBuffer::PackKeyFrameBuffer(void)
-{
-
-}
-
-inline PackKeyFrameBuffer::~PackKeyFrameBuffer(void)
-{
-
-}
-
-inline void PackKeyFrameBuffer::clear()
-{
- container_.clear();
-}
-
-inline bool PackKeyFrameBuffer::empty()
-{
- return container_.empty();
-}
-
-inline SizeStd PackKeyFrameBuffer::size(void)
-{
- return container_.size();
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/processor/processor_capture_holder.h b/include/base/processor/processor_capture_holder.h
deleted file mode 100644
index 9067b4cae4870f3c53cc7400e7d943ae20822a42..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_capture_holder.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/**
- * \file processor_capture_holder.h
- *
- * Created on: Jul 12, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_CAPTURE_HOLDER_H_
-#define _WOLF_PROCESSOR_CAPTURE_HOLDER_H_
-
-//Wolf includes
-#include "base/processor/processor_base.h"
-#include "base/capture/capture_base.h"
-#include "base/capture/capture_buffer.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorCaptureHolder);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder);
-
-/**
- * \brief ProcessorParamsCaptureHolder
- */
-struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
-{
- Scalar buffer_size = 30;
-};
-
-/**
- * \brief ProcessorCaptureHolder
- */
-class ProcessorCaptureHolder : public ProcessorBase
-{
-public:
-
- ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder);
- virtual ~ProcessorCaptureHolder() = default;
- virtual void configure(SensorBasePtr _sensor) override { };
-
- virtual void process(CaptureBasePtr _capture_ptr) override;
-
- /** \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!
- */
- virtual bool voteForKeyFrame() override { return false; }
-
- virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tolerance) override;
-
- /**
- * \brief Finds the capture that contains the closest previous motion of _ts
- * \return a pointer to the capture (if it exists) or a nullptr (otherwise)
- */
- CaptureBasePtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
-
-protected:
-
- ProcessorParamsCaptureHolderPtr params_capture_holder_;
- CaptureBuffer buffer_;
-
-public:
-
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr sensor_ptr = nullptr);
-};
-
-} // namespace wolf
-
-#endif // _WOLF_PROCESSOR_CAPTURE_HOLDER_H_
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
deleted file mode 100644
index f545972b8555a6de4bff794dc87a2a02a3dec497..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_diff_drive.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/**
- * \file processor_diff_drive.h
- *
- * Created on: Oct 15, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_
-#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
-
-#include "base/processor/processor_motion.h"
-#include "base/diff_drive_tools.h"
-#include "base/sensor/sensor_diff_drive.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
-
-struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
-{
-// ProcessorParamsDiffDrive(const Scalar _time_tolerance,
-// const Scalar _dist_travel_th,
-// const Scalar _theta_traveled_th,
-// const Scalar _cov_det_th,
-// const Scalar _unmeasured_perturbation_std = 0.0001) :
-// dist_traveled_th_(_dist_travel_th),
-// theta_traveled_th_(_theta_traveled_th),
-// cov_det_th_(_cov_det_th),
-// unmeasured_perturbation_std_(_unmeasured_perturbation_std)
-// {
-// time_tolerance = _time_tolerance;
-// }
- Scalar unmeasured_perturbation_std = 0.0001;
-};
-
-/**
- * @brief The ProcessorDiffDrive class.
- *
- * Velocity motion model.
- *
- * Integrate odometry from joint position.
- *
- * velocity : data is [d_vx, d_w]
- * position : data is [left_position_increment, right_position_increment]
- *
- * delta is [dx, dy, dtheta]
- */
-
-WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
-
-class ProcessorDiffDrive : public ProcessorMotion
-{
-public:
-
- ProcessorDiffDrive(SensorDiffDrivePtr _sensor_diff_drive_ptr, ProcessorParamsDiffDrivePtr _params);
-
- virtual ~ProcessorDiffDrive() = default;
- virtual void configure(SensorBasePtr _sensor) override { }
-
- virtual bool voteForKeyFrame() override;
-
-protected:
-
- SensorDiffDrivePtr sensor_diff_drive_ptr_;
- /// @brief Intrinsic params
- ProcessorParamsDiffDrivePtr params_motion_diff_drive_;
- MatrixXs unmeasured_perturbation_cov_;
-
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_delta_calib) override;
-
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) override;
-
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) override;
-
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta) override;
-
- virtual Eigen::VectorXs deltaZero() const override;
-
- virtual Motion interpolate(const Motion& _ref,
- Motion& _second,
- TimeStamp& _ts) override;
-
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) override;
-
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin) override;
-
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
-
-public:
-
- /// @brief Factory method
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr);
-};
-
-} // namespace wolf
-
-#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_H_ */
-
diff --git a/include/base/processor/processor_factory.h b/include/base/processor/processor_factory.h
deleted file mode 100644
index 8077aae495c053e229fc8fef08790f1ca6e3b099..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_factory.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/**
- * \file processor_factory.h
- *
- * Created on: May 4, 2016
- * \author: jsola
- */
-
-#ifndef PROCESSOR_FACTORY_H_
-#define PROCESSOR_FACTORY_H_
-
-namespace wolf
-{
-class ProcessorBase;
-struct ProcessorParamsBase;
-}
-
-// wolf
-#include "base/factory.h"
-
-// std
-
-namespace wolf
-{
-/** \brief Processor factory class
- *
- * This factory can create objects of classes deriving from ProcessorBase.
- *
- * Specific object creation is invoked by create(TYPE, params), and the TYPE of processor is identified with a string.
- * For example, the following processor types are implemented,
- * - "ODOM 3D" for ProcessorOdom3D
- * - "ODOM 2D" for ProcessorOdom2D
- * - "GPS" for ProcessorGPS
- *
- * The rule to make new TYPE strings unique is that you skip the prefix 'Processor' from your class name,
- * and you build a string in CAPITALS with space separators.
- * - ProcessorImageFeature -> ````"IMAGE"````
- * - ProcessorLaser2D -> ````"LASER 2D"````
- * - etc.
- *
- * The methods to create specific processors are called __creators__.
- * Creators must be registered to the factory before they can be invoked for processor creation.
- *
- * This documentation shows you how to:
- * - Access the Factory
- * - Register and unregister creators
- * - Create processors
- * - Write a processor creator for ProcessorOdom2D (example).
- *
- * #### Accessing the Factory
- * The ProcessorFactory class is a singleton: it can only exist once in your application.
- * To obtain an instance of it, use the static method get(),
- *
- * \code
- * ProcessorFactory::get()
- * \endcode
- *
- * You can then call the methods you like, e.g. to create a processor, you type:
- *
- * \code
- * ProcessorFactory::get().create(...); // see below for creating processors ...
- * \endcode
- *
- * #### Registering processor creators
- * Prior to invoking the creation of a processor of a particular type,
- * you must register the creator for this type into the factory.
- *
- * Registering processor creators into the factory is done through registerCreator().
- * You provide a processor type string (above), and a pointer to a static method
- * that knows how to create your specific processor, e.g.:
- *
- * \code
- * ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
- * \endcode
- *
- * The method ProcessorOdom2D::create() exists in the ProcessorOdom2D class as a static method.
- * All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type.
- * This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBasePtr,
- * that can be derived for each derived processor.
- *
- * Here is an example of ProcessorOdom2D::create() extracted from processor_odom_2D.h:
- *
- * \code
- * static ProcessorBasePtr create(const std::string& _name, ProcessorParamsBasePtr _params)
- * {
- * // cast _params to good type
- * ProcessorParamsOdom2D* params = (ProcessorParamsOdom2D*)_params;
- *
- * ProcessorBasePtr prc = new ProcessorOdom2D(params);
- * prc->setName(_name); // pass the name to the created ProcessorOdom2D.
- * return prc;
- * }
- * \endcode
- *
- * #### Achieving automatic registration
- * Currently, registering is performed in each specific ProcessorXxxx source file, processor_xxxx.cpp.
- * For example, in processor_odom_2D.cpp we find the line:
- *
- * \code
- * const bool registered_odom_2D = ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
- * \endcode
- *
- * which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2D class).
- * Therefore, at application level, all processors that have a .cpp file compiled are automatically registered.
- *
- * #### Unregister processor creators
- * The method unregisterCreator() unregisters the ProcessorXxx::create() method.
- * It only needs to be passed the string of the processor type.
- *
- * \code
- * ProcessorFactory::get().unregisterCreator("ODOM 2D");
- * \endcode
- *
- * #### Creating processors
- * Prior to invoking the creation of a processor of a particular type,
- * you must register the creator for this type into the factory.
- *
- * To create a ProcessorOdom2D, you type:
- *
- * \code
- * ProcessorFactory::get().create("ODOM 2D", "main odometry", params_ptr);
- * \endcode
- *
- * #### Example 1 : using the Factories alone
- * We provide the necessary steps to create a processor of class ProcessorOdom2D in our application,
- * and bind it to a SensorOdom2D:
- *
- * \code
- * #include "base/sensor/sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory
- * #include "base/processor/processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory
- *
- * // Note: SensorOdom2D::create() is already registered, automatically.
- * // Note: ProcessorOdom2D::create() is already registered, automatically.
- *
- * // First create the sensor (See SensorFactory for details)
- * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
- *
- * // To create a odometry integrator, provide a type="ODOM 2D", a name="main odometry", and a pointer to the parameters struct:
- *
- * ProcessorParamsOdom2D params({...}); // fill in the derived struct (note: ProcessorOdom2D actually has no input params)
- *
- * ProcessorBasePtr processor_ptr =
- * ProcessorFactory::get().create ( "ODOM 2D" , "main odometry" , ¶ms );
- *
- * // Bind processor to sensor
- * sensor_ptr->addProcessor(processor_ptr);
- * \endcode
- *
- * #### Example 2: Using the helper API in class Problem
- * The WOLF uppermost node, Problem, makes the creation of sensors and processors, and the binding between them, even simpler.
- *
- * The creation is basically replicating the factories' API. The binding is accomplished by passing the sensor name to the Processor installer.
- *
- * The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation),
- *
- * \code
- * #include "base/sensor/sensor_odom_2D.h"
- * #include "base/processor/processor_odom_2D.h"
- * #include "base/problem.h"
- *
- * Problem problem(FRM_PO_2D);
- * problem.installSensor ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
- * problem.installProcessor ( "ODOM 2D" , "Odometry" , "Main odometer" , ¶ms );
- * \endcode
- *
- * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
- */
-
-typedef Factory<ProcessorBase,
- const std::string&,
- const ProcessorParamsBasePtr,
- const SensorBasePtr> ProcessorFactory;
-template<>
-inline std::string ProcessorFactory::getClass()
-{
- return "ProcessorFactory";
-}
-
-#define WOLF_REGISTER_PROCESSOR(ProcessorType, ProcessorName) \
- namespace{ const bool WOLF_UNUSED ProcessorName##Registered = \
- wolf::ProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); }\
-
-} /* namespace wolf */
-
-#endif /* PROCESSOR_FACTORY_H_ */
diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h
deleted file mode 100644
index be1205e27988461473786627ca5e0d3acc62748f..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ /dev/null
@@ -1,114 +0,0 @@
-#ifndef _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-#define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-
-// Wolf related headers
-#include "base/processor/processor_loopclosure_base.h"
-#include "base/state_block.h"
-
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsFrameNearestNeighborFilter)
-WOLF_PTR_TYPEDEFS(ProcessorFrameNearestNeighborFilter)
-
-enum class LoopclosureDistanceType : std::size_t
-{
- LC_POINT_ELLIPSE = 1, // 2D
- LC_ELLIPSE_ELLIPSE, // 2D
- LC_POINT_ELLIPSOID, // 3D
- LC_ELLIPSOID_ELLIPSOID, // 3D
- LC_MAHALANOBIS_DISTANCE // 2D & 3D
-};
-
-struct ProcessorParamsFrameNearestNeighborFilter : public ProcessorParamsLoopClosure
-{
- using DistanceType = LoopclosureDistanceType;
-
- ProcessorParamsFrameNearestNeighborFilter() :
- buffer_size_(10),
- sample_step_degree_(10),
- distance_type_(LoopclosureDistanceType::LC_POINT_ELLIPSE),
- probability_(5.991) { }
-
- ProcessorParamsFrameNearestNeighborFilter(
- const ProcessorParamsFrameNearestNeighborFilter& o) :
- buffer_size_(o.buffer_size_),
- sample_step_degree_(o.sample_step_degree_),
- distance_type_(o.distance_type_),
- probability_(o.probability_)
- {
- //
- }
-
- virtual ~ProcessorParamsFrameNearestNeighborFilter() = default;
-
- int buffer_size_;
- int sample_step_degree_;
- DistanceType distance_type_;
- Scalar probability_;
-};
-
-class ProcessorFrameNearestNeighborFilter : public ProcessorLoopClosureBase
-{
-private:
-
- // area of the computed covariance ellipse.
- // depends on how many percent of data should be considered.
- Scalar area_;
-
- ProcessorParamsFrameNearestNeighborFilterPtr params_NNF;
-
-public:
-
- using Params = ProcessorParamsFrameNearestNeighborFilter;
- using ParamsPtr = ProcessorParamsFrameNearestNeighborFilterPtr;
- using DistanceType = Params::DistanceType;
-
- ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF);
- virtual ~ProcessorFrameNearestNeighborFilter() = default;
- virtual void configure(SensorBasePtr _sensor) { };
-
- inline DistanceType getDistanceType() const noexcept {return params_NNF->distance_type_;}
-
-protected:
-
- virtual bool findCandidates(const CaptureBasePtr& _incoming_ptr);
-
- // returns Ellipse in 2D case [ pos_x, pos_y, a, b, tilt]
- bool computeEllipse2D(const FrameBasePtr& frame_ptr,
- Eigen::Vector5s& ellipse);
-
- // returns Ellipsoid in 3D case
- // [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
- bool computeEllipsoid3D(const FrameBasePtr& frame_ptr,
- Eigen::Vector10s& ellipsoid);
-
- // returns true if the two 2D ellipses intersect
- bool ellipse2DIntersect(const Eigen::VectorXs &ellipse1,
- const Eigen::VectorXs &ellipse2);
-
- // returns true if a 2D point lies inside a 2D ellipse
- bool point2DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipse);
-
- // returns true if the two 3D ellipsoids intersect
- bool ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
- const Eigen::VectorXs &ellipsoid2);
-
- // returns true if a 3D point lies inside a 3D ellipsoid
- bool point3DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipsoid);
-
- // returns true if frame lies within Mahalanobis Distance
- bool insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame);
-
- // computes the Mahalanobis Distance
- Scalar MahalanobisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame);
-
- bool frameInsideBuffer(const FrameBasePtr& frame_ptr);
-};
-
-} // namespace wolf
-
-#endif // _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H_
diff --git a/include/base/processor/processor_logging.h b/include/base/processor/processor_logging.h
deleted file mode 100644
index 4add7a8e9d7945f0aaab54c8e7e7f1acdc4bfd8e..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_logging.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/**
- * \file processor_logging.h
- *
- * Created on: Oct 5, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_LOGGING_H_
-#define _WOLF_PROCESSOR_LOGGING_H_
-
-/// @brief un-comment for IDE highlights.
-//#include "base/logging.h"
-
-#define __INTERNAL_WOLF_ASSERT_PROCESSOR \
- static_assert(std::is_base_of<ProcessorBase, \
- typename std::remove_pointer<decltype(this)>::type>::value, \
- "This macro can be used only within the body of a " \
- "non-static " \
- "ProcessorBase (and derived) function !");
-
-#define WOLF_PROCESSOR_INFO(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED(getType(), __VA_ARGS__);
-
-#define WOLF_PROCESSOR_INFO_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED_COND(getType(), cond, __VA_ARGS__);
-
-#endif /* _WOLF_PROCESSOR_LOGGING_H_ */
diff --git a/include/base/processor/processor_loopclosure_base.h b/include/base/processor/processor_loopclosure_base.h
deleted file mode 100644
index a9d8e878eeabdf9e97db4f659ad45a403d9acaca..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_loopclosure_base.h
+++ /dev/null
@@ -1,139 +0,0 @@
-#ifndef _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-#define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-
-// Wolf related headers
-#include "base/processor/processor_base.h"
-
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure);
-
-struct ProcessorParamsLoopClosure : public ProcessorParamsBase
-{
-// virtual ~ProcessorParamsLoopClosure() = default;
-
- // add neccesery parameters for loop closure initialisation here and initialize
- // them in constructor
-};
-
-/** \brief General loop closure processor
- *
- * This is an abstract class.
- *
- * It establishes factors XXX
- *
- * Should you need extra functionality for your derived types,
- * you can overload these two methods,
- *
- * - preProcess() { }
- * - postProcess() { }
- *
- * which are called at the beginning and at the end of process() respectively.
- */
-
-class ProcessorLoopClosureBase : public ProcessorBase
-{
-protected:
-
- ProcessorParamsLoopClosurePtr params_loop_closure_;
-
- // Frames that are possible loop closure candidates according to
- // findLoopClosure()
- std::vector<FrameBasePtr> loop_closure_candidates;
-
- // Frames that are possible loop closure candidates according to
- // findLoopClosure(), but are too recent in the timeline, aka still in a
- // 'buffer zone'. This vector will capture the frames that were set just before
- // the last keyframe.
- std::vector<FrameBasePtr> close_candidates;
-
-public:
-
- ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
-
- virtual ~ProcessorLoopClosureBase() = default;
- virtual void configure(SensorBasePtr _sensor) override { };
-
- /** \brief Full processing of an incoming Capture.
- *
- * Usually you do not need to overload this method in derived classes.
- * Overload it only if you want to alter this algorithm.
- */
- virtual void process(CaptureBasePtr _incoming_ptr) override;
-
- virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tol_other) override ;
-
- const std::vector<FrameBasePtr>& getCandidates() const noexcept;
-
- const std::vector<FrameBasePtr>& getCloseCandidates() const noexcept;
-
-protected:
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Overload this function to prepare stuff on derived classes.
- *
- * Typical uses of prePrecess() are:
- * - casting base types to derived types
- * - initializing counters, flags, or any derived variables
- * - initializing algorithms needed for processing the derived data
- */
- virtual void preProcess() { }
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * Overload this function to post-process stuff on derived classes.
- *
- * Typical uses of postPrecess() are:
- * - resetting and/or clearing variables and/or algorithms at the end of processing
- * - drawing / printing / logging the results of the processing
- */
- virtual void postProcess() { }
-
- /** Find a loop closure between incoming data and all keyframe data
- *
- * This is called by process() .
- *
- * Overload this function in derived classes to find loop closure.
- */
- virtual bool findCandidates(const CaptureBasePtr& _incoming_ptr) = 0;
-
- /** perform a validation among the found possible loop closures to confirm
- * or dismiss them based on available data
- *
- * This is called by process() .
- *
- * Overload this function in derived classes to confirm loop closure.
- */
- virtual bool confirmLoopClosure() = 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!
- */
- virtual bool voteForKeyFrame() override = 0;
-};
-
-inline const std::vector<FrameBasePtr>&
-ProcessorLoopClosureBase::getCandidates() const noexcept
-{
- return loop_closure_candidates;
-}
-
-inline const std::vector<FrameBasePtr>&
-ProcessorLoopClosureBase::getCloseCandidates() const noexcept
-{
- return close_candidates;
-}
-
-} // namespace wolf
-
-#endif /* _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H */
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
deleted file mode 100644
index b66feb38face40a05ce5c96696920ece1ca86c1b..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_motion.h
+++ /dev/null
@@ -1,625 +0,0 @@
-/**
- * \file processor_motion.h
- *
- * Created on: 15/03/2016
- * \author: jsola
- */
-
-#ifndef PROCESSOR_MOTION_H_
-#define PROCESSOR_MOTION_H_
-
-// Wolf
-#include "base/capture/capture_motion.h"
-#include "base/processor/processor_base.h"
-#include "base/time_stamp.h"
-
-// std
-#include <iomanip>
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion);
-
-struct ProcessorParamsMotion : public ProcessorParamsBase
-{
- Scalar max_time_span = 0.5;
- unsigned int max_buff_length = 10;
- Scalar dist_traveled = 5;
- Scalar angle_turned = 0.5;
-};
-
-/** \brief class for Motion processors
- *
- * This processor integrates motion data into vehicle states.
- *
- * The motion data is provided by the sensor owning this processor.
- *
- * This data is, in the general case, in the reference frame of the sensor:
- *
- * - Beware of the frame transformations Map to Robot, and Robot to Sensor, so that your produced
- * motion Factors are correctly expressed.
- * - The robot state R is expressed in a global or 'Map' reference frame, named M.
- * - The sensor frame S is expressed in the robot frame R.
- * - The motion data data_ is expressed in the sensor frame S.
- * - You can use three basic methods for this:
- * - The trivial: make the sensor frame and the robot frame the same frame, that is, S = Id.
- * - Transform incoming data from sensor frame to robot frame, and then integrate motion in robot frame.
- * - Integrate motion directly in sensor frame, and transform to robot frame at the time of:
- * - Publishing the robot state (see getCurrentState() and similar functions)
- * - Creating Keyframes and Factors (see emplaceFactor() ).
- *
- * Should you need extra functionality for your derived types, you can overload these two methods,
- *
- * - preProcess() { }
- * - postProcess() { }
- *
- * which are called at the beginning and at the end of process(). See the doc of these functions for more info.
- */
- /* // TODO: JS: review these instructions from here onwards:
- *
- * while the integrated motion refers to the robot frame.
- *
- * The reference frame convention used are specified as follows.
- * - The robot state R is expressed in a global or 'Map' reference frame, named M.
- * - The sensor frame S is expressed in the robot frame R.
- * - The motion data data_ is expressed in the sensor frame S.
- *
- * This processor, therefore, needs to convert the motion data in two ways:
- * - First, convert the format of this data into a delta-state.
- * A delta-state is an expression of a state increment that can be treated
- * algebraically together with states (operations sum (+) for an additive composition,
- * and substract (-) for the reverse).
- * - Second, it needs to be converted from the Sensor frame to the Robot frame: R <-- S:
- *
- * delta_R = fromSensorFrame(delta_S) : this transforms delta_S from frame S to frame R.
- * - The two operations are performed by the pure virtual method data2delta(). A possible implementation
- * of data2delta() could be (we use the data member delta_ as the return value):
- * \code
- * void data2delta(const VectorXs _data)
- * {
- * delta_S = format(_data);
- * delta_R = fromSensorFrame(delta_S);
- * delta_ = delta_R;
- * }
- * \endcode
- * where format() is any code you need to format the data into a delta form,
- * and fromSensorFrame() is explained below.
- *
- * Only when the motion delta is expressed in the robot frame R, we can integrate it
- * on top of the current Robot frame: R <-- R (+) delta_R
- *
- * \code statePlusDelta(R_old, delta_R, R_new) \endcode
- *
- * ### Defining (or not) the fromSensorFrame():
- *
- * In most cases, one will be interested in avoiding the \b fromSensorFrame() issue.
- * This can be trivially done by defining the Robot frame precisely at the Sensor frame,
- * so that S is the identity. In this case, \b fromSensorFrame() does nothing and delta_R = delta_S.
- *
- * Notes:
- * - This class does not declare any prototype for \b fromSensorFrame().
- * - In cases where this identification is not possible, or not desired,
- * classes deriving from this class will have to implement fromSensorFrame(),
- * and call it within data2delta(), or write the frame transformation code directly in data2delta().
- *
- * // TODO: JS: review instructions up to here
- *
- */
-class ProcessorMotion : public ProcessorBase
-{
- public:
- typedef enum {
- RUNNING_WITHOUT_PACK,
- RUNNING_WITH_PACK_BEFORE_ORIGIN,
- RUNNING_WITH_PACK_ON_ORIGIN,
- RUNNING_WITH_PACK_AFTER_ORIGIN
- } ProcessingStep ;
-
- protected:
- ProcessorParamsMotionPtr params_motion_;
- ProcessingStep processing_step_; ///< State machine controlling the processing step
-
- // This is the main public interface
- public:
- ProcessorMotion(const std::string& _type,
- SizeEigen _state_size,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- SizeEigen _data_size,
- SizeEigen _calib_size,
- ProcessorParamsMotionPtr _params_motion);
- virtual ~ProcessorMotion();
-
- // Instructions to the processor:
-
- void process(CaptureBasePtr _incoming_ptr);
- virtual void resetDerived();
-
- // Queries to the processor:
- virtual bool isMotion();
-
- virtual bool voteForKeyFrame();
-
- /** \brief Fill a reference to the state integrated so far
- * \param _x the returned state vector
- */
- void getCurrentState(Eigen::VectorXs& _x);
- void getCurrentTimeStamp(TimeStamp& _ts){ _ts = getBuffer().get().back().ts_; }
-
- /** \brief Get the state integrated so far
- * \return the state vector
- */
- Eigen::VectorXs getCurrentState();
- TimeStamp getCurrentTimeStamp();
-
- /** \brief Fill the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \param _x the returned state
- * \return if state in the provided time-stamp could be resolved
- */
- bool getState(const TimeStamp& _ts, Eigen::VectorXs& _x);
-
- /** \brief Get the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \return the state vector
- */
- Eigen::VectorXs getState(const TimeStamp& _ts);
-
- /** \brief Gets the delta preintegrated covariance from all integrations so far
- * \return the delta preintegrated covariance matrix
- */
- const Eigen::MatrixXs getCurrentDeltaPreintCov();
-
- /** \brief Provide the motion integrated so far
- * \return the integrated motion
- */
- Motion getMotion() const;
-
- /** \brief Provide the motion integrated until a given timestamp
- * \return the integrated motion
- */
- Motion getMotion(const TimeStamp& _ts) const;
-
- /** \brief Finds the capture that contains the closest previous motion of _ts
- * \return a pointer to the capture (if it exists) or a nullptr (otherwise)
- */
- CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
-
- /** Set the origin of all motion for this processor
- * \param _origin_frame the keyframe to be the origin
- */
- void setOrigin(FrameBasePtr _origin_frame);
-
- /** Set the origin of all motion for this processor
- * \param _x_origin the state at the origin
- * \param _ts_origin origin timestamp.
- */
- FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
-
- MotionBuffer& getBuffer();
- const MotionBuffer& getBuffer() const;
-
- // Helper functions:
- protected:
-
- Scalar updateDt();
- void integrateOneStep();
- void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
- void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
- TimeStamp ts_split,
- const FrameBasePtr& keyframe_target,
- const wolf::CaptureMotionPtr& capture_target);
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Overload this function to prepare stuff on derived classes.
- *
- * Typical uses of prePrecess() are:
- * - casting base types to derived types
- * - initializing counters, flags, or any derived variables
- * - initializing algorithms needed for processing the derived data
- */
- virtual void preProcess() { };
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * Overload this function to post-process stuff on derived classes.
- *
- * Typical uses of postPrecess() are:
- * - resetting and/or clearing variables and/or algorithms at the end of processing
- * - drawing / printing / logging the results of the processing
- */
- virtual void postProcess() { };
-
- PackKeyFramePtr computeProcessingStep();
-
- // These are the pure virtual functions doing the mathematics
- protected:
-
- /** \brief convert raw CaptureMotion data to the delta-state format
- *
- * This function accepts raw data and time step dt,
- * and computes the value of the delta-state and its covariance. Note that these values are
- * held by the members delta_ and delta_cov_.
- *
- * @param _data measured motion data
- * @param _data_cov covariance
- * @param _dt time step
- * @param _delta computed delta
- * @param _delta_cov covariance
- * @param _calib current state of the calibrated parameters
- * @param _jacobian_calib Jacobian of the delta wrt calib
- *
- * Rationale:
- *
- * The delta-state format must be compatible for integration using
- * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
- * See the class documentation for some Eigen::VectorXs suggestions.
- *
- * The data format is generally not the same as the delta format,
- * because it is the format of the raw data provided by the Capture,
- * which is unaware of the needs of this processor.
- *
- * Additionally, sometimes the data format is in the form of a
- * velocity, or a higher derivative, while the delta is in the form of an increment.
- * In such cases, converting from data to delta-state needs integrating
- * the data over the period dt.
- *
- * Two trivial implementations would establish:
- * - If `_data` is an increment:
- *
- * delta_ = _data;
- * delta_cov_ = _data_cov
- * - If `_data` is a velocity:
- *
- * delta_ = _data * _dt
- * delta_cov_ = _data_cov * _dt.
- *
- * However, other more complicated relations are possible.
- * In general, we'll have a nonlinear
- * function relating `delta_` to `_data` and `_dt`, as follows:
- *
- * delta_ = f ( _data, _dt)
- *
- * The delta covariance is obtained by classical uncertainty propagation of the data covariance,
- * that is, through the Jacobians of `f()`,
- *
- * delta_cov_ = F_data * _data_cov * F_data.transpose
- *
- * where `F_data = d_f/d_data` is the Jacobian of `f()`.
- */
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) = 0;
-
- /** \brief composes a delta-state on top of another delta-state
- * \param _delta1 the first delta-state
- * \param _delta2 the second delta-state
- * \param _dt2 the second delta-state's time delta
- * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
- *
- * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
- */
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _dt2,
- Eigen::VectorXs& _delta1_plus_delta2) = 0;
-
- /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
- * \param _delta1 the first delta-state
- * \param _delta2 the second delta-state
- * \param _dt2 the second delta-state's time delta
- * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
- * \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
- * \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
- *
- * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians.
- */
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) = 0;
-
- /** \brief composes a delta-state on top of a state
- * \param _x the initial state
- * \param _delta the delta-state
- * \param _x_plus_delta the updated state. It has the same format as the initial state.
- * \param _dt the time interval spanned by the Delta
- *
- * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
- */
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta) = 0;
-
- /** \brief Delta zero
- * \return a delta state equivalent to the null motion.
- *
- * Examples (see documentation of the the class for info on Eigen::VectorXs):
- * - 2D odometry: a 3-vector with all zeros, e.g. VectorXs::Zero(3)
- * - 3D odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
- * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
- */
- virtual Eigen::VectorXs deltaZero() const = 0;
-
- /** \brief Interpolate motion to an intermediate time-stamp
- *
- * @param _ref The motion reference
- * @param _second The second motion. It is modified by the function (see documentation below).
- * @param _ts The intermediate time stamp: it must be bounded by `_ref.ts_ <= _ts <= _second.ts_`.
- * @return The interpolated motion (see documentation below).
- *
- * This function interpolates a motion between two existing motions.
- *
- * In this base implementation, we just provide the closest motion provided (ref or second),
- * so really no interpolation takes place and just the current data and delta are updated.
- *
- * Should you require finer interpolation, you must overload this method in your derived class.
- */
- virtual Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts);
-
- /** \brief Interpolate motion to an intermediate time-stamp
- *
- * @param _ref1 The first motion reference
- * @param _ref2 The second motion reference.
- * @param _ts The intermediate time stamp: it must be bounded by `_ref.ts_ <= _ts <= _second.ts_`.
- * @param _second The second part motion after interpolation, so that return (+) second = ref2.
- * @return The interpolated motion (see documentation below).
- *
- * This function interpolates a motion between two existing motions.
- *
- * In this base implementation, we just provide the closest motion provided (ref1 or ref2),
- * the second motion being the complementary part,
- * so really no interpolation takes place and just the current data and delta are updated.
- *
- * Should you require finer interpolation, you must overload this method in your derived class.
- */
- virtual Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second);
-
- /** \brief create a CaptureMotion and add it to a Frame
- * \param _ts time stamp
- * \param _sensor Sensor that produced the Capture
- * \param _data a vector of motion data
- * \param _sata_cov covariances matrix of the motion data data
- * \param _frame_own frame owning the Capture to create
- * \param _frame_origin frame acting as the origin of motions for the MorionBuffer of the created MotionCapture
- */
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
- const FrameBasePtr& _frame_origin);
-
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) = 0;
-
- /** \brief create a feature corresponding to given capture and add the feature to this capture
- * \param _capture_motion: the parent capture
- */
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own);
-
- /** \brief create a feature corresponding to given capture
- * \param _capture_motion: the parent capture
- */
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
-
- /** \brief create 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
- */
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
-
- Motion motionZero(const TimeStamp& _ts);
-
- bool hasCalibration() {return calib_size_ > 0;}
-
- public:
- //getters
- CaptureMotionPtr getOrigin();
- CaptureMotionPtr getLast();
- CaptureMotionPtr getIncoming();
-
- Scalar getMaxTimeSpan() const;
- Scalar getMaxBuffLength() const;
- Scalar getDistTraveled() const;
- Scalar getAngleTurned() const;
-
- void setMaxTimeSpan(const Scalar& _max_time_span);
- void setMaxBuffLength(const Scalar& _max_buff_length);
- void setDistTraveled(const Scalar& _dist_traveled);
- void setAngleTurned(const Scalar& _angle_turned);
-
- protected:
- // Attributes
- SizeEigen x_size_; ///< The size of the state vector
- SizeEigen data_size_; ///< the size of the incoming data
- SizeEigen delta_size_; ///< the size of the deltas
- SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix
- SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes)
- CaptureMotionPtr origin_ptr_;
- CaptureMotionPtr last_ptr_;
- CaptureMotionPtr incoming_ptr_;
-
- protected:
- // helpers to avoid allocation
- Scalar dt_; ///< Time step
- Eigen::VectorXs x_; ///< current state
- Eigen::VectorXs data_; ///< current data
- Eigen::VectorXs delta_; ///< current delta
- Eigen::MatrixXs delta_cov_; ///< current delta covariance
- Eigen::VectorXs delta_integrated_; ///< integrated delta
- Eigen::MatrixXs delta_integrated_cov_; ///< integrated delta covariance
- Eigen::VectorXs calib_preint_; ///< calibration vector used during pre-integration
- Eigen::MatrixXs jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
- Eigen::MatrixXs jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
- Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
- Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
-};
-
-}
-
-#include "base/frame_base.h"
-
-namespace wolf{
-
-inline void ProcessorMotion::resetDerived()
-{
- // Blank function, to be implemented in derived classes
-}
-
-inline bool ProcessorMotion::voteForKeyFrame()
-{
- return false;
-}
-
-inline Eigen::VectorXs ProcessorMotion::getState(const TimeStamp& _ts)
-{
- Eigen::VectorXs x(getProblem()->getFrameStructureSize());
- getState(_ts, x);
- return x;
-}
-
-inline TimeStamp ProcessorMotion::getCurrentTimeStamp()
-{
- return getBuffer().get().back().ts_;
-}
-
-inline Eigen::VectorXs ProcessorMotion::getCurrentState()
-{
- Eigen::VectorXs x(getProblem()->getFrameStructureSize());
- getCurrentState(x);
- return x;
-}
-
-inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
-{
- Scalar Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
-}
-
-inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov()
-{
- return getBuffer().get().back().delta_integr_cov_;
-}
-
-inline Motion ProcessorMotion::getMotion() const
-{
- return getBuffer().get().back();
-}
-
-inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
-{
- auto capture_ptr = findCaptureContainingTimeStamp(_ts);
- assert(capture_ptr != nullptr && "ProcessorMotion::getMotion: timestamp older than first motion");
-
- return capture_ptr->getBuffer().getMotion(_ts);
-}
-
-inline bool ProcessorMotion::isMotion()
-{
- return true;
-}
-
-inline Scalar ProcessorMotion::updateDt()
-{
- return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
-}
-
-inline const MotionBuffer& ProcessorMotion::getBuffer() const
-{
- return last_ptr_->getBuffer();
-}
-
-inline MotionBuffer& ProcessorMotion::getBuffer()
-{
- return last_ptr_->getBuffer();
-}
-
-inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
-{
- return Motion(_ts,
- VectorXs::Zero(data_size_), // data
- Eigen::MatrixXs::Zero(data_size_, data_size_), // Cov data
- deltaZero(),
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
- deltaZero(),
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
- Eigen::MatrixXs::Zero(delta_cov_size_, calib_size_) // Jac calib
- );
-}
-
-inline CaptureMotionPtr ProcessorMotion::getOrigin()
-{
- return origin_ptr_;
-}
-
-inline CaptureMotionPtr ProcessorMotion::getLast()
-{
- return last_ptr_;
-}
-
-inline CaptureMotionPtr ProcessorMotion::getIncoming()
-{
- return incoming_ptr_;
-}
-
-inline Scalar ProcessorMotion::getMaxTimeSpan() const
-{
- return params_motion_->max_time_span;
-}
-
-inline Scalar ProcessorMotion::getMaxBuffLength() const
-{
- return params_motion_->max_buff_length;
-}
-
-inline Scalar ProcessorMotion::getDistTraveled() const
-{
- return params_motion_->dist_traveled;
-}
-
-inline Scalar ProcessorMotion::getAngleTurned() const
-{
- return params_motion_->angle_turned;
-}
-
-inline void ProcessorMotion::setMaxTimeSpan(const Scalar& _max_time_span)
-{
- params_motion_->max_time_span = _max_time_span;
-}
-inline void ProcessorMotion::setMaxBuffLength(const Scalar& _max_buff_length)
-{
- params_motion_->max_buff_length = _max_buff_length;
-}
-inline void ProcessorMotion::setDistTraveled(const Scalar& _dist_traveled)
-{
- params_motion_->dist_traveled = _dist_traveled;
-}
-inline void ProcessorMotion::setAngleTurned(const Scalar& _angle_turned)
-{
- params_motion_->angle_turned = _angle_turned;
-}
-
-} // namespace wolf
-
-#endif /* PROCESSOR_MOTION2_H_ */
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
deleted file mode 100644
index 000a7bc9eedf447850d8d42cf414ae8bf1fc4d7f..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_odom_2D.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/**
- * \file processor_odom_2D.h
- *
- * Created on: Apr 15, 2016
- * \author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_ODOM_2D_H_
-#define SRC_PROCESSOR_ODOM_2D_H_
-
-#include "base/processor/processor_motion.h"
-#include "base/capture/capture_odom_2D.h"
-#include "base/factor/factor_odom_2D.h"
-#include "base/rotations.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorOdom2D);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
-
-struct ProcessorParamsOdom2D : public ProcessorParamsMotion
-{
- Scalar cov_det = 1.0; // 1 rad^2
- Scalar unmeasured_perturbation_std = 0.001; // no particular dimension: the same for displacement and angle
-};
-
-class ProcessorOdom2D : public ProcessorMotion
-{
- public:
- ProcessorOdom2D(ProcessorParamsOdom2DPtr _params);
- virtual ~ProcessorOdom2D();
- virtual void configure(SensorBasePtr _sensor) override { };
-
- virtual bool voteForKeyFrame() override;
-
- protected:
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) override;
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta) override;
- virtual Eigen::VectorXs deltaZero() const override;
- virtual Motion interpolate(const Motion& _ref,
- Motion& _second,
- TimeStamp& _ts) override;
- virtual Motion interpolate(const Motion& _ref1,
- const Motion& _ref2,
- const TimeStamp& _ts,
- Motion& _second) override;
-
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) override;
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin) override;
-
- protected:
- ProcessorParamsOdom2DPtr params_odom_2D_;
- MatrixXs unmeasured_perturbation_cov_;
-
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline Eigen::VectorXs ProcessorOdom2D::deltaZero() const
-{
- return Eigen::VectorXs::Zero(delta_size_);
-}
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_ODOM_2D_H_ */
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
deleted file mode 100644
index 2cbc26a068f13c8e316bf876dd8d483b7eb0a125..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_odom_3D.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/**
- * \file processor_odom_3D.h
- *
- * Created on: Mar 18, 2016
- * \author: jsola
- */
-
-#ifndef SRC_PROCESSOR_ODOM_3D_H_
-#define SRC_PROCESSOR_ODOM_3D_H_
-
-#include "base/processor/processor_motion.h"
-#include "base/sensor/sensor_odom_3D.h"
-#include "base/capture/capture_odom_3D.h"
-#include "base/factor/factor_odom_3D.h"
-#include "base/rotations.h"
-#include <cmath>
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
-
-struct ProcessorParamsOdom3D : public ProcessorParamsMotion
-{
- //
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
-
-/** \brief Processor for 3d odometry integration.
- *
- * This processor integrates motion data in the form of 3D odometry.
- *
- * The odometry data is extracted from Captures of the type CaptureOdometry3d.
- * This data comes in the form of a 6-vector, or a 7-vector, containing the following components:
- * - a 3d position increment in the local frame of the robot (dx, dy, dz).
- * - a 3d orientation increment in the local frame of the robot (droll, dpitch, dyaw), or quaternion (dqx, dqy, dqz, dqw).
- *
- * The produced integrated deltas are in the form of 7-vectors with the following components:
- * - a 3d position increment in the local frame of the robot (Dx, Dy, Dz)
- * - a quaternion orientation increment in the local frame of the robot (Dqx, Dqy, Dqz, Dqw)
- *
- * The produced states are in the form of 7-vectors with the following components:
- * - a 3d position of the robot in the world frame (x, y, z)
- * - a quaternion orientation of the robot in the world frame (qx, qy, qz, qw)
- *
- * The processor integrates data by ignoring the time increment dt_
- * (as it integrates motion directly, not velocities).
- *
- * All frames are assumed FLU ( x: Front, y: Left, z: Up ).
- */
-class ProcessorOdom3D : public ProcessorMotion
-{
- public:
- ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr = nullptr);
- virtual ~ProcessorOdom3D();
- virtual void configure(SensorBasePtr _sensor) override;
-
- public:
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) override;
- void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) override;
- void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) override;
- void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta) override;
- Eigen::VectorXs deltaZero() const override;
- Motion interpolate(const Motion& _motion_ref,
- Motion& _motion,
- TimeStamp& _ts) override;
- virtual Motion interpolate(const Motion& _ref1,
- const Motion& _ref2,
- const TimeStamp& _ts,
- Motion& _second) override;
- bool voteForKeyFrame() override;
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) override;
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
-
- protected:
- ProcessorParamsOdom3DPtr params_odom_3D_;
-
- // noise parameters (stolen from owner SensorOdom3D)
- Scalar k_disp_to_disp_; // displacement variance growth per meter of linear motion
- Scalar k_disp_to_rot_; // orientation variance growth per meter of linear motion
- Scalar k_rot_to_rot_; // orientation variance growth per radian of rotational motion
- Scalar min_disp_var_; // floor displacement variance when no motion
- Scalar min_rot_var_; // floor orientation variance when no motion
-
- // Eigen::Map helpers
- Eigen::Map<const Eigen::Vector3s> p1_, p2_;
- Eigen::Map<Eigen::Vector3s> p_out_;
- Eigen::Map<const Eigen::Quaternions> q1_, q2_;
- Eigen::Map<Eigen::Quaternions> q_out_;
- void remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x2, Eigen::VectorXs& _x_out);
-
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline Eigen::VectorXs ProcessorOdom3D::deltaZero() const
-{
- return (Eigen::VectorXs(7) << 0,0,0, 0,0,0,1).finished(); // p, q
-}
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_ODOM_3D_H_ */
diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h
deleted file mode 100644
index 6ce3eaace3119ed1e68725bb33d2ca0df4684bbb..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_params_image.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef PROCESSOR_IMAGE_PARAMS_H
-#define PROCESSOR_IMAGE_PARAMS_H
-
-// wolf
-#include "base/processor/processor_tracker_feature.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureImage);
-
-struct ProcessorParamsTrackerFeatureImage : public ProcessorParamsTrackerFeature
-{
- std::string yaml_file_params_vision_utils;
-
- Scalar min_response_for_new_features; ///< minimum value of the response to create a new feature
- Scalar distance;
-
- Scalar pixel_noise_std; ///< std noise of the pixel
- Scalar pixel_noise_var; ///< var noise of the pixel
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkImage);
-
-struct ProcessorParamsTrackerLandmarkImage : public ProcessorParamsTrackerLandmark
-{
- std::string yaml_file_params_vision_utils;
-
- Scalar min_response_for_new_features; ///< minimum value of the response to create a new feature
- Scalar distance;
-
- Scalar pixel_noise_std; ///< std noise of the pixel
- Scalar pixel_noise_var; ///< var noise of the pixel
-};
-}
-
-#endif // PROCESSOR_IMAGE_PARAMS_H
diff --git a/include/base/processor/processor_polyline.h b/include/base/processor/processor_polyline.h
deleted file mode 100644
index 959c9d59dbc6fa70653b49b70b4d84509cd05c41..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_polyline.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * processor_polyline.h
- *
- * Created on: Sep 14, 2017
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_POLYLINE_H_
-#define SRC_PROCESSOR_POLYLINE_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_match.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/processor/processor_base.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
-WOLF_PTR_TYPEDEFS(ProcessorPolyline);
-
-struct ProcessorParamsPolyline : public ProcessorParamsBase
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar match_position_error_th;
- Scalar class_position_error_th;
- unsigned int new_features_th;
- Scalar loop_time_th;
- std::vector<PolylineRectangularClass> polyline_classes;
- Scalar position_error_th_;
- Scalar min_features_ratio_th_;
-};
-
-class ProcessorPolyline : public ProcessorBase
-{
- protected:
- CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
- CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
- laserscanutils::LineFinderIterative line_finder_;
-
- ProcessorParamsPolylinePtr params_;
-
- FeatureBasePtrList new_features_incoming_, new_features_last_, known_features_incoming_, known_features_last_;
- LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
- LandmarkMatchMap matches_landmark_from_incoming_;
- LandmarkMatchMap matches_landmark_from_last_;
-
- std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
-
- Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Matrix2s R_current_prev_;
- Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Vector2s t_current_prev_;
- Eigen::Vector2s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorPolyline(ProcessorParamsPolylinePtr _params);
-
- virtual ~ProcessorPolyline();
-
- virtual void configure(SensorBasePtr _sensor) { };
-
- // Algorithm
- virtual void process(CaptureBasePtr _capture_ptr);
-
- void processNew();
-
- virtual bool voteForKeyFrame();
-
- // Implementation
- void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
- unsigned int findLandmarks(const LandmarkBasePtrList& _landmark_list_in, FeatureBasePtrList& _feature_list_out, LandmarkMatchMap& _feature_landmark_correspondences);
-
- virtual void createNewLandmarks();
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- virtual void establishFactors();
- void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id);
- void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id);
- bool rejectOutlier(FactorBasePtr& fac_ptr);
- void classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list);
- LandmarkMatchPolyline2DPtr tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr);
- LandmarkMatchPolyline2DPtr tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
-
- // Gets/Sets
- const FeatureBasePtrList& getLastNewPolylines() const;
- const FeatureBasePtrList& getLastKnownPolylines() const;
- virtual void setKeyFrame(CaptureBasePtr _capture_ptr);
- virtual CaptureBasePtr getLast();
-
- // Maths
- void computeTransformations(const TimeStamp& _ts);
- void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_);
-
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline void ProcessorPolyline::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id)
-{
- assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
-
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-inline void ProcessorPolyline::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id)
-{
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-inline const FeatureBasePtrList& ProcessorPolyline::getLastNewPolylines() const
-{
- return new_features_last_;
-}
-
-inline const FeatureBasePtrList& ProcessorPolyline::getLastKnownPolylines() const
-{
- if (last_ptr_->getFrame()->isKey())
- return last_ptr_->getFeatureList();
- else
- return known_features_last_;
-}
-
-inline CaptureBasePtr ProcessorPolyline::getLast()
-{
- return last_ptr_;
-}
-
-} /* namespace wolf */
-
-#endif /* SRC_PROCESSOR_POLYLINE_H_ */
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
deleted file mode 100644
index 38dc26b30d80c419c69f3640c71645352afa6f66..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker.h
+++ /dev/null
@@ -1,278 +0,0 @@
-/*
- * processor_tracker.h
- *
- * Created on: Apr 7, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_TRACKER_H_
-#define SRC_PROCESSOR_TRACKER_H_
-
-#include "base/processor/processor_base.h"
-#include "base/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
-
-struct ProcessorParamsTracker : public ProcessorParamsBase
-{
- unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
- int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorTracker);
-
-/** \brief General tracker processor
- *
- * This is an abstract class.
- *
- * This class implements the incremental tracker, that can be used to track either Features in
- * other Captures, or Landmarks in a Map, through two derived classes,
- * - ProcessorTrackerFeature,
- * - ProcessorTrackerLandmark.
- *
- * The incremental tracker contains three pointers to three Captures of type CaptureBase,
- * named \b origin, \b last and \b incoming:
- * - \b origin: this points to a Capture where all Feature tracks start.
- * - \b last: the last Capture tracked by the tracker.
- * A sufficient subset of the Features in \b origin is still alive in \b last.
- * - \b incoming: the capture being received. The tracker operates on this Capture,
- * establishing correspondences between the features here and the features in \b origin.
- * Each successful correspondence
- * results in an extension of the track of the Feature up to the \b incoming Capture.
- *
- * It establishes factors Feature-Feature or Feature-Landmark;
- * Implement these options in two separate derived classes:
- * - ProcessorTrackerFeature : for Feature-Feature correspondences (no landmarks)
- * - ProcessorTrackerLandmark : for Feature-Landmark correspondences (with landmarks)
- *
- * The pipeline of actions for an autonomous tracker can be resumed as follows (see process() for full detail).
- * We highlight the functions to be implemented by derived classes with the sign '<=== IMPLEMENT'
- * - On each incoming Capture,
- * - Track known features in the \b incoming Capture: processKnown() <=== IMPLEMENT
- * - Check if enough Features are still tracked, and vote for a new KeyFrame if this number is too low:
- * - if voteForKeyFrame() <=== IMPLEMENT
- * - Populate the tracker with new Features : processNew() <=== IMPLEMENT
- * - Make a KeyFrame with the \b last Capture: makeFrame(), setKey()
- * - Establish factors of the new Features: establishFactors() <=== IMPLEMENT
- * - Reset the tracker with the \b last Capture as the new \b origin: reset() <=== IMPLEMENT
- * - else
- * - Advance the tracker one Capture ahead: advance() <=== IMPLEMENT
- *
- * This functionality exists by default in the virtual method process().
- * Should you need extra functionality for your derived types, you can overload the two methods,
- *
- * - preProcess() { }
- * - postProcess() { }
- *
- * which are called at the beginning and at the end of process(). See the doc of these functions for more info.
- */
-class ProcessorTracker : public ProcessorBase
-{
- public:
- typedef enum {
- FIRST_TIME_WITH_PACK,
- FIRST_TIME_WITHOUT_PACK,
- SECOND_TIME_WITH_PACK,
- SECOND_TIME_WITHOUT_PACK,
- RUNNING_WITH_PACK,
- RUNNING_WITHOUT_PACK
- } ProcessingStep ;
-
- protected:
- ProcessorParamsTrackerPtr params_tracker_; ///< parameters for the tracker
- ProcessingStep processing_step_; ///< State machine controlling the processing step
- CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
- CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
- CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
- FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
- FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
-
- SizeStd number_of_tracks_;
-
- public:
- ProcessorTracker(const std::string& _type,
- ProcessorParamsTrackerPtr _params_tracker);
- virtual ~ProcessorTracker();
-
- /** \brief Full processing of an incoming Capture.
- *
- * Usually you do not need to overload this method in derived classes.
- * Overload it only if you want to alter this algorithm.
- */
- virtual void process(CaptureBasePtr const _incoming_ptr);
-
- bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
- bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
- bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
- bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
-
- virtual CaptureBasePtr getOrigin();
- virtual CaptureBasePtr getLast();
- virtual CaptureBasePtr getIncoming();
-
- protected:
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Overload this function to prepare stuff on derived classes.
- *
- * Typical uses of prePrecess() are:
- * - casting base types to derived types
- * - initializing counters, flags, or any derived variables
- * - initializing algorithms needed for processing the derived data
- */
- virtual void preProcess() { };
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * Overload this function to post-process stuff on derived classes.
- *
- * Typical uses of postPrecess() are:
- * - resetting and/or clearing variables and/or algorithms at the end of processing
- * - drawing / printing / logging the results of the processing
- */
- virtual void postProcess() { };
-
- /** \brief Tracker function
- * \return The number of successful tracks.
- *
- * This is the tracker function to be implemented in derived classes.
- * It operates on the \b incoming capture pointed by incoming_ptr_.
- *
- * This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
- * - Track Features against other Features in the \b origin Capture. Tips:
- * - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
- * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
- * - If required, correct the drift by re-comparing against the Features in origin.
- * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
- * - Track Features against Landmarks in the Map. Tips:
- * - The links to the Landmarks are in the Features' Factors in last.
- * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
- *
- * The function must generate the necessary Features in the \b incoming Capture,
- * of the correct type, derived from FeatureBase.
- *
- * It must also generate the factors, of the correct type, derived from FactorBase
- * (through FactorAnalytic or FactorSparse).
- */
- virtual unsigned int processKnown() = 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!
- */
- virtual bool voteForKeyFrame() = 0;
-
- /** \brief Advance the incoming Capture to become the last.
- *
- * Call this when the tracking and keyframe policy work is done and
- * we need to get ready to accept a new incoming Capture.
- */
- virtual void advanceDerived() = 0;
-
- /**\brief Process new Features or Landmarks
- *
- */
- virtual unsigned int processNew(const int& _max_features) = 0;
-
- /**\brief Creates and adds factors from last_ to origin_
- *
- */
- virtual void establishFactors() = 0;
-
- /** \brief Reset the tracker using the \b last Capture as the new \b origin.
- */
- virtual void resetDerived() = 0;
-
- public:
-
- FeatureBasePtrList& getNewFeaturesListLast();
-
- const SizeStd& previousNumberOfTracks() const
- {
- return number_of_tracks_;
- }
-
- SizeStd& previousNumberOfTracks()
- {
- return number_of_tracks_;
- }
-
- protected:
-
- void computeProcessingStep();
-
- void addNewFeatureLast(FeatureBasePtr _feature_ptr);
-
- FeatureBasePtrList& getNewFeaturesListIncoming();
-
- void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
-
-};
-
-inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListLast()
-{
- return new_features_last_;
-}
-
-inline void ProcessorTracker::addNewFeatureLast(FeatureBasePtr _feature_ptr)
-{
- new_features_last_.push_back(_feature_ptr);
-}
-
-inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
-{
- return new_features_incoming_;
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
-{
- return (std::fabs(_ts2 - _ts2) < params_tracker_->time_tolerance);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
-{
- return checkTimeTolerance(_cap->getTimeStamp(), _ts);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts)
-{
- return checkTimeTolerance(_frm->getTimeStamp(), _ts);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap)
-{
- return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
-}
-
-inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
-{
- new_features_incoming_.push_back(_feature_ptr);
-}
-
-inline CaptureBasePtr ProcessorTracker::getOrigin()
-{
- return origin_ptr_;
-}
-
-inline CaptureBasePtr ProcessorTracker::getLast()
-{
- return last_ptr_;
-}
-
-inline CaptureBasePtr ProcessorTracker::getIncoming()
-{
- return incoming_ptr_;
-}
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_TRACKER_H_ */
diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
deleted file mode 100644
index 2f0e7399ca78b42ff2ad5ddb4a4f53a1280fbbdf..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature.h
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
- * \processor_tracker_feature.h
- *
- * Created on: 27/02/2016
- * \author: jsola
- */
-
-#ifndef PROCESSOR_TRACKER_FEATURE_H_
-#define PROCESSOR_TRACKER_FEATURE_H_
-
-//wolf includes
-#include "base/processor/processor_tracker.h"
-#include "base/capture/capture_base.h"
-#include "base/feature/feature_match.h"
-#include "base/track_matrix.h"
-#include "base/wolf.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeature);
-
-struct ProcessorParamsTrackerFeature : public ProcessorParamsTracker
-{
- //
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
-
-/** \brief Feature tracker processor
- *
- * This is an abstract class.
- *
- * This class implements the incremental feature tracker.
- *
- * The incremental tracker contains three pointers to three Captures of type CaptureBase,
- * named \b origin, \b last and \b incoming:
- * - \b origin: this points to a Capture where all Feature tracks start.
- * - \b last: the last Capture tracked by the tracker.
- * A sufficient subset of the Features in \b origin is still alive in \b last.
- * - \b incoming: the capture being received. The tracker operates on this Capture,
- * establishing correspondences between the features here and the features in \b origin.
- * Each successful correspondence
- * results in an extension of the track of the Feature up to the \b incoming Capture.
- *
- * It establishes factors Feature-Feature or Feature-Landmark.
- *
- * This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
- * As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
- * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by derived classes with '<=== IMPLEMENT'
- *
- * - On each incoming Capture,
- * - Track known features in the \b incoming Capture: processKnown() <--- IMPLEMENTED
- * - Check if enough Features are still tracked, and vote for a new KeyFrame if this number is too low:
- * - if voteForKeyFrame() <=== IMPLEMENT
- * - Populate the tracker with new Features : processNew() <--- IMPLEMENTED
- * - Make a KeyFrame with the \b last Capture: makeFrame(), setKey()
- * - Establish factors of the new Features: establishFactors() <--- IMPLEMENTED
- * - Reset the tracker with the \b last Capture as the new \b origin: reset() <--- IMPLEMENTED
- * - else
- * - Advance the tracker one Capture ahead: advance() <--- IMPLEMENTED
- *
- * The most important implemented methods are:
- * - processKnown() : which calls the pure virtuals, to be implemented in derived classes:
- * - trackFeatures() : track Features from \b last to \b incoming <=== IMPLEMENT
- * - correctFeatureDrift() : correct the drift by re-matching from \b origin to \b incoming
- * - processNew() : which calls the pure virtuals:
- * - detectNewFeatures() : detects new Features in \b last <=== IMPLEMENT
- * - trackFeatures() : track these new Features again in \b incoming <=== IMPLEMENT
- * - establishFactors() : which calls the pure virtual:
- * - createFactor() : create factor of the correct derived type <=== IMPLEMENT
- *
- * Should you need extra functionality for your derived types, you can override these two methods,
- *
- * - preProcess() { }
- * - postProcess() { }
- *
- * which are called at the beginning and at the end of process() respectively.
- * See the doc of these functions for more info.
- */
-class ProcessorTrackerFeature : public ProcessorTracker
-{
- public:
-
- /** \brief Constructor with type
- */
- ProcessorTrackerFeature(const std::string& _type,
- ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
- virtual ~ProcessorTrackerFeature();
-
- protected:
- ProcessorParamsTrackerFeaturePtr params_tracker_feature_;
- TrackMatrix track_matrix_;
-
- FeatureBasePtrList known_features_incoming_;
- FeatureMatchMap matches_last_from_incoming_;
-
- /** \brief Process known Features
- * \return The number of successful matches.
- *
- * This function operates on the \b incoming capture pointed by incoming_ptr_.
- *
- * This function does:
- * - Track Features against other Features in the \b origin Capture. Tips:
- * - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
- * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
- * - If required, correct the drift by re-comparing against the Features in origin.
- * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
- * - Create the necessary Features in the \b incoming Capture,
- * of the correct type, derived from FeatureBase.
- * - Create the factors, of the correct type, derived from FactorBase
- * (through FactorAnalytic or FactorSparse).
- */
- virtual unsigned int processKnown();
-
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _origin_feature input feature in origin capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) = 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!
- */
- virtual bool voteForKeyFrame() = 0;
-
- // We overload the advance and reset functions to update the lists of matches
- void advanceDerived();
- void resetDerived();
-
- /**\brief Process new Features
- *
- */
- virtual unsigned int processNew(const int& _max_features);
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) = 0;
-
- /** \brief Create a new factor and link it to the wolf tree
- * \param _feature_ptr pointer to the parent Feature
- * \param _feature_other_ptr pointer to the other feature constrained.
- *
- * Implement this method in derived classes.
- *
- * This function creates a factor of the appropriate type for the derived processor.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
-
- /** \brief Establish factors between features in Captures \b last and \b origin
- */
- virtual void establishFactors();
-
- public:
- Track track(SizeStd _trk_id);
-};
-
-inline wolf::Track ProcessorTrackerFeature::track(SizeStd _trk_id)
-{
- return track_matrix_.track(_trk_id);
-}
-
-} // namespace wolf
-
-#endif /* PROCESSOR_TRACKER_FEATURE_H_ */
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
deleted file mode 100644
index 05e3723e3f75e49a8e6917ef8c2c9aa4cd1858f8..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/**
- * \file processor_tracker_feature_corner.h
- *
- * Created on: Apr 11, 2016
- * \author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_FEATURE_CORNER_H_
-#define PROCESSOR_TRACKER_FEATURE_CORNER_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_corner_2D.h"
-#include "base/landmark/landmark_corner_2D.h"
-#include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_feature.h"
-
-//laser_scan_utils
-//#include "laser_scan_utils/scan_basics.h"
-//#include "laser_scan_utils/corner_detector.h"
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/corner_finder.h"
-
-namespace wolf
-{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureCorner);
-
-struct ProcessorParamsTrackerFeatureCorner : public ProcessorParamsTrackerFeature
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- unsigned int n_corners_th;
- const Scalar position_error_th = 1;
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureCorner);
-
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-//const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-//const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-//const Scalar position_error_th_ = 1;
-//const Scalar min_features_ratio_th_ = 0.5;
-
-class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
-{
- private:
- ProcessorParamsTrackerFeatureCornerPtr params_tracker_feature_corner_;
- //laserscanutils::ScanParams scan_params_;
- //laserscanutils::ExtractCornerParams corner_alg_params_;
- //laserscanutils::LaserScan laser_data_;
- laserscanutils::LineFinderIterative line_finder_;
- laserscanutils::CornerFinder corner_finder_;
- //TODO: add corner_finder_params
-
- FeatureBasePtrList corners_incoming_;
- FeatureBasePtrList corners_last_;
-
- Eigen::Matrix3s R_world_sensor_, R_world_sensor_prev_;
- Eigen::Matrix3s R_robot_sensor_;
- Eigen::Matrix3s R_current_prev_;
- Eigen::Vector3s t_world_sensor_, t_world_sensor_prev_;
- Eigen::Vector3s t_robot_sensor_;
- Eigen::Vector3s t_current_prev_;
- Eigen::Vector3s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorTrackerFeatureCorner(ProcessorParamsTrackerFeatureCornerPtr _params_tracker_feature_corner);
- virtual ~ProcessorTrackerFeatureCorner();
- virtual void configure(SensorBasePtr _sensor) { };
-
- protected:
-
- virtual void preProcess();
-
- void advanceDerived();
-
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences);
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _last_feature input feature in last capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
-
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
-
- private:
-
- void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _corner_list);
-
-};
-
-inline bool ProcessorTrackerFeatureCorner::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
-{
- return true;
-}
-
-} // namespace wolf
-
-#endif /* PROCESSOR_TRACKER_FEATURE_CORNER_H_ */
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
deleted file mode 100644
index d517431b134026fdebeb91d17748bb49471b38cc..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/**
- * \file processor_tracker_feature_dummy.h
- *
- * Created on: Apr 11, 2016
- * \author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_FEATURE_DUMMY_H_
-#define PROCESSOR_TRACKER_FEATURE_DUMMY_H_
-
-#include "base/wolf.h"
-#include "base/processor/processor_tracker_feature.h"
-#include "base/factor/factor_epipolar.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
-
-//Class
-class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
-{
-
- public:
- ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
- virtual ~ProcessorTrackerFeatureDummy();
- virtual void configure(SensorBasePtr _sensor) { };
-
- protected:
-
- static unsigned int count_;
- unsigned int n_feature_;
-
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences);
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _last_feature input feature in last capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
-
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
-
-};
-
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature),
- n_feature_(0)
-{
- //
-}
-
-inline ProcessorTrackerFeatureDummy::~ProcessorTrackerFeatureDummy()
-{
- //
-}
-
-inline bool ProcessorTrackerFeatureDummy::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
-{
- return true;
-}
-
-} // namespace wolf
-
-#endif /* PROCESSOR_TRACKER_FEATURE_DUMMY_H_ */
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
deleted file mode 100644
index 3f730b60dfd540cf9fd2a44da40b61240a4d6f9d..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature_image.h
+++ /dev/null
@@ -1,171 +0,0 @@
-#ifndef PROCESSOR_TRACKER_FEATURE_IMAGE_H
-#define PROCESSOR_TRACKER_FEATURE_IMAGE_H
-
-// Wolf includes
-#include "base/sensor/sensor_camera.h"
-#include "base/capture/capture_image.h"
-#include "base/feature/feature_point_image.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-#include "base/processor/processor_tracker_feature.h"
-#include "base/factor/factor_epipolar.h"
-#include "base/processor/processor_params_image.h"
-
-// vision_utils
-#include <detectors/detector_base.h>
-#include <descriptors/descriptor_base.h>
-#include <matchers/matcher_base.h>
-#include <algorithms/activesearch/alg_activesearch.h>
-
-// General includes
-#include <cmath>
-#include <complex> // std::complex, std::norm
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureImage);
-
-//class
-class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
-{
- // vision utils params
- protected:
-
- vision_utils::DetectorBasePtr det_ptr_;
- vision_utils::DescriptorBasePtr des_ptr_;
- vision_utils::MatcherBasePtr mat_ptr_;
- vision_utils::AlgorithmACTIVESEARCHPtr active_search_ptr_; // Active Search
-
- int cell_width_; ///< Active search cell width
- int cell_height_; ///< Active search cell height
- vision_utils::AlgorithmParamsACTIVESEARCHPtr params_tracker_feature_image_activesearch_ptr_; ///< Active search parameters
-
- protected:
-
- ProcessorParamsTrackerFeatureImagePtr params_tracker_feature_image_; ///< Struct with parameters of the processors
- cv::Mat image_last_, image_incoming_; ///< Images of the "last" and "incoming" Captures
-
- struct
- {
- unsigned int width_; ///< width of the image
- unsigned int height_; ///< height of the image
- } image_;
-
- public:
-
- // Lists to store values to debug
- std::list<cv::Rect> tracker_roi_;
- std::list<cv::Rect> tracker_roi_inflated_;
- std::list<cv::Rect> detector_roi_;
- std::list<cv::Point> tracker_target_;
-
- ProcessorTrackerFeatureImage(ProcessorParamsTrackerFeatureImagePtr _params_image);
- virtual ~ProcessorTrackerFeatureImage();
-
- virtual void configure(SensorBasePtr _sensor) ;
-
- protected:
-
- /**
- * \brief Does cast of the images and renews the active grid.
- */
- void preProcess();
-
- /**
- * \brief Does the drawing of the features.
- *
- * Used for debugging
- */
- void postProcess();
-
- void advanceDerived()
- {
- ProcessorTrackerFeature::advanceDerived();
- image_last_ = image_incoming_;
- }
-
- void resetDerived()
- {
- ProcessorTrackerFeature::resetDerived();
- image_last_ = image_incoming_;
- }
-
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences);
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _last_feature input feature in last capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out);
-
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
-
- private:
-
- /**
- * \brief Detects keypoints its descriptors in a specific roi of the image
- * \param _image input image in which the algorithm will search
- * \param _roi input roi used to define the area of search within the image
- * \param _new_keypoints output keypoints obtained in the function
- * \param new_descriptors output descriptors obtained in the function
- * \return the number of detected features
- */
- virtual unsigned int detect(cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,
- cv::Mat& _new_descriptors);
-
- /**
- * \brief Does the match between a target descriptor and (potentially) multiple candidate descriptors of a Feature.
- * \param _target_descriptor descriptor of the target
- * \param _candidate_descriptors descriptors of the candidates
- * \param _cv_matches output variable in which the best result will be stored (in the position [0])
- * \return normalized score of similarity (1 - exact match; 0 - complete mismatch)
- */
- virtual Scalar match(cv::Mat _target_descriptor, cv::Mat _candidate_descriptors,
- std::vector<cv::DMatch>& _cv_matches);
-
- // These only to debug, will disappear one day
- public:
- virtual void drawFeatures(cv::Mat _image);
- virtual void drawTarget(cv::Mat _image, std::list<cv::Point> _target_list);
- virtual void drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color);
- virtual void resetVisualizationFlag(FeatureBasePtrList& _feature_list_last);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params,
- const SensorBasePtr sensor_ptr = nullptr);
-
-};
-
-inline bool ProcessorTrackerFeatureImage::voteForKeyFrame()
-{
- return (incoming_ptr_->getFeatureList().size() < params_tracker_feature_image_->min_features_for_keyframe);
-}
-
-} // namespace wolf
-
-#endif // PROCESSOR_TRACKER_FEATURE_IMAGE_H
diff --git a/include/base/processor/processor_tracker_feature_polyline_2D.h b/include/base/processor/processor_tracker_feature_polyline_2D.h
deleted file mode 100644
index 7ac9687dfbb5aa8541bdc74306f38d98a2c75696..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature_polyline_2D.h
+++ /dev/null
@@ -1,209 +0,0 @@
-/*
- * processor_tracker_feature_polyline_2D.h
- *
- * Created on: Mar 11, 2019
- * Author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_
-#define PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_
-
-#include "base/processor/polyline_2D_utils.h"
-#include "base/processor/processor_tracker_feature.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/landmark/landmark_match_polyline_2D.h"
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/feature/feature_match_polyline_2D.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline2D);
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2D);
-typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DPtrList;
-typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DPtrList;
-typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
-typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
-
-struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar match_alignment_position_sq_norm_th;
- Scalar match_landmark_pose_sq_norm_th;
- Scalar match_feature_pose_sq_norm_th;
- Scalar class_position_error_th;
- unsigned int new_features_th;
- Scalar loop_time_th;
- std::vector<PolylineRectangularClass> polyline_classes;
-};
-
-class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
-{
- protected:
- ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_;
- laserscanutils::LineFinderIterative line_finder_;
-
- FeatureBasePtrList all_features_incoming_, all_features_last_;
- LandmarkMatchPolyline2DMap landmark_match_map_;
- LandmarkPolyline2DPtrList modified_lmks_;
- std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
-
- Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
- Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
- Eigen::Matrix2s R_sensor_world_incoming_, R_world_sensor_incoming_;
- Eigen::Vector2s t_sensor_world_incoming_, t_world_sensor_incoming_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Matrix2s R_last_incoming_, R_incoming_last_;
- Eigen::Vector2s t_last_incoming_, t_incoming_last_;
- bool extrinsics_transformation_computed_;
-
- public:
- ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params);
-
- virtual ~ProcessorTrackerFeaturePolyline2D();
-
- virtual void configure(SensorBasePtr _sensor){};
-
- protected:
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences);
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _origin_feature input feature in origin capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /**\brief Process new Features
- *
- */
- virtual unsigned int processNew(const unsigned int& _max_features);
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_new_features,
- FeatureBasePtrList& _features_last_out);
-
- /** \brief Create a new factor and link it to the wolf tree
- * \param _feature_ptr pointer to the parent Feature
- * \param _feature_other_ptr pointer to the other feature constrained.
- *
- * Implement this method in derived classes.
- *
- * This function creates a factor of the appropriate type for the derived processor.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr){ return nullptr; };
-
- /** \brief Establish factors between features in Captures \b last and \b origin
- */
- virtual void establishFactors();
-
- void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id);
-
- void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id);
-
- /** \brief create a landmark from a feature
- *
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr);
-
- /** \brief advance pointers
- *
- */
- virtual void advanceDerived() override;
-
- /** \brief reset pointers and match lists at KF creation
- *
- */
- virtual void resetDerived() override;
-
- /** \brief Pre-process
- *
- */
- virtual void preProcess() override;
-
- /** \brief Post-process
- *
- */
- virtual void postProcess() override;
-
- void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
- const FeaturePolyline2DPtr& _ftr_last,
- const FeaturePolyline2DPtr& _ftr_incoming,
- const Eigen::Matrix3s& _T_last_incoming_prior) const;
-
- void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const;
-
- bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const;
-
- void computeTransformations();
-
- LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
- FeatureMatchPolyline2DPtr ftr_match_incoming_last,
- LandmarkMatchPolyline2DPtr lmk_match_last) const;
-
- public:
-
- /// @brief Factory method
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr);
-
- const FeatureBasePtrList& getLastNewFeatures() const
- {
- return all_features_last_;
- }
-};
-
-} /* namespace wolf */
-
-#endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_ */
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
deleted file mode 100644
index b4e079eb723b60aa28e873167f4f4d7fb409cdeb..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark.h
+++ /dev/null
@@ -1,173 +0,0 @@
-/**
- * \file processor_tracker_landmark.h
- *
- * Created on: Apr 7, 2016
- * \author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_LANDMARK_H_
-#define PROCESSOR_TRACKER_LANDMARK_H_
-
-//wolf includes
-#include "base/processor/processor_tracker.h"
-#include "base/capture/capture_base.h"
-#include "base/landmark/landmark_match.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmark);
-
-struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker
-{
- //
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
-
-/** \brief Landmark tracker processor
- *
- * This is an abstract class.
- *
- * This class implements the incremental landmark tracker.
- *
- * The incremental tracker contains three pointers to three Captures of type CaptureBase,
- * named \b origin, \b last and \b incoming:
- * - \b origin: this points to a Capture where all Feature tracks start.
- * - \b last: the last Capture tracked by the tracker.
- * A sufficient subset of the Features in \b origin is still alive in \b last.
- * - \b incoming: the capture being received. The tracker operates on this Capture,
- * establishing correspondences between the features here and the features in \b origin.
- * Each successful correspondence
- * results in an extension of the track of the Feature up to the \b incoming Capture.
- *
- * This processor creates landmarks for new detected Features, and establishes factors Feature-Landmark.
- *
- * This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
- * As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
- * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by derived classes with '<=== IMPLEMENT'
- *
- * - On each incoming Capture,
- * - processKnown() : Track known features in the \b incoming Capture <--- IMPLEMENTED
- * - Check if enough Features are still tracked, and vote for a new KeyFrame if this number is too low:
- * - if voteForKeyFrame() <=== IMPLEMENT
- * - processNew() : Populate the tracker with new Features <--- IMPLEMENTED
- * - makeFrame(), setKey() : Make a KeyFrame with the \b last Capture <--- IMPLEMENTED
- * - establishFactors() : Establish factors of the new Features <--- IMPLEMENTED
- * - reset() : Reset the tracker with the \b last Capture as the new \b origin<--- IMPLEMENTED
- * - else
- * - advance() : Advance the tracker one Capture ahead <--- IMPLEMENTED
- *
- * The most important implemented methods are:
- * - processKnown() : which calls the pure virtual, to be implemented in derived classes:
- * - findLandmarks() : find Landmarks from the \b map in \b incoming <=== IMPLEMENT
- * - processNew() : which calls the pure virtuals:
- * - detectNewFeatures() : detects new Features in \b last <=== IMPLEMENT
- * - 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
- *
- * Should you need extra functionality for your derived types, you can overload these two methods,
- *
- * - preProcess() { }
- * - postProcess() { }
- *
- * which are called at the beginning and at the end of process() respectively.
- * See the doc of these functions for more info.
- */
-class ProcessorTrackerLandmark : public ProcessorTracker
-{
- public:
- ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
- virtual ~ProcessorTrackerLandmark();
-
- protected:
-
- ProcessorParamsTrackerLandmarkPtr params_tracker_landmark_;
- LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
- LandmarkMatchMap matches_landmark_from_incoming_;
- LandmarkMatchMap matches_landmark_from_last_;
-
- /** \brief Tracker function
- * \return The number of successful tracks.
- *
- * Find Features in \b incoming Capture corresponding to known landmarks in the \b Map.
- *
- * This is the tracker function to be implemented in derived classes.
- * It operates on the \b incoming capture pointed by incoming_ptr_.
- *
- * The function must populate the list of Features in the \b incoming Capture.
- * Features need to be of the correct type, derived from FeatureBase.
- */
- virtual unsigned int processKnown();
-
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences) = 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!
- */
- virtual bool voteForKeyFrame() = 0;
-
- // We overload the advance and reset functions to update the lists of matches
- void advanceDerived();
- void resetDerived();
-
- /** \brief Process new Features
- *
- */
- unsigned int processNew(const int& _max_features);
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out) = 0;
-
- /** \brief Creates a landmark for each of new_features_last_
- **/
- virtual void createNewLandmarks();
-
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) = 0;
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
-
- /** \brief Establish factors between features in Capture \b last and landmarks
- */
- virtual void establishFactors();
-};
-
-}// namespace wolf
-
-// IMPLEMENTATION
-#include "base/landmark/landmark_base.h"
-
-#endif /* PROCESSOR_TRACKER_LANDMARK_H_ */
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
deleted file mode 100644
index 903f14490f3dcb2c2f79e0900a35fd3e26a71173..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ /dev/null
@@ -1,181 +0,0 @@
-/*
- * processor_tracker_landmark_corner.h
- *
- * Created on: Apr 4, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_TRACKER_LANDMARK_CORNER_H_
-#define SRC_PROCESSOR_TRACKER_LANDMARK_CORNER_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_corner_2D.h"
-#include "base/landmark/landmark_corner_2D.h"
-#include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-//laser_scan_utils
-//#include "laser_scan_utils/scan_basics.h"
-//#include "laser_scan_utils/corner_detector.h"
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/corner_finder.h"
-
-namespace wolf
-{
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-const Scalar position_error_th_ = 1;
-const Scalar min_features_ratio_th_ = 0.5;
-
-//forward declaration to typedef class pointers
-struct ProcessorParamsLaser;
-typedef std::shared_ptr<ProcessorParamsLaser> ProcessorParamsLaserPtr;
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkCorner);
-
-struct ProcessorParamsLaser : public ProcessorParamsTrackerLandmark
-{
- laserscanutils::LineFinderIterativeParams line_finder_params_;
- //TODO: add corner_finder_params
- unsigned int new_corners_th;
- unsigned int loop_frames_th;
-
- // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
- // Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
- // Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
- // Scalar position_error_th_ = 1;
- // Scalar min_features_ratio_th_ = 0.5;
-};
-
-class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
-{
- private:
- laserscanutils::LineFinderIterative line_finder_;
- laserscanutils::CornerFinder corner_finder_;
- //TODO: add corner_finder_params
-
- FeatureBasePtrList corners_incoming_;
- FeatureBasePtrList corners_last_;
- unsigned int new_corners_th_;
- unsigned int loop_frames_th_;
-
- // These values are constant -- provide a setter or accept them at construction time if you need to configure them
- Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-// Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-// Scalar position_error_th_ = 1;
-// Scalar min_features_ratio_th_ = 0.5;
-
- Eigen::Matrix3s R_sensor_world_, R_world_sensor_;
- Eigen::Matrix3s R_robot_sensor_;
- Eigen::Matrix3s R_current_prev_;
- Eigen::Vector3s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
- Eigen::Vector3s t_robot_sensor_;
- Eigen::Vector3s t_current_prev_;
- Eigen::Vector3s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorTrackerLandmarkCorner(ProcessorParamsLaserPtr _params_laser);
-
- virtual ~ProcessorTrackerLandmarkCorner();
- virtual void configure(SensorBasePtr _sensor) { };
-
- protected:
-
- virtual void preProcess();
-// virtual void postProcess() { }
-
- void advanceDerived()
- {
- //std::cout << "\tProcessorTrackerLandmarkCorner::advance:" << std::endl;
- //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << corners_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::advanceDerived();
- while (!corners_last_.empty())
- {
- corners_last_.front()->remove();
- corners_last_.pop_front(); // TODO check if this is needed
- }
- corners_last_ = std::move(corners_incoming_);
- }
-
- void resetDerived()
- {
- //std::cout << "\tProcessorTrackerLandmarkCorner::reset:" << std::endl;
- //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << corners_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::resetDerived();
- corners_last_ = std::move(corners_incoming_);
- }
-
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
-
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-
- private:
-
- void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _corner_list);
-
- void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Vector4s& expected_feature_,
- Eigen::Matrix3s& expected_feature_cov_);
-
- Eigen::VectorXs computeSquaredMahalanobisDistances(const FeatureBasePtr _feature_ptr,
- const Eigen::Vector4s& _expected_feature,
- const Eigen::Matrix3s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
deleted file mode 100644
index a312ce4a08b8b71a2cc9d8519932d5c4074fc653..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/**
- * \file processor_tracker_landmark_dummy.h
- *
- * Created on: Apr 12, 2016
- * \author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
-#define PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
-
-#include "base/processor/processor_tracker_landmark.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
-
-class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
-{
- public:
- ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
- virtual ~ProcessorTrackerLandmarkDummy();
- virtual void configure(SensorBasePtr _sensor) { };
-
- protected:
-
- unsigned int n_feature_;
- unsigned int landmark_idx_non_visible_;
-
-// virtual void preProcess() { }
- virtual void postProcess(); // implemented
-
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
-
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-};
-
-inline void ProcessorTrackerLandmarkDummy::postProcess()
-{
- landmark_idx_non_visible_++;
- std::cout << "------- Landmarks until " << landmark_idx_non_visible_ << " are now out of scope" << std::endl
- << std::endl;
-}
-
-} // namespace wolf
-
-// IMPLEMENTATION
-
-namespace wolf
-{
-
-} // namespace wolf
-
-#endif /* PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ */
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
deleted file mode 100644
index 9765efd4be3584479e4322ca268d4de3438a28ba..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ /dev/null
@@ -1,194 +0,0 @@
-#ifndef PROCESSOR_TRACKER_LANDMARK_IMAGE_H
-#define PROCESSOR_TRACKER_LANDMARK_IMAGE_H
-
-// Wolf includes
-
-#include "base/landmark/landmark_AHP.h"
-#include "base/landmark/landmark_match.h"
-#include "base/processor/processor_params_image.h"
-#include "base/processor/processor_tracker_landmark.h"
-#include "base/wolf.h"
-
-#include <algorithms/activesearch/alg_activesearch.h>
-#include <descriptors/descriptor_base.h>
-#include <detectors/detector_base.h>
-#include <matchers/matcher_base.h>
-
-#include <opencv2/core/mat.hpp>
-#include <opencv2/core/mat.inl.hpp>
-#include <opencv2/core/types.hpp>
-
-#include <list>
-#include <memory>
-#include <string>
-#include <vector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkImage);
-
-//Class
-class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
-{
- protected:
-
- vision_utils::DetectorBasePtr det_ptr_;
- vision_utils::DescriptorBasePtr des_ptr_;
- vision_utils::MatcherBasePtr mat_ptr_;
- vision_utils::AlgorithmACTIVESEARCHPtr active_search_ptr_; // Active Search
-
- int cell_width_; ///< Active search cell width
- int cell_height_; ///< Active search cell height
- vision_utils::AlgorithmParamsACTIVESEARCHPtr params_tracker_landmark_image_activesearch_ptr_; ///< Active search parameters
-
- protected:
- ProcessorParamsTrackerLandmarkImagePtr params_tracker_landmark_image_; // Struct with parameters of the processors
-
- cv::Mat image_last_, image_incoming_; // Images of the "last" and "incoming" Captures
-
- struct
- {
- unsigned int width_; ///< width of the image
- unsigned int height_; ///< height of the image
- }image_;
-
- unsigned int landmarks_tracked_ = 0;
-
- /* pinhole params */
- Eigen::Vector2s distortion_;
- Eigen::Vector2s correction_;
-
- /* transformations */
- Eigen::Vector3s world2cam_translation_;
- Eigen::Vector4s world2cam_orientation_;
-
- Eigen::Vector3s cam2world_translation_;
- Eigen::Vector4s cam2world_orientation_;
-
- unsigned int n_feature_;
- unsigned int landmark_idx_non_visible_;
-
- std::list<float> list_response_;
-
- public:
-
- // Lists to store values to debug
- std::list<cv::Rect> tracker_roi_;
- std::list<cv::Rect> detector_roi_;
- std::list<cv::Point> tracker_target_;
- FeatureBasePtrList feat_lmk_found_;
-
- ProcessorTrackerLandmarkImage(ProcessorParamsTrackerLandmarkImagePtr _params_tracker_landmark_image);
- virtual ~ProcessorTrackerLandmarkImage();
-
- virtual void configure(SensorBasePtr _sensor) ;
-
- protected:
-
- /**
- * \brief Does cast of the images and renews the active grid.
- */
- void preProcess();
-
- void advanceDerived()
- {
- ProcessorTrackerLandmark::advanceDerived();
- image_last_ = image_incoming_;
- }
-
- void resetDerived()
- {
- ProcessorTrackerLandmark::resetDerived();
- image_last_ = image_incoming_;
- }
-
- /**
- * \brief Does the drawing of the features.
- *
- * Used for debugging
- */
- void postProcess();
-
- //Pure virtual
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out);
-
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-
- //Other functions
- private:
-
- /**
- * \brief Detects keypoints its descriptors in a specific roi of the image
- * \param _image input image in which the algorithm will search
- * \param _roi input roi used to define the area of search within the image
- * \param _new_keypoints output keypoints obtained in the function
- * \param new_descriptors output descriptors obtained in the function
- * \return the number of detected features
- */
- unsigned int detect(const cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,cv::Mat& new_descriptors);
-
- /**
- * \brief Does the match between a target descriptor and (potentially) multiple candidate descriptors of a Feature.
- * \param _target_descriptor descriptor of the target
- * \param _candidate_descriptors descriptors of the candidates
- * \param _cv_matches output variable in which the best result will be stored (in the position [0])
- * \return normalized score of similarity (1 - exact match; 0 - complete mismatch)
- */
- Scalar match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, std::vector<cv::DMatch>& _cv_matches);
-
- void landmarkInCurrentCamera(const Eigen::VectorXs& _frame_state,
- const LandmarkAHPPtr _landmark,
- Eigen::Vector4s& _point3D_hmg);
-
- // These only to debug, will disappear one day soon
- public:
- void drawLandmarks(cv::Mat _image);
- void drawFeaturesFromLandmarks(cv::Mat _image);
- void drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color);
- void drawTrackerRoi(cv::Mat _image, cv::Scalar _color);
-
-};
-
-} // namespace wolf
-
-#endif // PROCESSOR_TRACKER_LANDMARK_IMAGE_H
diff --git a/include/base/rotations.h b/include/base/rotations.h
deleted file mode 100644
index c7c983a8beb855fbf519246ed9b8687ac8e89dcc..0000000000000000000000000000000000000000
--- a/include/base/rotations.h
+++ /dev/null
@@ -1,658 +0,0 @@
-/**
- * \file rotations.h
- *
- * Created on: Sep 6, 2016
- * \author: jsola
- */
-
-#ifndef ROTATIONS_H_
-#define ROTATIONS_H_
-
-#include "base/wolf.h"
-
-namespace wolf
-{
-
-//////////////////////////////////////////////////////////////
-
-/** \brief Return angle between -pi and pi
- *
- * @param angle
- * @return formatted angle
- */
-template<typename T>
-inline T
-pi2pi(const T angle)
-{
- using std::fmod;
-
- return (angle > (T)0 ?
- fmod(angle + (T)M_PI, (T)(2*M_PI)) - (T)M_PI :
- fmod(angle - (T)M_PI, (T)(2*M_PI)) + (T)M_PI);
-}
-
-/** \brief Conversion to radians
- *
- * @param deg angle in degrees
- * @return angle in radians
- */
-template<typename T>
-inline T
-toRad(const T deg)
-{
- return (T)M_TORAD * deg;
-}
-
-/** \brief Conversion to degrees
- *
- * @param rad angle in radians
- * @return angle in degrees
- */
-template<typename T>
-inline T
-toDeg(const T rad)
-{
- return (T)M_TODEG * rad;
-}
-
-//////////////////////////////////////////////////////////////////
-// Operators skew and vee
-
-/** \brief Skew symmetric matrix
- *
- * @param _v a 3D vector
- * @return the skew-symmetric matrix V so that V*u = _v.cross(u), for u in R^3
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-skew(const Eigen::MatrixBase<Derived>& _v)
-{
- MatrixSizeCheck<3,1>::check(_v);
-
- typedef typename Derived::Scalar T;
-
- Eigen::Matrix<T, 3, 3> sk;
-
- sk << (T)0.0 , -_v(2), +_v(1),
- +_v(2), (T)0.0, -_v(0),
- -_v(1), +_v(0), (T)0.0;
-
- return sk;
-}
-
-/** \brief Inverse of skew symmetric matrix
- *
- * @param _m A skew-symmetric matrix
- * @return a 3-vector v such that skew(v) = _m
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-vee(const Eigen::MatrixBase<Derived>& _m)
-{
- MatrixSizeCheck<3,3>::check(_m);
-
- typedef typename Derived::Scalar T;
-
- return (Eigen::Matrix<T, 3, 1>() << _m(2, 1), _m(0, 2), _m(1, 0)).finished();
-}
-
-////////////////////////////////////////////////////
-// Rotation conversions q, R, Euler, back and forth
-//
-// Euler angles are denoted 'e' and are in the ZYX convention
-
-/** \brief quaternion to rotation matrix conversion
- *
- * @param _q a right-handed unit quaternion
- * @return the equivalent rotation matrix
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-q2R(const Eigen::QuaternionBase<Derived>& _q)
-{
- return _q.matrix();
-}
-
-/** \brief quaternion to rotation matrix conversion
- *
- * @param _q a right-handed unit quaternion
- * @return the equivalent rotation matrix
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-q2R(const Eigen::MatrixBase<Derived>& _q)
-{
- MatrixSizeCheck<4,1>::check(_q);
- Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2));
- return q2R( q );
-}
-
-/** \brief rotation matrix to quaternion conversion
- *
- * @param _R a rotation matrix
- * @return the equivalent right-handed unit quaternion
- */
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar>
-R2q(const Eigen::MatrixBase<Derived>& _R)
-{
- MatrixSizeCheck<3,3>::check(_R);
-
- return Eigen::Quaternion<typename Derived::Scalar>(_R);
-}
-
-/** \brief Euler angles to quaternion
- * \param _euler = (roll pitch yaw) in ZYX convention
- * \return equivalent quaternion
- */
-template<typename D>
-inline Eigen::Quaternion<typename D::Scalar>
-e2q(const Eigen::MatrixBase<D>& _euler)
-{
- MatrixSizeCheck<3,1>::check(_euler);
-
- typedef typename D::Scalar T;
-
- const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(_euler(0), Eigen::Matrix<T, 3, 1>::UnitX());
- const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(_euler(1), Eigen::Matrix<T, 3, 1>::UnitY());
- const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(_euler(2), Eigen::Matrix<T, 3, 1>::UnitZ());
-
- return Eigen::Quaternion<T>(az * ay * ax);
-}
-
-/** \brief Euler angles to rotation matrix
- * \param _e = (roll pitch yaw) in ZYX convention
- * \return equivalent rotation matrix
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-e2R(const Eigen::MatrixBase<Derived>& _e)
-{
- MatrixSizeCheck<3, 1>::check(_e);
-
- return e2q(_e).matrix();
-}
-
-template <typename Derived>
-inline typename Eigen::MatrixBase<Derived>::Scalar
-getYaw(const Eigen::MatrixBase<Derived>& R)
-{
- MatrixSizeCheck<3, 3>::check(R);
-
- using std::atan2;
- return atan2( R(1, 0), R(0, 0) );
-}
-
-template<typename Derived>
-inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
-R2e(const Eigen::MatrixBase<Derived>& _R)
-{
- Eigen::Matrix<typename Derived::Scalar, 3, 1> e;
-
- e(0) = atan2( _R(2,1), _R(2,2));
- e(1) = atan2(-_R(2,0), sqrt(_R(0,0)*_R(0,0) + _R(1,0)*_R(1,0)));
- e(2) = atan2( _R(1,0), _R(0,0));
-
- return e;
-}
-
-template<typename Derived>
-inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
-q2e(const Eigen::MatrixBase<Derived>& _q)
-{
- typedef typename Derived::Scalar T;
- Eigen::Matrix<T, 3, 1> e;
-
- Scalar w = _q(3);
- Scalar x = _q(0);
- Scalar y = _q(1);
- Scalar z = _q(2);
-
- Scalar xx = x*x;
- Scalar xy = x*y;
- Scalar xz = x*z;
- Scalar yy = y*y;
- Scalar yz = y*z;
- Scalar zz = z*z;
- Scalar wx = w*x;
- Scalar wy = w*y;
- Scalar wz = w*z;
-
- Scalar r00 = T(1) - T(2) * ( yy + zz );
- Scalar r10 = T(2) * ( xy + wz );
- Scalar r20 = T(2) * ( xz - wy );
- Scalar r21 = T(2) * ( yz + wx );
- Scalar r22 = T(1) - T(2) * ( xx + yy );
-
- e(0) = atan2( r21, r22);
- e(1) = atan2(-r20, sqrt(r00*r00 + r10*r10));
- e(2) = atan2( r10, r00);
-
- return e;
-}
-
-template<typename Derived>
-inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
-q2e(const Eigen::QuaternionBase<Derived>& _q)
-{
- return q2e(_q.coeffs());
-}
-
-///////////////////////////////////////////////////////////////
-// Rotation conversions - exp and log maps
-
-/** \brief Quaternion exponential map
- *
- * @param _v a rotation vector with _v.norm() the rotated angle and _v.normalized() the rotation axis.
- * @return the right-handed unit quaternion performing the rotation encoded by _v
- */
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar>
-exp_q(const Eigen::MatrixBase<Derived>& _v)
-{
- using std::sqrt;
- using std::cos;
- using std::sin;
-
- MatrixSizeCheck<3,1>::check(_v);
-
- typedef typename Derived::Scalar T;
-
- T angle_squared = _v.squaredNorm();
- T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
-
- if (angle > (T)(wolf::Constants::EPS_SMALL))
- {
- return Eigen::Quaternion<T> ( Eigen::AngleAxis<T>(angle, _v.normalized()) );
- }
- else
- {
- return Eigen::Quaternion<T> ( (T)1.0 , _v(0,0)/(T)2 , _v(1,0)/(T)2 , _v(2,0)/(T)2 );
- }
-}
-
-/** \brief Quaternion logarithmic map
- *
- * @param _q a unit right-handed quaternion
- * @return a rotation vector v such that _q = exp_q(v)
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-log_q(const Eigen::QuaternionBase<Derived>& _q)
-{
-
- // Will try this implementation once Eigen accepts it!
- // see https://forum.kde.org/viewtopic.php?f=74&t=143269&p=385299#p385265
- // typedef typename Derived::Scalar T;
- // Eigen::AngleAxis<T> aa(_q);
- // return aa.angle() * aa.axis();
-
- // In the meanwhile, we have a custom implementation as follows
-
- typedef typename Derived::Scalar T;
-
- Eigen::Matrix<T, 3, 1> vec = _q.vec();
- const T sin_angle_squared = vec.squaredNorm();
- if (sin_angle_squared > (T)wolf::Constants::EPS_SMALL)
- {
- const T sin_angle = sqrt(sin_angle_squared); // Allow ceres::Jet to use its own sqrt() version.
- const T& cos_angle = _q.w();
-
- /* If (cos_angle < 0) then angle >= pi/2 , means : angle for angle_axis vector >= pi (== 2*angle)
- |-> results in correct rotation but not a normalized angle_axis vector
-
- In that case we observe that 2 * angle ~ 2 * angle - 2 * pi,
- which is equivalent saying
-
- angle - pi = atan(sin(angle - pi), cos(angle - pi))
- = atan(-sin(angle), -cos(angle))
- */
- const T two_angle = T(2.0) * ((cos_angle < T(0.0)) ? atan2(-sin_angle, -cos_angle) : atan2(sin_angle, cos_angle));
- const T k = two_angle / sin_angle;
- return vec * k;
- }
- else
- {
- // small-angle approximation
- return vec * (T)2.0;
- }
-}
-
-/** \brief Rotation matrix exponential map
- *
- * @param _v a rotation vector
- * @return the rotation matrix performing the rotation encoded by _v
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-exp_R(const Eigen::MatrixBase<Derived>& _v)
-{
- using std::sqrt;
-
- MatrixSizeCheck<3, 1>::check(_v);
-
- typedef typename Derived::Scalar T;
-
- T angle_squared = _v.squaredNorm();
- T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
-
- if (angle > wolf::Constants::EPS_SMALL)
- return Eigen::AngleAxis<T>(angle, _v.normalized()).toRotationMatrix();
- else
- return Eigen::Matrix<T, 3, 3>::Identity() + skew(_v);
-}
-
-/** \brief Rotation matrix logarithmic map
- *
- * @param _R a 3D rotation matrix
- * @return the rotation vector v such that _R = exp_R(v)
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-log_R(const Eigen::MatrixBase<Derived>& _R)
-{
- return log_q(R2q(_R));
-}
-
-/** \brief Rotation vector to quaternion conversion
- *
- * @param _v a rotation vector
- * @return the equivalent right-handed unit quaternion
- */
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar>
-v2q(const Eigen::MatrixBase<Derived>& _v)
-{
- return exp_q(_v);
-}
-
-/** \brief Quaternion to rotation vector conversion
- *
- * @param _q a right-handed unit quaternion
- * @return the equivalent rotation vector
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-q2v(const Eigen::QuaternionBase<Derived>& _q)
-{
- return log_q(_q);
-}
-
-/** \brief Quaternion to rotation vector conversion
- *
- * @param _q a right-handed unit quaternion
- * @return the equivalent rotation vector
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-q2v(const Eigen::MatrixBase<Derived>& _qv)
-{
- MatrixSizeCheck<4, 1>::check(_qv);
- return log_q(Quaternions(_qv(3), _qv(0), _qv(1), _qv(2))); // caution quaternion constructor (w,x,y,z) but storage (x,y,z,w)
-}
-
-/** \brief Rotation vector to rotation matrix conversion
- *
- * @param _v a rotation vector
- * @return the equivalent rotation matrix
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-v2R(const Eigen::MatrixBase<Derived>& _v)
-{
- return exp_R(_v);
-}
-
-/** \brief Rotation matrix to rotation vector conversion
- *
- * @param _R a rotation matrix
- * @return the equivalent rotation vector
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-R2v(const Eigen::MatrixBase<Derived>& _R)
-{
- return log_R(_R);
-}
-
-/////////////////////////////////////////////////////////////////
-// Jacobians of SO(3)
-
-/** \brief Compute Jr (Right Jacobian)
- * Right Jacobian for exp map in SO(3) - equation (10.86) and following equations in
- * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
- *
- * expmap( theta + d_theta ) \approx expmap(theta) * expmap(Jr * d_theta)
- *
- * where Jr = jac_SO3_right(theta);
- *
- * This maps a perturbation in the tangent space (d_theta) to a perturbation on the manifold (expmap(Jr * d_theta))
- * so that:
- *
- * exp(theta+d_theta) = exp(theta)*exp(Jr(theta)*d_theta)
- */
-
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
-{
- using std::sqrt;
- using std::cos;
- using std::sin;
-
- MatrixSizeCheck<3, 1>::check(_theta);
-
- typedef typename Derived::Scalar T;
-
- T theta2 = _theta.squaredNorm();
- Eigen::Matrix<T, 3, 3> W(skew(_theta));
- if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
- Eigen::Matrix<T, 3, 3> M1, M2;
- M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
- M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
- return Eigen::Matrix<T, 3, 3>::Identity() - M1 + M2;
-}
-
-/** \brief Compute Jrinv (inverse of Right Jacobian which corresponds to the jacobian of log)
- * Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
- * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
- *
- * logmap( R * expmap(d_theta) ) \approx logmap( R ) + Jrinv * d_theta
- * logmap( q * expmap(d_theta) ) \approx logmap( q ) + Jrinv * d_theta
- *
- * where Jrinv = jac_SO3_right_inv(theta);
- *
- * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so that
- *
- * log( exp(theta) * exp(d_theta) ) = theta + Jrinv(theta) * d_theta
- *
- * or, having R = exp(theta),
- *
- * log( R * exp(d_theta) ) = log(R) + Jrinv(theta) * d_theta
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
-{
- using std::sqrt;
- using std::cos;
- using std::sin;
-
- MatrixSizeCheck<3, 1>::check(_theta);
-
- typedef typename Derived::Scalar T;
-
- T theta2 = _theta.squaredNorm();
- Eigen::Matrix<T, 3, 3> W(skew(_theta));
- if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
- Eigen::Matrix<T, 3, 3> M;
- M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
- return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; //is this really more optimized?
-}
-
-/** \brief Compute Jl (Left Jacobian)
- * Left Jacobian for exp map in SO(3) - equation (10.86) and following equations in
- * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
- *
- * expmap( theta + d_theta ) \approx expmap(Jl * d_theta) * expmap(theta)
- *
- * where Jl = jac_SO3_left(theta);
- *
- * This maps a perturbation in the tangent space (d_theta) to a perturbation on the manifold (expmap(Jl * d_theta))
- * so that:
- *
- * exp(theta+d_theta) = exp(Jl(theta)*d_theta)*exp(theta)
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
-{
- using std::sqrt;
- using std::cos;
- using std::sin;
-
- MatrixSizeCheck<3, 1>::check(_theta);
-
- typedef typename Derived::Scalar T;
-
- T theta2 = _theta.squaredNorm();
- Eigen::Matrix<T, 3, 3> W(skew(_theta));
- if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
- Eigen::Matrix<T, 3, 3> M1, M2;
- M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
- M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
- return Eigen::Matrix<T, 3, 3>::Identity() + M1 + M2;
-}
-
-/** \brief Compute Jl_inv (inverse of Left Jacobian which corresponds to the jacobian of log)
- * Left Jacobian for Log map in SO(3) - equation (10.86) and following equations in
- * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
- *
- * logmap( expmap(d_theta) * R ) \approx logmap( R ) + Jlinv * d_theta
- *
- * where Jlinv = jac_SO3_left_inv(theta);
- *
- * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so that
- *
- * log( exp(d_theta) * exp(theta) ) = theta + Jlinv(theta) * d_theta
- *
- * or, having R = exp(theta),
- *
- * log( exp(d_theta) * R ) = log(R) + Jlinv(theta) * d_theta
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
-{
- using std::sqrt;
- using std::cos;
- using std::sin;
-
- MatrixSizeCheck<3, 1>::check(_theta);
-
- typedef typename Derived::Scalar T;
-
- T theta2 = _theta.squaredNorm();
- Eigen::Matrix<T, 3, 3> W(skew(_theta));
- if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
- Eigen::Matrix<T, 3, 3> M;
- M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
- return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; //is this really more optimized?
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void
-compose(const Eigen::QuaternionBase<D1>& _q1,
- const Eigen::QuaternionBase<D2>& _q2,
- Eigen::QuaternionBase<D3>& _q_comp,
- Eigen::MatrixBase<D4>& _J_comp_q1,
- Eigen::MatrixBase<D5>& _J_comp_q2)
-{
- MatrixSizeCheck<3, 3>::check(_J_comp_q1);
- MatrixSizeCheck<3, 3>::check(_J_comp_q2);
-
- _q_comp = _q1 * _q2;
-
- _J_comp_q1 = q2R(_q2.conjugate()); // R2.tr
- _J_comp_q2 . setIdentity();
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void
-between(const Eigen::QuaternionBase<D1>& _q1,
- const Eigen::QuaternionBase<D2>& _q2,
- Eigen::QuaternionBase<D3>& _q_between,
- Eigen::MatrixBase<D4>& _J_between_q1,
- Eigen::MatrixBase<D5>& _J_between_q2)
-{
- MatrixSizeCheck<3, 3>::check(_J_between_q1);
- MatrixSizeCheck<3, 3>::check(_J_between_q2);
-
- _q_between = _q1.conjugate() * _q2;
-
- _J_between_q1 = -q2R(_q2.conjugate()*_q1); // - R2.tr * R1
- _J_between_q2 . setIdentity();
-}
-
-template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar>
-plus_right(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
-{
- MatrixSizeCheck<3,1>::check(v);
- return q * exp_q(v);
-}
-
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-minus_right(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
-{
- return log_q(q1.conjugate() * q2);
-}
-
-template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar>
-plus_left(const Eigen::MatrixBase<D2>& v, const Eigen::QuaternionBase<D1>& q)
-{
- MatrixSizeCheck<3,1>::check(v);
- return exp_q(v) * q;
-}
-
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-minus_left(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
-{
- return log_q(q2 * q1.conjugate());
-}
-
-template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar>
-plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
-{
- return plus_right(q, v);
-}
-
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
-{
- return minus_right(q1, q2);
-}
-
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
-{
- return minus(q1, q2);
-}
-
-} // namespace wolf
-
-#endif /* ROTATIONS_H_ */
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
deleted file mode 100644
index a56b8c26776e054dc469d563b113df7c4b68c4d3..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_GPS.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#ifndef SENSOR_GPS_H_
-#define SENSOR_GPS_H_
-
-/* WARNING: from here you cannot include gps_scan_utils
- * because is included in factorGPS, and factorGPS is included in wolf.h (or some other wolf file)
- * Otherwise wolf will not build without my library installed
- *
- * --- MAYBE IS NO MORE TRUE, AFTER THE INCLUDES FIX!! ---
- */
-
-//wolf
-#include "base/sensor/sensor_base.h"
-//#include "sensor_factory.h"
-//#include "base/factory.h"
-
-// std
-
-namespace wolf {
-
-struct IntrinsicsGPS : public IntrinsicsBase
-{
- // add GPS parameters here
- virtual ~IntrinsicsGPS() = default;
-};
-
-WOLF_PTR_TYPEDEFS(SensorGPS);
-
-class SensorGPS : public SensorBase
-{
-public:
- //pointer to sensor position, orientation, bias, init vehicle position and orientation
- SensorGPS(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _bias_ptr, StateBlockPtr _map_position_ptr, StateBlockPtr _map_orientation_ptr);
-
- StateBlockPtr getMapP() const;
-
- StateBlockPtr getMapO() const;
-
- virtual ~SensorGPS();
-
-public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
-
-};
-
-} // namespace wolf
-
-#endif //SENSOR_GPS_H_
diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
deleted file mode 100644
index a3f48258965fecfed21273dbf041c6713cf8623e..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_GPS_fix.h
+++ /dev/null
@@ -1,38 +0,0 @@
-
-#ifndef SENSOR_GPS_FIX_H_
-#define SENSOR_GPS_FIX_H_
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-
-// std includes
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
-struct IntrinsicsGPSFix : public IntrinsicsBase
-{
- Eigen::Vector3s noise_std;
- // Empty -- it acts only as a typedef for IntrinsicsBase, but allows future extension with parameters
- virtual ~IntrinsicsGPSFix() = default;
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
-WOLF_PTR_TYPEDEFS(SensorGPSFix);
-
-class SensorGPSFix : public SensorBase
-{
- public:
- SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics);
- SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr);
-
- virtual ~SensorGPSFix();
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
deleted file mode 100644
index c62ec6fc11e2f3e0c194744843bb0b5b8556e460..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_IMU.h
+++ /dev/null
@@ -1,98 +0,0 @@
-#ifndef SENSOR_IMU_H
-#define SENSOR_IMU_H
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-#include <iostream>
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
-
-//TODO : bias_ptr defined as intrinsics StateBlock in constructor (see SensorBase) but here we also have another intrinsics
-// This is confusing.
-
-struct IntrinsicsIMU : public IntrinsicsBase
-{
- //noise std dev
- Scalar w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
- Scalar a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
-
- //Initial biases std dev
- Scalar ab_initial_stdev = 0.01; //accelerometer micro_g/sec
- Scalar wb_initial_stdev = 0.01; //gyroscope rad/sec
-
- // bias rate of change std dev
- Scalar ab_rate_stdev = 0.00001;
- Scalar wb_rate_stdev = 0.00001;
-
- virtual ~IntrinsicsIMU() = default;
-};
-
-WOLF_PTR_TYPEDEFS(SensorIMU);
-
-class SensorIMU : public SensorBase
-{
-
- protected:
- Scalar a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
- Scalar w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
-
- //This is a trial to factor how much can the bias change in 1 sec at most
- Scalar ab_initial_stdev; //accelerometer micro_g/sec
- Scalar wb_initial_stdev; //gyroscope rad/sec
- Scalar ab_rate_stdev; //accelerometer micro_g/sec
- Scalar wb_rate_stdev; //gyroscope rad/sec
-
- public:
-
- SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params);
- SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params);
-
- Scalar getGyroNoise() const;
- Scalar getAccelNoise() const;
- Scalar getWbInitialStdev() const;
- Scalar getAbInitialStdev() const;
- Scalar getWbRateStdev() const;
- Scalar getAbRateStdev() const;
-
- virtual ~SensorIMU();
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics = nullptr);
-
-};
-
-inline Scalar SensorIMU::getGyroNoise() const
-{
- return w_noise;
-}
-
-inline Scalar SensorIMU::getAccelNoise() const
-{
- return a_noise;
-}
-
-inline Scalar SensorIMU::getWbInitialStdev() const
-{
- return wb_initial_stdev;
-}
-
-inline Scalar SensorIMU::getAbInitialStdev() const
-{
- return ab_initial_stdev;
-}
-
-inline Scalar SensorIMU::getWbRateStdev() const
-{
- return wb_rate_stdev;
-}
-
-inline Scalar SensorIMU::getAbRateStdev() const
-{
- return ab_rate_stdev;
-}
-
-} // namespace wolf
-
-#endif // SENSOR_IMU_H
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
deleted file mode 100644
index 64ed42ba22597606c616af57c9ba23984ad3b2a7..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_base.h
+++ /dev/null
@@ -1,323 +0,0 @@
-#ifndef SENSOR_BASE_H_
-#define SENSOR_BASE_H_
-
-// Fwd refs
-namespace wolf{
-class HardwareBase;
-class ProcessorBase;
-class StateBlock;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
-
-//std includes
-
-namespace wolf {
-
-/** \brief base struct for intrinsic sensor parameters
- *
- * Derive from this struct to create structs of sensor intrinsic parameters.
- */
-struct IntrinsicsBase
-{
- virtual ~IntrinsicsBase() = default;
-};
-
-class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
-{
- private:
- HardwareBaseWPtr hardware_ptr_;
- ProcessorBasePtrList processor_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
- SizeEigen calib_size_;
-
- static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
-
- protected:
- unsigned int sensor_id_; // sensor ID
-
- bool extrinsic_dynamic_;// extrinsic parameters vary with time? If so, they will be taken from the Capture nodes.
- bool intrinsic_dynamic_;// intrinsic parameters vary with time? If so, they will be taken from the Capture nodes.
- bool has_capture_; // indicates this sensor took at least one capture
-
- Eigen::VectorXs noise_std_; // std of sensor noise
- Eigen::MatrixXs noise_cov_; // cov matrix of noise
-
- std::map<unsigned int,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by index in state_block_vec_)
-
- public:
- /** \brief Constructor with noise size
- *
- * Constructor with parameter vector
- * \param _tp Type of the sensor (types defined at wolf.h)
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
- * \param _noise_size dimension of the noise term
- * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
- *
- **/
- SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const unsigned int _noise_size,
- const bool _extr_dyn = false,
- const bool _intr_dyn = false);
-
- /** \brief Constructor with noise std vector
- *
- * Constructor with parameter vector
- * \param _tp Type of the sensor (types defined at wolf.h)
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
- * \param _noise_std standard deviations of the noise term
- * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
- *
- **/
- SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn = false,
- const bool _intr_dyn = false);
-
- virtual ~SensorBase();
-
- unsigned int id();
-
- virtual void setProblem(ProblemPtr _problem) final;
-
- HardwareBasePtr getHardware();
- void setHardware(const HardwareBasePtr _hw_ptr);
-
- ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
- ProcessorBasePtrList& getProcessorList();
-
- CaptureBasePtr lastKeyCapture(void);
- CaptureBasePtr lastCapture(const TimeStamp& _ts);
-
- bool process(const CaptureBasePtr capture_ptr);
-
- // State blocks
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
- StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
- StateBlockPtr getStateBlock(unsigned int _i);
- StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts);
- void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
- void resizeStateBlockVec(unsigned int _size);
-
- bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap);
- bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap);
- bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts);
- bool isStateBlockDynamic(unsigned int _i);
-
- StateBlockPtr getP(const TimeStamp _ts);
- StateBlockPtr getO(const TimeStamp _ts);
- StateBlockPtr getIntrinsic(const TimeStamp _ts);
- StateBlockPtr getP() ;
- StateBlockPtr getO();
- StateBlockPtr getIntrinsic();
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
- void setIntrinsic(const StateBlockPtr _intr_ptr);
- void removeStateBlocks();
-
- void fix();
- void unfix();
- void fixExtrinsics();
- void unfixExtrinsics();
- void fixIntrinsics();
- void unfixIntrinsics();
-
- /** \brief Add an absolute factor to a parameter
- *
- * Add an absolute factor to a parameter
- * \param _i state block index (in state_block_vec_) of the parameter to be constrained
- * \param _x prior value
- * \param _cov covariance
- * \param _start_idx state segment starting index (not used in quaternions)
- * \param _size state segment size (-1: whole state) (not used in quaternions)
- *
- **/
- void addPriorParameter(const unsigned int _i,
- const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov,
- unsigned int _start_idx = 0,
- int _size = -1);
- void addPriorP(const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov,
- unsigned int _start_idx = 0,
- int _size = -1);
- void addPriorO(const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov);
- void addPriorIntrinsics(const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov,
- unsigned int _start_idx = 0,
- int _size = -1);
-
- SizeEigen getCalibSize() const;
- Eigen::VectorXs getCalibration() const;
-
- virtual void registerNewStateBlocks();
-
- bool isExtrinsicDynamic() const;
- bool isIntrinsicDynamic() const;
- bool hasCapture() const {return has_capture_;}
- void setHasCapture() {has_capture_ = true;}
- bool extrinsicsInCaptures() const { return extrinsic_dynamic_ && has_capture_; }
- bool intrinsicsInCaptures() const { return intrinsic_dynamic_ && has_capture_; }
-
- void setNoiseStd(const Eigen::VectorXs & _noise_std);
- void setNoiseCov(const Eigen::MatrixXs & _noise_std);
- Eigen::VectorXs getNoiseStd();
- Eigen::MatrixXs getNoiseCov();
- void setExtrinsicDynamic(bool _extrinsic_dynamic);
- void setIntrinsicDynamic(bool _intrinsic_dynamic);
-
- protected:
- SizeEigen computeCalibSize() const;
-
- private:
- void updateCalibSize();
-};
-
-}
-
-#include "base/problem.h"
-#include "base/hardware_base.h"
-#include "base/capture/capture_base.h"
-#include "base/processor/processor_base.h"
-
-namespace wolf{
-
-inline unsigned int SensorBase::id()
-{
- return sensor_id_;
-}
-
-inline ProcessorBasePtrList& SensorBase::getProcessorList()
-{
- return processor_list_;
-}
-
-inline const std::vector<StateBlockPtr>& SensorBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& SensorBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline StateBlockPtr SensorBase::getStateBlockPtrStatic(unsigned int _i) const
-{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
-}
-
-inline void SensorBase::setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr)
-{
- assert (_i < state_block_vec_.size() && "Setting a state block pointer out of the vector range!");
- assert((params_prior_map_.find(_i) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
- state_block_vec_[_i] = _sb_ptr;
-}
-
-inline void SensorBase::resizeStateBlockVec(unsigned int _size)
-{
- if (_size > state_block_vec_.size())
- state_block_vec_.resize(_size);
-}
-
-inline bool SensorBase::isExtrinsicDynamic() const
-{
- // If this Sensor has no Capture yet, we'll consider it static
- return extrinsic_dynamic_;
-}
-
-inline bool SensorBase::isIntrinsicDynamic() const
-{
- // If this Sensor has no Capture yet, we'll consider it static
- return intrinsic_dynamic_;
-}
-
-inline Eigen::VectorXs SensorBase::getNoiseStd()
-{
- return noise_std_;
-}
-
-inline Eigen::MatrixXs SensorBase::getNoiseCov()
-{
- return noise_cov_;
-}
-
-inline HardwareBasePtr SensorBase::getHardware()
-{
- return hardware_ptr_.lock();
-}
-
-inline void SensorBase::setP(const StateBlockPtr _p_ptr)
-{
- setStateBlockPtrStatic(0, _p_ptr);
-}
-
-inline void SensorBase::setO(const StateBlockPtr _o_ptr)
-{
- setStateBlockPtrStatic(1, _o_ptr);
-}
-
-inline void SensorBase::setIntrinsic(const StateBlockPtr _intr_ptr)
-{
- setStateBlockPtrStatic(2, _intr_ptr);
-}
-
-inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
-{
- hardware_ptr_ = _hw_ptr;
-}
-
-inline void SensorBase::addPriorP(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
-{
- addPriorParameter(0, _x, _cov, _start_idx, _size);
-}
-
-inline void SensorBase::addPriorO(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
-{
- addPriorParameter(1, _x, _cov);
-}
-
-inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
-{
- addPriorParameter(2, _x, _cov);
-}
-
-inline SizeEigen SensorBase::getCalibSize() const
-{
- return calib_size_;
-}
-
-inline void SensorBase::updateCalibSize()
-{
- calib_size_ = computeCalibSize();
-}
-
-inline void SensorBase::setExtrinsicDynamic(bool _extrinsic_dynamic)
-{
- extrinsic_dynamic_ = _extrinsic_dynamic;
-}
-
-inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic)
-{
- intrinsic_dynamic_ = _intrinsic_dynamic;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
deleted file mode 100644
index c5b8f4423661a0b4dc2e5dc00cd17bd3392455cb..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_camera.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#ifndef SENSOR_CAMERA_H
-#define SENSOR_CAMERA_H
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera);
-/** Struct of intrinsic camera parameters
- */
-struct IntrinsicsCamera : public IntrinsicsBase
-{
- unsigned int width; ///< Image width in pixels
- unsigned int height; ///< Image height in pixels
- Eigen::Vector4s pinhole_model_raw; ///< k = [u_0, v_0, alpha_u, alpha_v] vector of pinhole intrinsic parameters
- Eigen::Vector4s pinhole_model_rectified;///< k = [u_0, v_0, alpha_u, alpha_v] vector of pinhole intrinsic parameters
- Eigen::VectorXs distortion; ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
-
- virtual ~IntrinsicsCamera() = default;
-};
-
-WOLF_PTR_TYPEDEFS(SensorCamera);
-/**Pin-hole camera sensor
- */
-class SensorCamera : public SensorBase
-{
- public:
-
- SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics);
- SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr);
-
- virtual ~SensorCamera();
-
- Eigen::VectorXs getDistortionVector() { return distortion_; }
- Eigen::VectorXs getCorrectionVector() { return correction_; }
- Eigen::Matrix3s getIntrinsicMatrix() { return K_; }
-
- bool isUsingRawImages() { return using_raw_; }
- bool useRawImages();
- bool useRectifiedImages();
-
-
- int getImgWidth(){return img_width_;}
- int getImgHeight(){return img_height_;}
- void setImgWidth(int _w){img_width_ = _w;}
- void setImgHeight(int _h){img_height_ = _h;}
-
- private:
- int img_width_;
- int img_height_;
- Eigen::VectorXs distortion_;
- Eigen::VectorXs correction_;
- Eigen::Vector4s pinhole_model_raw_, pinhole_model_rectified_;
- Eigen::Matrix3s K_;
- bool using_raw_;
-
- virtual Eigen::Matrix3s setIntrinsicMatrix(Eigen::Vector4s _pinhole_model);
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- static SensorBasePtr create(const std::string & _unique_name, //
- const Eigen::VectorXs& _extrinsics, //
- const IntrinsicsBasePtr _intrinsics);
-
-};
-
-inline bool SensorCamera::useRawImages()
-{
- getIntrinsic()->setState(pinhole_model_raw_);
- K_ = setIntrinsicMatrix(pinhole_model_raw_);
- using_raw_ = true;
-
- return true;
-}
-
-inline bool SensorCamera::useRectifiedImages()
-{
- getIntrinsic()->setState(pinhole_model_rectified_);
- K_ = setIntrinsicMatrix(pinhole_model_rectified_);
- using_raw_ = false;
-
- return true;
-}
-
-} // namespace wolf
-
-#endif // SENSOR_CAMERA_H
diff --git a/include/base/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
deleted file mode 100644
index 0f5cf42592ddf2cf1929ff6e31a9aa294a1963e4..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_diff_drive.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/**
- * \file sensor_diff_drive.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef WOLF_SENSOR_DIFF_DRIVE_H_
-#define WOLF_SENSOR_DIFF_DRIVE_H_
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-#include "base/diff_drive_tools.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive);
-WOLF_PTR_TYPEDEFS(SensorDiffDrive);
-
-struct IntrinsicsDiffDrive : public IntrinsicsBase
-{
- Scalar left_radius_;
- Scalar right_radius_;
- Scalar separation_;
-
- DiffDriveModel model_ = DiffDriveModel::Three_Factor_Model;
-
- Eigen::VectorXs factors_ = Eigen::Vector3s(1, 1, 1);
-
- Scalar left_resolution_;
- Scalar right_resolution_;
-
- Scalar left_gain_ = 0.01;
- Scalar right_gain_ = 0.01;
-
- virtual ~IntrinsicsDiffDrive() = default;
-};
-
-typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;
-
-class SensorDiffDrive : public SensorBase
-{
-public:
-
- /**
- * \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base.
- * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base.
- * \param _i_ptr StateBlock pointer to the sensor dynamic intrinsics (factors).
- * \param _intrinsics The intrinsics parameters of the sensor.
- *
- **/
- SensorDiffDrive(const StateBlockPtr& _p_ptr,
- const StateBlockPtr& _o_ptr,
- const StateBlockPtr& _i_ptr,
- const IntrinsicsDiffDrivePtr& _intrinsics);
-
- /**
- * \brief Default destructor (not recommended)
- **/
- virtual ~SensorDiffDrive() = default;
-
- inline IntrinsicsDiffDrivePtr getIntrinsics() const {return intrinsics_ptr_;}
-
-protected:
-
- IntrinsicsDiffDrivePtr intrinsics_ptr_;
-
-public:
-
- /**
- * @brief create. Factory function to create a SensorDiffDrive.
- * @param _unique_name. A unique name for the sensor.
- * @param _extrinsics_po. The (2d) position of the sensor w.r.t to the robot base frame.
- * @param _intrinsics. The sensor extrinsics parameters.
- * @return a SensorBasePtr holding the sensor. If the sensor creation failed,
- * the returned pointer is nullptr.
- */
- static SensorBasePtr create(const std::string& _unique_name,
- const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics);
-};
-
-} // namespace wolf
-
-#endif /* WOLF_SENSOR_DIFF_DRIVE_H_ */
diff --git a/include/base/sensor/sensor_factory.h b/include/base/sensor/sensor_factory.h
deleted file mode 100644
index 32884c18f0b7b1e3133648ebb946d216898a8f56..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_factory.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/**
- * \file sensor_factory.h
- *
- * Created on: Apr 25, 2016
- * \author: jsola
- */
-
-#ifndef SENSOR_FACTORY_H_
-#define SENSOR_FACTORY_H_
-
-namespace wolf
-{
-class SensorBase;
-struct IntrinsicsBase;
-}
-
-// wolf
-#include "base/factory.h"
-
-namespace wolf
-{
-
-/** \brief Sensor factory class
- *
- * This factory can create objects of classes deriving from SensorBase.
- *
- * Specific object creation is invoked by ````create(TYPE, params ... )````, and the TYPE of sensor is identified with a string.
- * Currently, the following sensor types are implemented,
- * - "CAMERA" for SensorCamera
- * - "ODOM 2D" for SensorOdom2D
- * - "GPS FIX" for SensorGPSFix
- *
- * The rule to make new TYPE strings unique is that you skip the prefix 'Sensor' from your class name,
- * and you build a string in CAPITALS with space separators, e.g.:
- * - SensorCamera -> ````"CAMERA"````
- * - SensorLaser2D -> ````"LASER 2D"````
- * - etc.
- *
- * The methods to create specific sensors are called __creators__.
- * Creators must be registered to the factory before they can be invoked for sensor creation.
- *
- * This documentation shows you how to:
- * - Access the factory
- * - Register and unregister creators
- * - Create sensors
- * - Write a sensor creator for SensorCamera (example).
- *
- * #### Accessing the factory
- * The SensorFactory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
- * To obtain an instance of it, use the static method get(),
- *
- * \code
- * SensorFactory::get()
- * \endcode
- *
- * You can then call the methods you like, e.g. to create a sensor, you type:
- *
- * \code
- * SensorFactory::get().create(...); // see below for creating sensors ...
- * \endcode
- *
- * #### Registering sensor creators
- * Prior to invoking the creation of a sensor of a particular type,
- * you must register the creator for this type into the factory.
- *
- * Registering sensor creators into the factory is done through registerCreator().
- * You provide a sensor type string (above), and a pointer to a static method
- * that knows how to create your specific sensor, e.g.:
- *
- * \code
- * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
- * \endcode
- *
- * The method SensorCamera::create() exists in the SensorCamera class as a static method.
- * All these ````SensorXxx::create()```` methods need to have exactly the same API, regardless of the sensor type.
- * This API includes a sensor name, a vector of extrinsic parameters,
- * and a pointer to a base struct of intrinsic parameters, IntrinsicsBasePtr,
- * that can be derived for each derived sensor:
- *
- * \code
- * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
- * \endcode
- *
- * See further down for an implementation example.
- *
- * #### Achieving automatic registration
- * Currently, registering is performed in each specific SensorXxxx source file, sensor_xxxx.cpp.
- * For example, in sensor_camera.cpp we find the line:
- *
- * \code
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
- * \endcode
- *
- * which is a static invocation (i.e., it is placed at global scope outside of the SensorCamera class).
- * Therefore, at application level, all sensors that have a .cpp file compiled are automatically registered.
- *
- * #### Unregistering sensor creators
- * The method unregisterCreator() unregisters the SensorXxx::create() method. It only needs to be passed the string of the sensor type.
- *
- * \code
- * SensorFactory::get().unregisterCreator("CAMERA");
- * \endcode
- *
- * #### Creating sensors
- * Note: Prior to invoking the creation of a sensor of a particular type,
- * you must register the creator for this type into the factory.
- *
- * To create e.g. a SensorCamera, you type:
- *
- * \code
- * SensorFactory::get().create("CAMERA", "Front-left camera", extrinsics, intrinsics_ptr);
- * \endcode
- *
- * where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
- * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS!
- *
- * #### See also
- * - IntrinsicsFactory: to create intrinsic structs deriving from IntrinsicsBase directly from YAML files.
- * - ProcessorFactory: to create processors that will be bound to sensors.
- * - Problem::installSensor() : to install sensors in WOLF Problem.
- *
- * #### Example 1: writing a specific sensor creator
- * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp:
- *
- * \code
- * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
- * {
- * // check extrinsics vector
- * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
- *
- * // cast instrinsics to good type
- * IntrinsicsCamera* intrinsics_ptr = (IntrinsicsCamera*) _intrinsics;
- *
- * // Do create the SensorCamera object, and complete its setup
- * SensorCamera* sen_ptr = new SensorCamera(_extrinsics_pq, intrinsics_ptr);
- * sen_ptr->setName(_unique_name);
- *
- * return sen_ptr;
- * }
- * \endcode
- *
- * #### Example 2: registering a sensor creator into the factory
- * Registration can be done manually or automatically. It involves the call to static functions.
- * It is advisable to put these calls within unnamed namespaces.
- *
- * - __Manual registration__: you control registration at application level.
- * Put the code either at global scope (you must define a dummy variable for this),
- * \code
- * namespace {
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
- * }
- * main () { ... }
- * \endcode
- * or inside your main(), where a direct call is possible:
- * \code
- * main () {
- * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
- * ...
- * }
- * \endcode
- *
- * - __Automatic registration__: registration is performed at library level.
- * Put the code at the last line of the sensor_xxx.cpp file,
- * \code
- * namespace {
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
- * }
- * \endcode
- * Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
- * You are free to comment out these lines and place them wherever you consider it more convenient for your design.
- *
- * #### Example 2: creating sensors
- * We finally provide the necessary steps to create a sensor of class SensorCamera in our application:
- *
- * \code
- * #include "sensor_factory.h"
- * #include "base/sensor/sensor_camera.h" // provides SensorCamera
- *
- * // Note: SensorCamera::create() is already registered, automatically.
- *
- * using namespace wolf;
- * int main() {
- *
- * // To create a camera, provide:
- * // a type = "CAMERA",
- * // a name = "Front-left camera",
- * // an extrinsics vector, and
- * // a pointer to the intrinsics struct:
- *
- * Eigen::VectorXs extrinsics_1(7); // give it some values...
- * IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct
- *
- * SensorBasePtr camera_1_ptr =
- * SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 );
- *
- * // A second camera... with a different name!
- *
- * Eigen::VectorXs extrinsics_2(7);
- * IntrinsicsCamera intrinsics_2({...});
- *
- * SensorBasePtr camera_2_ptr =
- * SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 );
- *
- * return 0;
- * }
- * \endcode
- *
- * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
- */
-
-typedef Factory<SensorBase,
- const std::string&,
- const Eigen::VectorXs&, const IntrinsicsBasePtr> SensorFactory;
-
-template<>
-inline std::string SensorFactory::getClass()
-{
- return "SensorFactory";
-}
-
-#define WOLF_REGISTER_SENSOR(SensorType, SensorName) \
- namespace{ const bool WOLF_UNUSED SensorName##Registered = \
- SensorFactory::get().registerCreator(SensorType, SensorName::create); }\
-
-} /* namespace wolf */
-
-#endif /* SENSOR_FACTORY_H_ */
diff --git a/include/base/sensor/sensor_laser_2D.h b/include/base/sensor/sensor_laser_2D.h
deleted file mode 100644
index 6b927c5b4bbbdc23a05cad52cc9f1be4a8590733..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_laser_2D.h
+++ /dev/null
@@ -1,86 +0,0 @@
-// sensor_laser_2D.h
-
-#ifndef SENSOR_LASER_2D_H_
-#define SENSOR_LASER_2D_H_
-
-//wolf
-#include "base/sensor/sensor_base.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-
-// std
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsLaser2D);
-
-struct IntrinsicsLaser2D : public IntrinsicsBase
-{
- virtual ~IntrinsicsLaser2D() = default;
-
- laserscanutils::LaserScanParams scan_params;
-};
-
-WOLF_PTR_TYPEDEFS(SensorLaser2D);
-
-class SensorLaser2D : public SensorBase
-{
- protected:
- laserscanutils::LaserScanParams scan_params_;
-
- public:
- /** \brief Constructor with extrinsics
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
-
- /** \brief Constructor with extrinsics and scan parameters
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev);
-
- /** \brief Constructor with extrinsics and scan parameters
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _params Scan parameters
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
-
- virtual ~SensorLaser2D();
-
- void setDefaultScanParams();
-
- /** \brief Set scanner intrinsic parameters
- *
- * \param _params struct with scanner intrinsic parameters. See laser_scan_utils library API for reference.
- *
- **/
- void setScanParams(const laserscanutils::LaserScanParams & _params);
-
- /** \brief Get scanner intrinsic parameters
- *
- * Get scanner intrinsic parameters
- *
- **/
- const laserscanutils::LaserScanParams & getScanParams() const;
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBasePtr _intrinsics);
- static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml);
-
-};
-
-} // namespace wolf
-
-#endif /*SENSOR_LASER_2D_H_*/
diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h
deleted file mode 100644
index f1528ec377f88f854ee905fbde29e0db5c381265..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_odom_2D.h
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#ifndef SENSOR_ODOM_2D_H_
-#define SENSOR_ODOM_2D_H_
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom2D);
-
-struct IntrinsicsOdom2D : public IntrinsicsBase
-{
- virtual ~IntrinsicsOdom2D() = default;
-
- Scalar k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
- Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
-};
-
-WOLF_PTR_TYPEDEFS(SensorOdom2D);
-
-class SensorOdom2D : public SensorBase
-{
- protected:
- Scalar k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
- Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
-
- public:
- SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics);
- SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics);
-
- virtual ~SensorOdom2D();
-
- /** \brief Returns displacement noise factor
- *
- * Returns displacement noise factor
- *
- **/
- Scalar getDispVarToDispNoiseFactor() const;
-
- /** \brief Returns rotation noise factor
- *
- * Returns rotation noise factor
- *
- **/
- Scalar getRotVarToRotNoiseFactor() const;
-
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
-
-};
-
-} // namespace wolf
-
-#endif // SENSOR_ODOM_2D_H_
diff --git a/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h
deleted file mode 100644
index be3694b2da8b69ad7201d57251f0246deb6dba92..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_odom_3D.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/**
- * \file sensor_odom_3D.h
- *
- * Created on: Oct 7, 2016
- * \author: jsola
- */
-
-#ifndef SRC_SENSOR_ODOM_3D_H_
-#define SRC_SENSOR_ODOM_3D_H_
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom3D);
-
-struct IntrinsicsOdom3D : public IntrinsicsBase
-{
- Scalar k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
- Scalar k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation
- Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
- Scalar min_disp_var;
- Scalar min_rot_var;
-
- virtual ~IntrinsicsOdom3D() = default;
-};
-
-WOLF_PTR_TYPEDEFS(SensorOdom3D);
-
-class SensorOdom3D : public SensorBase
-{
- protected:
- Scalar k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
- Scalar k_disp_to_rot_; ///< ratio of displacement variance to rotation, for odometry noise calculation
- Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
- Scalar min_disp_var_;
- Scalar min_rot_var_;
-
- public:
- SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& params);
- SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr params);
-
- virtual ~SensorOdom3D();
-
- Scalar getDispVarToDispNoiseFactor() const;
- Scalar getDispVarToRotNoiseFactor() const;
- Scalar getRotVarToRotNoiseFactor() const;
- Scalar getMinDispVar() const;
- Scalar getMinRotVar() const;
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
-
-};
-
-inline Scalar SensorOdom3D::getDispVarToDispNoiseFactor() const
-{
- return k_disp_to_disp_;
-}
-
-inline Scalar SensorOdom3D::getDispVarToRotNoiseFactor() const
-{
- return k_disp_to_rot_;
-}
-
-inline Scalar SensorOdom3D::getRotVarToRotNoiseFactor() const
-{
- return k_rot_to_rot_;
-}
-
-inline Scalar SensorOdom3D::getMinDispVar() const
-{
- return min_disp_var_;
-}
-
-inline Scalar SensorOdom3D::getMinRotVar() const
-{
- return min_rot_var_;
-}
-
-} /* namespace wolf */
-
-#endif /* SRC_SENSOR_ODOM_3D_H_ */
diff --git a/include/base/singleton.h b/include/base/singleton.h
deleted file mode 100644
index 3595ea584249a0261e6cd2f65b3c017846d0a2e3..0000000000000000000000000000000000000000
--- a/include/base/singleton.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/**
- * \file singleton.h
- *
- * Created on: Aug 31, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef WOLF_SINGLETON_H_
-#define WOLF_SINGLETON_H_
-
-#include <memory>
-
-namespace wolf {
-namespace internal {
-
-/**
- * \brief A thread-safer (?) Singleton implementation with
- * argument forwarding.
- **/
-template <class T>
-class Singleton
-{
- using SingletonOPtr = std::unique_ptr<T>;
-
-public:
-
- template <typename... Args>
- static T& get(Args&&... args)
- {
- static SingletonOPtr instance_(new T(std::forward<Args>(args)...));
-
- return *instance_;
- }
-
- constexpr Singleton(const Singleton&) = delete;
- constexpr Singleton(const Singleton&&) = delete;
-
- constexpr Singleton& operator=(const Singleton&) const = delete;
- constexpr Singleton& operator=(const Singleton&&) const = delete;
-
-protected:
-
- constexpr Singleton() = default;
- virtual ~Singleton() = default;
-};
-
-} // namespace internal
-} // namespace wolf
-
-#endif /* WOLF_SINGLETON_H_ */
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
deleted file mode 100644
index f64b3571970400704e9ea680e291b705362cdebc..0000000000000000000000000000000000000000
--- a/include/base/solver/solver_manager.h
+++ /dev/null
@@ -1,94 +0,0 @@
-#ifndef _WOLF_SOLVER_MANAGER_H_
-#define _WOLF_SOLVER_MANAGER_H_
-
-//wolf includes
-#include "base/wolf.h"
-#include "base/state_block.h"
-#include "base/factor/factor_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(SolverManager)
-
-/**
- * \brief Solver manager for WOLF
- */
-class SolverManager
-{
-public:
-
- /** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
- enum class CovarianceBlocksToBeComputed : std::size_t
- {
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
- };
-
- /**
- * \brief Enumeration for the verbosity of the solver report.
- */
- enum class ReportVerbosity : std::size_t
- {
- QUIET = 0,
- BRIEF,
- FULL
- };
-
-protected:
-
- ProblemPtr wolf_problem_;
-
-public:
-
- SolverManager(const ProblemPtr& wolf_problem);
-
- virtual ~SolverManager() = default;
-
- std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET);
-
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
-
- virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
-
- virtual bool hasConverged() = 0;
-
- virtual SizeStd iterations() = 0;
-
- virtual Scalar initialCost() = 0;
-
- virtual Scalar finalCost() = 0;
-
- virtual void update();
-
- ProblemPtr getProblem();
-
-protected:
-
- std::map<StateBlockPtr, Eigen::VectorXs> state_blocks_;
-
- virtual Eigen::VectorXs& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
- virtual Scalar* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
-
- virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
-
- virtual void addFactor(const FactorBasePtr& fac_ptr) = 0;
-
- virtual void removeFactor(const FactorBasePtr& fac_ptr) = 0;
-
- virtual void addStateBlock(const StateBlockPtr& state_ptr) = 0;
-
- virtual void removeStateBlock(const StateBlockPtr& state_ptr) = 0;
-
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) = 0;
-
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) = 0;
-};
-
-} // namespace wolf
-
-#endif /* _WOLF_SOLVER_MANAGER_H_ */
diff --git a/include/base/solver_suitesparse/ccolamd_ordering.h b/include/base/solver_suitesparse/ccolamd_ordering.h
deleted file mode 100644
index b20b857bdf543f65ec61d23a06a3a6ca9de567cb..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/ccolamd_ordering.h
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
- * ccolamd_ordering.h
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
-
-#ifndef TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
-#define TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
-
-//std includes
-#include <iostream>
-
-// Eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
-
-// ccolamd
-#include "ccolamd.h"
-
-namespace Eigen{
-
-template<typename Index>
-class CCOLAMDOrdering
-{
- public:
- typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
- typedef Matrix<Index, Dynamic, 1> IndexVector;
-
- template<typename MatrixType>
- void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr)
- {
- Index m = mat.rows();
- Index n = mat.cols();
- Index nnz = mat.nonZeros();
-
- // Get the recommended value of Alen to be used by colamd
- Index Alen = ccolamd_recommended(nnz, m, n);
- // Set the default parameters
- double knobs[CCOLAMD_KNOBS];
- Index stats[CCOLAMD_STATS];
- ccolamd_set_defaults(knobs);
-
- IndexVector p(n + 1), A(Alen);
- for (Index i = 0; i <= n; i++)
- p(i) = mat.outerIndex()[i];
- for (Index i = 0; i < nnz; i++)
- A(i) = mat.innerIndex()[i];
-// std::cout << "p = " << std::endl << p.transpose() << std::endl;
-// std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl;
-
- // Call CColamd routine to compute the ordering
- Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember);
- if (!info)
- assert(info && "CCOLAMD failed ");
-
- perm.resize(n);
- for (Index i = 0; i < n; i++)
- perm.indices()(p(i)) = i;
- }
-
- private:
- int compute_ccolamd(int &m, int &n, int &Alen, int* A, int* p, double* knobs, int* stats, int* cmember)
- {
- int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember);
- //ccolamd_report (stats) ;
- return info;
- }
-
- long int compute_ccolamd(long int &m, long int &n, long int &Alen, long int* A, long int* p, double* knobs, long int* stats, long int* cmember)
- {
- long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember);
- //ccolamd_l_report (stats) ;
- return info;
- }
-};
-}
-
-#endif /* TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ */
diff --git a/include/base/solver_suitesparse/cost_function_base.h b/include/base/solver_suitesparse/cost_function_base.h
deleted file mode 100644
index 8aae707aed16ec7229c3d4a6730a148d6492c785..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/cost_function_base.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- * cost_function_base.h
- *
- * Created on: Jun 25, 2015
- * Author: jvallve
- */
-
-#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
-#define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
-
-#include "base/wolf.h"
-#include <Eigen/StdVector>
-
-class CostFunctionBase
-{
- protected:
- unsigned int n_blocks_;
- Eigen::MatrixXs J_0_;
- Eigen::MatrixXs J_1_;
- Eigen::MatrixXs J_2_;
- Eigen::MatrixXs J_3_;
- Eigen::MatrixXs J_4_;
- Eigen::MatrixXs J_5_;
- Eigen::MatrixXs J_6_;
- Eigen::MatrixXs J_7_;
- Eigen::MatrixXs J_8_;
- Eigen::MatrixXs J_9_;
- Eigen::VectorXs residual_;
- std::vector<Eigen::MatrixXs*> jacobians_;
- std::vector<unsigned int> block_sizes_;
-
- public:
- CostFunctionBase(const unsigned int &_measurement_size,
- const unsigned int &_block_0_size,
- const unsigned int &_block_1_size,
- const unsigned int &_block_2_size,
- const unsigned int &_block_3_size,
- const unsigned int &_block_4_size,
- const unsigned int &_block_5_size,
- const unsigned int &_block_6_size,
- const unsigned int &_block_7_size,
- const unsigned int &_block_8_size,
- const unsigned int &_block_9_size) :
- n_blocks_(10),
- J_0_(_measurement_size, _block_0_size),
- J_1_(_measurement_size, _block_1_size),
- J_2_(_measurement_size, _block_2_size),
- J_3_(_measurement_size, _block_3_size),
- J_4_(_measurement_size, _block_4_size),
- J_5_(_measurement_size, _block_5_size),
- J_6_(_measurement_size, _block_6_size),
- J_7_(_measurement_size, _block_7_size),
- J_8_(_measurement_size, _block_8_size),
- J_9_(_measurement_size, _block_9_size),
- residual_(_measurement_size),
- jacobians_({&J_0_,&J_1_,&J_2_,&J_3_,&J_4_,&J_5_,&J_6_,&J_7_,&J_8_,&J_9_}),
- block_sizes_({_block_0_size, _block_1_size, _block_2_size, _block_3_size, _block_4_size, _block_5_size, _block_6_size, _block_7_size, _block_8_size, _block_9_size})
- {
- for (unsigned int i = 1; i<n_blocks_; i++)
- {
- if (block_sizes_.at(i) == 0)
- {
- n_blocks_ = i;
- jacobians_.resize(n_blocks_);
- block_sizes_.resize(n_blocks_);
- break;
- }
- }
- }
-
- virtual ~CostFunctionBase()
- {
-
- }
-
- virtual void evaluateResidualJacobians() = 0;
-
- void getResidual(Eigen::VectorXs &residual)
- {
- residual.resize(residual_.size());
- residual = residual_;
- }
-
- std::vector<Eigen::MatrixXs*> getJacobians()
- {
- return jacobians_;
- }
-
- void getJacobians(std::vector<Eigen::MatrixXs>& jacobians)
- {
- jacobians.resize(n_blocks_);
- for (unsigned int i = 0; i<n_blocks_; i++)
- jacobians.at(i) = (*jacobians_.at(i));
- }
-};
-
-#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_ */
diff --git a/include/base/solver_suitesparse/cost_function_sparse.h b/include/base/solver_suitesparse/cost_function_sparse.h
deleted file mode 100644
index bdcde3834cc43b950fd968a1c559ee778365e0b0..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/cost_function_sparse.h
+++ /dev/null
@@ -1,509 +0,0 @@
-#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
-#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
-
-//wolf includes
-#include "base/wolf.h"
-#include "cost_function_sparse_base.h"
-
-// CERES JET
-#include "ceres/jet.h"
-
-namespace wolf
-{
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
-class CostFunctionSparse : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
-// if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE > 0)
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ , this->jets_9_ ,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE ==0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_);
-// else
-// assert("Wrong combination of zero sized blocks");
-
- }
-
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_,this->residuals_jet_);
- }
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- 0,
- 0>
-{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_,this->residuals_jet_);
- }
-};
-
-} //namespace wolf
-
-#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_ */
diff --git a/include/base/solver_suitesparse/cost_function_sparse_base.h b/include/base/solver_suitesparse/cost_function_sparse_base.h
deleted file mode 100644
index 9aeff876c9134363b1c95055f4fbef9b1d87a740..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/cost_function_sparse_base.h
+++ /dev/null
@@ -1,325 +0,0 @@
-/*
- * cost_function_sparse.h
- *
- * Created on: Jun 25, 2015
- * Author: jvallve
- */
-
-#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
-#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
-
-//wolf includes
-#include "base/wolf.h"
-#include "cost_function_base.h"
-
-// CERES JET
-#include "ceres/jet.h"
-
-namespace wolf
-{
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE = 0,
- unsigned int BLOCK_2_SIZE = 0,
- unsigned int BLOCK_3_SIZE = 0,
- unsigned int BLOCK_4_SIZE = 0,
- unsigned int BLOCK_5_SIZE = 0,
- unsigned int BLOCK_6_SIZE = 0,
- unsigned int BLOCK_7_SIZE = 0,
- unsigned int BLOCK_8_SIZE = 0,
- unsigned int BLOCK_9_SIZE = 0>
-class CostFunctionSparseBase : CostFunctionBase
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE +
- BLOCK_1_SIZE +
- BLOCK_2_SIZE +
- BLOCK_3_SIZE +
- BLOCK_4_SIZE +
- BLOCK_5_SIZE +
- BLOCK_6_SIZE +
- BLOCK_7_SIZE +
- BLOCK_8_SIZE +
- BLOCK_9_SIZE> WolfJet;
- protected:
- FactorT* factor_ptr_;
- WolfJet jets_0_[BLOCK_0_SIZE];
- WolfJet jets_1_[BLOCK_1_SIZE];
- WolfJet jets_2_[BLOCK_2_SIZE];
- WolfJet jets_3_[BLOCK_3_SIZE];
- WolfJet jets_4_[BLOCK_4_SIZE];
- WolfJet jets_5_[BLOCK_5_SIZE];
- WolfJet jets_6_[BLOCK_6_SIZE];
- WolfJet jets_7_[BLOCK_7_SIZE];
- WolfJet jets_8_[BLOCK_8_SIZE];
- WolfJet jets_9_[BLOCK_9_SIZE];
- WolfJet residuals_jet_[MEASUREMENT_SIZE];
-
- public:
-
- /** \brief Constructor with factor pointer
- *
- * Constructor with factor pointer
- *
- */
- CostFunctionSparseBase(FactorT* _factor_ptr);
-
- /** \brief Default destructor
- *
- * Default destructor
- *
- */
- virtual ~CostFunctionSparseBase();
-
- /** \brief Evaluate residuals and jacobians of the factor in the current x
- *
- * Evaluate residuals and jacobians of the factor in the current x
- *
- */
- virtual void evaluateResidualJacobians();
-
- protected:
-
- /** \brief Calls the functor of the factor evaluating jets
- *
- * Calls the functor of the factor evaluating jets
- *
- */
- virtual void callFunctor() = 0;
-
- /** \brief Initialize the infinitesimal part of jets
- *
- * Initialize the infinitesimal part of jets with zeros and ones
- *
- */
- void initializeJets();
-
- /** \brief Gets the evaluation point
- *
- * Gets the evaluation point from the state
- *
- */
- void evaluateX();
-};
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
-CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::CostFunctionSparseBase(FactorT* _factor_ptr) :
- CostFunctionBase(MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE, BLOCK_5_SIZE, BLOCK_6_SIZE, BLOCK_7_SIZE, BLOCK_8_SIZE,BLOCK_9_SIZE),
- factor_ptr_(_factor_ptr)
-{
- initializeJets();
-}
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
-CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::~CostFunctionSparseBase()
-{
-
-}
-
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
-void CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::evaluateResidualJacobians()
-{
- evaluateX();
-
- callFunctor();
-
- // fill the jacobian matrices
- int jacobian_location = 0;
- for (int i = 0; i<n_blocks_; i++)
- {
- for (int row = 0; row < MEASUREMENT_SIZE; row++)
- jacobians_.at(i)->row(row) = residuals_jet_[row].v.segment(jacobian_location, block_sizes_.at(i));
- jacobian_location += block_sizes_.at(i);
- std::cout << "filled jacobian " << i << ":" << std::endl << (*jacobians_.at(i)) << std::endl;
- }
-
- // fill the residual vector
- for (int i = 0; i < MEASUREMENT_SIZE; i++)
- residual_(i) = residuals_jet_[i].a;
-}
-
-template <class FactorT,
-const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
- void CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::initializeJets()
-{
- int last_jet_idx = 0;
- // JET 0
- for (int i = 0; i < BLOCK_0_SIZE; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- // JET 1
- for (int i = 0; i < BLOCK_1_SIZE; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- // JET 2
- for (int i = 0; i < BLOCK_2_SIZE; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- // JET 3
- for (int i = 0; i < BLOCK_3_SIZE; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- // JET 4
- for (int i = 0; i < BLOCK_4_SIZE; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- // JET 5
- for (int i = 0; i < BLOCK_5_SIZE; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- // JET 6
- for (int i = 0; i < BLOCK_6_SIZE; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- // JET 7
- for (int i = 0; i < BLOCK_7_SIZE; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- // JET 8
- for (int i = 0; i < BLOCK_8_SIZE; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
- // JET 9
- for (int i = 0; i < BLOCK_9_SIZE; i++)
- jets_9_[i] = WolfJet(0, last_jet_idx++);
-}
-
-template <class FactorT,
-const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
- void CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::evaluateX()
-{
- // JET 0
- for (int i = 0; i < BLOCK_0_SIZE; i++)
- jets_0_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(0)+i);
- // JET 1
- for (int i = 0; i < BLOCK_1_SIZE; i++)
- jets_1_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(1)+i);
- // JET 2
- for (int i = 0; i < BLOCK_2_SIZE; i++)
- jets_2_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(2)+i);
- // JET 3
- for (int i = 0; i < BLOCK_3_SIZE; i++)
- jets_3_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(3)+i);
- // JET 4
- for (int i = 0; i < BLOCK_4_SIZE; i++)
- jets_4_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(4)+i);
- // JET 5
- for (int i = 0; i < BLOCK_5_SIZE; i++)
- jets_5_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(5)+i);
- // JET 6
- for (int i = 0; i < BLOCK_6_SIZE; i++)
- jets_6_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(6)+i);
- // JET 7
- for (int i = 0; i < BLOCK_7_SIZE; i++)
- jets_7_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(7)+i);
- // JET 8
- for (int i = 0; i < BLOCK_8_SIZE; i++)
- jets_8_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(8)+i);
- // JET 9
- for (int i = 0; i < BLOCK_9_SIZE; i++)
- jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9)+i);
-}
-
-} // wolf namespace
-
-#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_ */
diff --git a/include/base/solver_suitesparse/qr_solver.h b/include/base/solver_suitesparse/qr_solver.h
deleted file mode 100644
index 154b78639d03c71368b65bbaf65ee778421ba313..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/qr_solver.h
+++ /dev/null
@@ -1,594 +0,0 @@
-/*
- * qr_solver.h
- *
- * Created on: Jul 2, 2015
- * Author: jvallve
- */
-
-#ifndef TRUNK_SRC_SOLVER_QR_SOLVER_H_
-#define TRUNK_SRC_SOLVER_QR_SOLVER_H_
-
-//std includes
-#include <iostream>
-#include <ctime>
-
-//Wolf includes
-#include "base/state_block.h"
-#include "../factor_sparse.h"
-#include "base/factor/factor_odom_2D.h"
-#include "base/factor/factor_corner_2D.h"
-#include "base/factor/factor_container.h"
-#include "base/solver_suitesparse/sparse_utils.h"
-
-// wolf solver
-#include "solver/ccolamd_ordering.h"
-#include "solver/cost_function_sparse.h"
-
-// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
-#include <Eigen/StdVector>
-#include "base/factor/factor_pose_2D.h"
-
-namespace wolf
-{
-
-class SolverQR
-{
- protected:
- ProblemPtr problem_ptr_;
- Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> solver_;
- Eigen::SparseMatrix<double> A_, R_;
- Eigen::VectorXd b_, x_incr_;
- std::vector<StateBlockPtr> nodes_;
- std::vector<FactorBasePtr> factors_;
- std::vector<CostFunctionBasePtr> cost_functions_;
-
- // ordering
- Eigen::SparseMatrix<int> A_nodes_;
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> acc_node_permutation_;
- std::map<double*, unsigned int> id_2_idx_;
- Eigen::CCOLAMDOrdering<int> orderer_;
- Eigen::VectorXi node_ordering_restrictions_;
- Eigen::ArrayXi node_locations_;
- std::vector<unsigned int> factor_locations_;
- unsigned int n_new_factors_;
-
- // time
- clock_t t_ordering_, t_solving_, t_managing_;
- double time_ordering_, time_solving_, time_managing_;
-
- public:
- SolverQR(ProblemPtr problem_ptr_) :
- problem_ptr_(problem_ptr_), A_(0, 0), R_(0, 0), A_nodes_(0, 0), acc_node_permutation_(0), n_new_factors_(
- 0), time_ordering_(0), time_solving_(0), time_managing_(0)
- {
- node_locations_.resize(0);
- factor_locations_.resize(0);
- }
-
- virtual ~SolverQR()
- {
-
- }
-
- void update()
- {
- // UPDATE STATE BLOCKS
- while (!problem_ptr_->getStateBlockNotificationList().empty())
- {
- switch (problem_ptr_->getStateBlockNotificationList().front().notification_)
- {
- case ADD:
- {
- addStateBlock(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- case UPDATE:
- {
- updateStateBlockStatus(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- case REMOVE:
- {
- // TODO removeStateBlock((double *)(problem_ptr_->getStateBlockNotificationList().front().scalar_ptr_));
- break;
- }
- default:
- throw std::runtime_error("SolverQR::update: State Block notification must be ADD, UPATE or REMOVE.");
- }
- problem_ptr_->getStateBlockNotificationList().pop_front();
- }
- // UPDATE FACTORS
- while (!problem_ptr_->getFactorNotificationList().empty())
- {
- switch (problem_ptr_->getFactorNotificationList().front().notification_)
- {
- case ADD:
- {
- addFactor(problem_ptr_->getFactorNotificationList().front().factor_ptr_);
- break;
- }
- case REMOVE:
- {
- // TODO: removeFactor(problem_ptr_->getFactorNotificationList().front().id_);
- break;
- }
- default:
- throw std::runtime_error("SolverQR::update: Factor notification must be ADD or REMOVE.");
- }
- problem_ptr_->getFactorNotificationList().pop_front();
- }
- }
-
- void addStateBlock(StateBlockPtr _state_ptr)
- {
- t_managing_ = clock();
-
- std::cout << "adding state unit " << _state_ptr->get() << std::endl;
- if (!_state_ptr->isFixed())
- {
- nodes_.push_back(_state_ptr);
- id_2_idx_[_state_ptr->get()] = nodes_.size() - 1;
-
- std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl;
-
- // Resize accumulated permutations
- augmentPermutation(acc_node_permutation_, nNodes());
-
- // Resize state
- x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getSize());
-
- // Resize problem
- A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getSize());
- R_.conservativeResize(R_.cols() + _state_ptr->getSize(), R_.cols() + _state_ptr->getSize());
-
- }
- time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
- }
-
- void updateStateBlockStatus(StateBlockPtr _state_ptr)
- {
- //TODO
- }
-
- void addFactor(FactorBasePtr _factor_ptr)
- {
- std::cout << "adding factor " << _factor_ptr->id() << std::endl;
- t_managing_ = clock();
-
- factors_.push_back(_factor_ptr);
- cost_functions_.push_back(createCostFunction(_factor_ptr));
-
- unsigned int meas_dim = _factor_ptr->getSize();
-
- std::vector<Eigen::MatrixXs> jacobians(_factor_ptr->getStateBlockPtrVector().size());
- Eigen::VectorXs error(meas_dim);
-
- cost_functions_.back()->evaluateResidualJacobians();
- cost_functions_.back()->getResidual(error);
- cost_functions_.back()->getJacobians(jacobians);
-
- std::vector<unsigned int> idxs;
- for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++)
- if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed())
- idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]);
-
- n_new_factors_++;
- factor_locations_.push_back(A_.rows());
-
- // Resize problem
- A_.conservativeResize(A_.rows() + meas_dim, A_.cols());
- b_.conservativeResize(b_.size() + meas_dim);
- A_nodes_.conservativeResize(factors_.size(), nNodes());
-
- // ADD MEASUREMENTS
- for (unsigned int j = 0; j < idxs.size(); j++)
- {
- assert((unsigned int )(acc_node_permutation_.indices()(idxs.at(j))) == nodeOrder(idxs.at(j)));
- assert(jacobians.at(j).cols() == nodeDim(idxs.at(j)));
- assert(jacobians.at(j).rows() == meas_dim);
-
- addSparseBlock(jacobians.at(j), A_, A_.rows() - meas_dim, nodeLocation(idxs.at(j)));
-
- A_nodes_.coeffRef(A_nodes_.rows() - 1, nodeOrder(idxs.at(j))) = 1;
- }
-
- // error
- b_.tail(meas_dim) = error;
-
- time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
- }
-
- void ordering(const int & _first_ordered_idx)
- {
- std::cout << "ordering from idx " << _first_ordered_idx << std::endl;
- t_ordering_ = clock();
-
- // full problem ordering
- if (_first_ordered_idx == -1)
- {
- // ordering ordering factors
- node_ordering_restrictions_.resize(nNodes());
- node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
-
- // computing nodes partial ordering_
- A_nodes_.makeCompressed();
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(nNodes());
- orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
-
- // node ordering to variable ordering
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation(A_.cols());
- nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
-
- // apply partial_ordering orderings
- A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
- A_ = (A_ * incr_permutation.transpose()).sparseView();
-
- // ACCUMULATING PERMUTATIONS
- accumulatePermutation(incr_permutation_nodes);
- }
-
- // partial ordering
- else
- {
- unsigned int first_ordered_node = nodeOrder(_first_ordered_idx); //nodes_.at(_first_ordered_idx).order;
- unsigned int ordered_nodes = nNodes() - first_ordered_node;
-
- if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
- {
- // SUBPROBLEM ORDERING (from first node variable to last one)
- //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
- Eigen::SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
-
- // _partial_ordering ordering_ factors
- node_ordering_restrictions_.resize(ordered_nodes);
- node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
-
- // computing nodes partial ordering_
- sub_A_nodes_.makeCompressed();
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation_nodes(ordered_nodes);
- orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
-
- // node ordering to variable ordering
- unsigned int ordered_variables = A_.cols() - nodeLocation(_first_ordered_idx); //nodes_.at(_first_ordered_idx).location;
-// std::cout << "first_ordered_node " << first_ordered_node << std::endl;
-// std::cout << "A_.cols() " << A_.cols() << std::endl;
-// std::cout << "nodes_.at(_first_ordered_idx).location " << nodes_.at(_first_ordered_idx).location << std::endl;
-// std::cout << "ordered_variables " << ordered_variables << std::endl;
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation(ordered_variables);
- nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
-
- // apply partial_ordering orderings
- A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes)
- * partial_permutation_nodes.transpose()).sparseView();
- A_.rightCols(ordered_variables) =
- (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
- R_.rightCols(ordered_variables) =
- (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
-
- // ACCUMULATING PERMUTATIONS
- accumulatePermutation(partial_permutation_nodes);
- }
- }
- time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC;
- }
-
- unsigned int findFirstOrderedNode()
- {
- unsigned int first_ordered_node = nNodes();
- unsigned int first_ordered_idx;
- for (unsigned int i = 0; i < n_new_factors_; i++)
- {
- FactorBasePtr ct_ptr = factors_.at(factors_.size() - 1 - i);
- std::cout << "factor: " << i << " id: " << factors_.at(factors_.size() - 1 - i)->id()
- << std::endl;
- for (unsigned int j = 0; j < ct_ptr->getStateBlockPtrVector().size(); j++)
- {
- if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed())
- {
- unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()];
- //std::cout << "estimated idx " << idx << std::endl;
- //std::cout << "node_order(idx) " << node_order(idx) << std::endl;
- //std::cout << "first_ordered_node " << first_ordered_node << std::endl;
- if (first_ordered_node > nodeOrder(idx)) //nodes_.at(idx).order)
- {
- first_ordered_idx = idx;
- //std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
- first_ordered_node = nodeOrder(idx);
- //std::cout << "first_ordered_node " << first_ordered_node << std::endl;
- }
- }
- }
- }
- //std::cout << "found first_ordered_node " << first_ordered_node << std::endl;
- //std::cout << "found first_ordered_idx " << first_ordered_idx << std::endl;
-
- return first_ordered_idx;
- }
-
- bool solve(const unsigned int mode)
- {
- if (n_new_factors_ == 0)
- return 1;
-
- std::cout << "solving mode " << mode << std::endl;
-
- bool batch, order;
- unsigned int first_ordered_idx;
-
- switch (mode)
- {
- case 0:
- {
- batch = true;
- order = false;
- break;
- }
- case 1:
- {
- batch = true;
- order = (nNodes() > 1);
- break;
- }
- case 2:
- {
- first_ordered_idx = findFirstOrderedNode();
- batch = (nodeOrder(first_ordered_idx) == 0);
- order = (nNodes() > 1);
- }
- }
-
- // BATCH
- if (batch)
- {
- // REORDER
- if (order)
- ordering(-1);
-
- //printProblem();
-
- // SOLVE
- t_solving_ = clock();
- A_.makeCompressed();
- solver_.compute(A_);
- if (solver_.info() != Eigen::Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_ = solver_.solve(-b_);
- R_ = solver_.matrixR();
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
- time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
- }
- // INCREMENTAL
- else
- {
- // REORDER SUBPROBLEM
- ordering(first_ordered_idx);
- //printProblem();
-
- // SOLVE ORDERED SUBPROBLEM
- t_solving_ = clock();
- A_nodes_.makeCompressed();
- A_.makeCompressed();
-
- // finding measurements block
- Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx));
- //std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- //std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
- unsigned int first_ordered_measurement =
- measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
- unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement);
- unsigned int ordered_variables = A_.cols() - nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
- unsigned int unordered_variables = nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
-
- Eigen::SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
- solver_.compute(A_partial);
- if (solver_.info() != Eigen::Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
- x_incr_.tail(ordered_variables) = solver_.solve(-b_.tail(ordered_measurements));
-
- // store new part of R
- eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- R_.makeCompressed();
-
- // solving not ordered subproblem
- if (unordered_variables > 0)
- {
- //std::cout << "--------------------- solving unordered part" << std::endl;
- Eigen::SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
- //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
- Eigen::SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
- //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
- solver_.compute(R1);
- if (solver_.info() != Eigen::Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_.head(unordered_variables) = solver_.solve(
- -b_.head(unordered_variables) + R2 * x_incr_.tail(ordered_variables));
- }
-
- }
- // UPDATE X VALUE
- for (unsigned int i = 0; i < nodes_.size(); i++)
- {
- Eigen::Map < Eigen::VectorXs > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
- x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
- }
- // Zero the error
- b_.setZero();
-
- time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
- n_new_factors_ = 0;
- return 1;
- }
-
- void nodePermutation2VariablesPermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm_variables)
- {
- //std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl;
- nodePermutation2nodeLocations(_perm_nodes, node_locations_);
- //std::cout << "locations: " << locations.transpose() << std::endl;
- //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
-
- unsigned int last_idx = 0;
- for (unsigned int i = 0; i < node_locations_.size(); i++)
- {
- perm_variables.indices().segment(last_idx, nodeDim(i)) = Eigen::VectorXi::LinSpaced(
- nodeDim(i), node_locations_(i), node_locations_(i) + nodeDim(i) - 1);
- last_idx += nodeDim(i);
- //std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl;
- }
- //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
- }
-
- void nodePermutation2nodeLocations(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
- Eigen::ArrayXi& locations)
- {
- locations = _perm_nodes.indices().array();
-
- for (unsigned int i = 0; i < locations.size(); i++)
- locations = (locations > locations(i)).select(locations + nodeDim(i) - 1, locations);
- }
-
- void augmentPermutation(Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm, unsigned int new_size)
- {
- unsigned int old_size = perm.indices().size();
- unsigned int dim = new_size - old_size;
-
- Eigen::VectorXi new_indices(new_size);
- new_indices.head(old_size) = perm.indices();
- new_indices.tail(dim) = Eigen::ArrayXi::LinSpaced(dim, old_size, new_size - 1);
- perm.resize(new_size);
- perm.indices() = new_indices;
- std::cout << "permutation augmented" << std::endl;
-
- // resize and update locations
- node_locations_.conservativeResize(node_locations_.size() + 1);
- node_locations_(node_locations_.size() - 1) = x_incr_.size();
- std::cout << "node_locations_ augmented" << std::endl;
- }
-
- void accumulatePermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm)
- {
- //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
- //std::cout << "incr perm " << perm.indices().transpose() << std::endl;
-
- // acumulate permutation
- if (perm.size() == acc_node_permutation_.size()) //full permutation
- acc_node_permutation_ = perm * acc_node_permutation_;
- else //partial permutation
- {
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(
- Eigen::VectorXi::LinSpaced(nNodes(), 0, nNodes() - 1)); // identity permutation
- incr_permutation_nodes.indices().tail(perm.size()) = perm.indices()
- + Eigen::VectorXi::Constant(perm.size(), nNodes() - perm.size());
- //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
- acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
- }
- //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
-
- // update nodes orders and locations
- nodePermutation2nodeLocations(acc_node_permutation_, node_locations_);
- }
-
- unsigned int nodeDim(const unsigned int _idx)
- {
- assert(_idx < nNodes());
- return nodes_.at(_idx)->getSize();
- }
-
- unsigned int nodeOrder(const unsigned int _idx)
- {
- assert(_idx < nNodes());
- assert(_idx < acc_node_permutation_.indices().size());
- return acc_node_permutation_.indices()(_idx);
- }
-
- unsigned int nodeLocation(const unsigned int _idx)
- {
- assert(_idx < nNodes());
- assert(_idx < node_locations_.size());
- return node_locations_(_idx);
- }
-
- unsigned int nNodes()
- {
- return nodes_.size();
- }
-
- CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr)
- {
- //std::cout << "adding fac " << _fac_ptr->id() << std::endl;
- //_fac_ptr->print();
-
- switch (_fac_ptr->getTypeId())
- {
- case FAC_GPS_FIX_2D:
- {
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_fac_ptr);
- return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2D, specific_ptr->residualSize,
- specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
- specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
- specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr));
- break;
- }
- case FAC_ODOM_2D:
- {
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2D, specific_ptr->residualSize,
- specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
- specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
- specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_CORNER_2D:
- {
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2D, specific_ptr->residualSize,
- specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
- specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
- specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()"
- << std::endl;
-
- return nullptr;
- }
- }
-
- void printResults()
- {
- std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"
- << std::endl;
- std::cout << "x = " << x_incr_.transpose() << std::endl;
- }
-
- void printProblem()
- {
- std::cout << std::endl << "A_nodes_: " << std::endl
- << Eigen::MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
- //std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
- std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
- }
-};
-
-} // namespace wolf
-
-#endif /* TRUNK_SRC_SOLVER_QR_SOLVER_H_ */
diff --git a/include/base/solver_suitesparse/solver_QR.h b/include/base/solver_suitesparse/solver_QR.h
deleted file mode 100644
index e625515d2fcba147bb6cda32e1697bf893010551..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/solver_QR.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * solver_QR.h
- *
- * Created on: Jun 22, 2015
- * Author: jvallve
- */
-
-#ifndef TRUNK_SRC_SOLVER_SOLVER_QR_H_
-#define TRUNK_SRC_SOLVER_SOLVER_QR_H_
-
-using namespace Eigen;
-
-class SolverQR
-{
- protected:
- SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_;
- SparseMatrix<double> A, A_ordered, R;
- VectorXd b, x, x_ordered, x_ordered_partial;
- int n_measurements = 0;
- int n_nodes = 0;
-
- // ordering variables
- SparseMatrix<int> A_nodes_ordered;
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix;
-
- CCOLAMDOrdering<int> ordering, partial_ordering;
- VectorXi nodes_ordering_factors;
-
- private:
-};
-
-#endif /* TRUNK_SRC_SOLVER_SOLVER_QR_H_ */
diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h
deleted file mode 100644
index 09c0abe7f8a83cde800e76968da2a5a990e0007c..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/solver_manager.h
+++ /dev/null
@@ -1,48 +0,0 @@
-#ifndef CERES_MANAGER_H_
-#define CERES_MANAGER_H_
-
-//wolf includes
-#include "base/factor/factor_GPS_2D.h"
-#include "base/wolf.h"
-#include "base/state_block.h"
-#include "../state_point.h"
-#include "../state_complex_angle.h"
-#include "../state_theta.h"
-#include "../factor_sparse.h"
-#include "../factor_odom_2D_theta.h"
-#include "../factor_odom_2D_complex_angle.h"
-#include "../factor_corner_2D_theta.h"
-
-/** \brief solver manager for WOLF
- *
- */
-
-class SolverManager
-{
- protected:
-
- public:
- SolverManager(ceres::Problem::Options _options);
-
- ~SolverManager();
-
- ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
-
- //void computeCovariances(WolfProblemPtr _problem_ptr);
-
- void update(const WolfProblemPtr _problem_ptr);
-
- void addFactor(FactorBasePtr _fac_ptr);
-
- void removeFactor(const unsigned int& _fac_idx);
-
- void addStateUnit(StateBlockPtr _st_ptr);
-
- void removeAllStateUnits();
-
- void updateStateUnitStatus(StateBlockPtr _st_ptr);
-
- ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr);
-};
-
-#endif
diff --git a/include/base/solver_suitesparse/sparse_utils.h b/include/base/solver_suitesparse/sparse_utils.h
deleted file mode 100644
index 280aa2310c648ca360a491de6be6784826404f8a..0000000000000000000000000000000000000000
--- a/include/base/solver_suitesparse/sparse_utils.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * sparse_utils.h
- *
- * Created on: Jul 2, 2015
- * Author: jvallve
- */
-
-#ifndef SPARSE_UTILS_H_
-#define SPARSE_UTILS_H_
-
-// eigen includes
-#include <eigen3/Eigen/Sparse>
-
-namespace wolf
-{
-
-void eraseBlockRow(Eigen::SparseMatrixs& A, const unsigned int& _row, const unsigned int& _n_rows)
-{
- A.prune([](int i, int, Scalar) { return i >= _row && i < _row + _n_rows; });
-}
-
-void eraseBlockCol(Eigen::SparseMatrixs& A, const unsigned int& _col, const unsigned int& _n_cols)
-{
- A.prune([](int, int j, Scalar) { return j >= _col && j < _col + _n_cols; });
-}
-
-void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original, const unsigned int& row, const unsigned int& col)
-{
- for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
- for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col);
-
- original.makeCompressed();
-}
-
-void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original, const unsigned int& row, const unsigned int& col)
-{
- for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
- for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
-
- original.makeCompressed();
-}
-
-}
-#endif /* SPARSE_UTILS_H_ */
diff --git a/include/base/state_angle.h b/include/base/state_angle.h
deleted file mode 100644
index c61286e13eb4fcaa82c70c1c8db8784decfb1b86..0000000000000000000000000000000000000000
--- a/include/base/state_angle.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/**
- * \file state_angle.h
- *
- * Created on: Apr 4, 2017
- * \author: jsola
- */
-
-#ifndef STATE_ANGLE_H_
-#define STATE_ANGLE_H_
-
-#include "base/state_block.h"
-#include "base/local_parametrization_angle.h"
-
-namespace wolf
-{
-
-class StateAngle : public StateBlock
-{
- public:
- StateAngle(Scalar _angle = 0.0, bool _fixed = false);
-
- virtual ~StateAngle();
-};
-
-inline StateAngle::StateAngle(Scalar _angle, bool _fixed) :
- StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>())
-{
- state_(0) = _angle;
-}
-
-inline StateAngle::~StateAngle()
-{
- //
-}
-
-} /* namespace wolf */
-
-#endif /* STATE_ANGLE_H_ */
diff --git a/include/base/state_block.h b/include/base/state_block.h
deleted file mode 100644
index 447e8a0e9b3534f8048f3aa55131bdaed617e0d7..0000000000000000000000000000000000000000
--- a/include/base/state_block.h
+++ /dev/null
@@ -1,284 +0,0 @@
-
-#ifndef STATE_BLOCK_H_
-#define STATE_BLOCK_H_
-
-// Fwd references
-namespace wolf{
-class NodeBase;
-class LocalParametrizationBase;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-
-//std includes
-#include <iostream>
-#include <mutex>
-
-namespace wolf {
-
-/** \brief class StateBlock
- *
- * This class implements a state block for general nonlinear optimization. It offers the following functionality:
- * - A state vector storing the state values.
- * - A key to indicate whether the state is fixed or not.
- * - Fixed state blocks are not estimated and treated by the estimator as fixed parameters.
- * - Non-fixed state blocks are estimated.
- * - A local parametrization useful for optimizing in the tangent space to the manifold.
- */
-class StateBlock : public std::enable_shared_from_this<StateBlock>
-{
-public:
-
- protected:
-
- std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
-
- std::atomic<SizeEigen> state_size_; ///< State vector size
- Eigen::VectorXs state_; ///< State vector storing the state values
- mutable std::mutex mut_state_; ///< State vector mutex
-
- LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
-
- std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not
- 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
-
- public:
- /** \brief Constructor from size
- *
- * \param _size is state size
- * \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);
-
- /** \brief Constructor from vector
- *
- * \param _state is state vector
- * \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::VectorXs& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
-
- ///< Explicitly not copyable/movable
- StateBlock(const StateBlock& o) = delete;
- StateBlock(StateBlock&& o) = delete;
- StateBlock& operator=(const StateBlock& o) = delete;
-
- /** \brief Destructor
- **/
- virtual ~StateBlock();
-
- /** \brief Returns the state vector
- **/
- Eigen::VectorXs getState() const;
-
- /** \brief Sets the state vector
- **/
- void setState(const Eigen::VectorXs& _state, const bool _notify = true);
-
- /** \brief Returns the state size
- **/
- SizeEigen getSize() const;
-
- /**\brief Returns the size of the local parametrization
- */
- SizeEigen getLocalSize() const;
-
- /** \brief Returns if the state is fixed (not estimated)
- **/
- bool isFixed() const;
-
- /** \brief Sets the state as fixed
- **/
- void fix();
-
- /** \brief Sets the state as estimated
- **/
- void unfix();
-
- /** \brief Sets the state status
- **/
- void setFixed(bool _fixed);
-
- /** \brief Returns if the state has a local parametrization
- **/
- bool hasLocalParametrization() const;
-
- /** \brief Returns the state local parametrization ptr
- **/
- LocalParametrizationBasePtr getLocalParametrization() const;
-
- /** \brief Sets a local parametrization
- **/
- void setLocalParametrization(LocalParametrizationBasePtr _local_param);
-
- /** \brief Removes the state_block local parametrization
- **/
- void removeLocalParametrization();
-
- /** \brief Return if state has been updated
- **/
- bool stateUpdated() const;
-
- /** \brief Return if fix/unfix has been updated
- **/
- bool fixUpdated() const;
-
- /** \brief Return if local parameterization has been updated
- **/
- bool localParamUpdated() const;
-
- /** \brief Set state_updated_ to false
- **/
- void resetStateUpdated();
-
- /** \brief Set fix_updated_ to false
- **/
- void resetFixUpdated();
-
- /** \brief Set localk_param_updated_ to false
- **/
- void resetLocalParamUpdated();
-
- /** \brief Add this state_block to the problem
- **/
- //void addToProblem(ProblemPtr _problem_ptr);
-
- /** \brief Remove this state_block from the problem
- **/
- //void removeFromProblem(ProblemPtr _problem_ptr);
-};
-
-} // namespace wolf
-
-// IMPLEMENTATION
-#include "base/local_parametrization_base.h"
-#include "base/node_base.h"
-#include "base/problem.h"
-
-namespace wolf {
-
-inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-// notifications_{Notification::ADD},
- fixed_(_fixed),
- state_size_(_state.size()),
- state_(_state),
- local_param_ptr_(_local_param_ptr),
- state_updated_(false),
- fix_updated_(false),
- local_param_updated_(false)
-{
-// std::cout << "constructed +sb" << std::endl;
-}
-
-inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-// notifications_{Notification::ADD},
- fixed_(_fixed),
- state_size_(_size),
- state_(Eigen::VectorXs::Zero(_size)),
- local_param_ptr_(_local_param_ptr),
- state_updated_(false),
- fix_updated_(false),
- local_param_updated_(false)
-{
- //
-// std::cout << "constructed +sb" << std::endl;
-}
-
-inline StateBlock::~StateBlock()
-{
-// std::cout << "destructed -sb" << std::endl;
-}
-
-inline Eigen::VectorXs StateBlock::getState() const
-{
- std::lock_guard<std::mutex> lock(mut_state_);
- return state_;
-}
-
-inline SizeEigen StateBlock::getSize() const
-{
- return state_size_.load();
-}
-
-inline SizeEigen StateBlock::getLocalSize() const
-{
- if(local_param_ptr_)
- return local_param_ptr_->getLocalSize();
- return getSize();
-}
-
-inline bool StateBlock::isFixed() const
-{
- return fixed_.load();
-}
-
-inline void StateBlock::fix()
-{
- setFixed(true);
-}
-
-inline void StateBlock::unfix()
-{
- setFixed(false);
-}
-
-inline bool StateBlock::hasLocalParametrization() const
-{
- return (local_param_ptr_ != nullptr);
-}
-
-inline LocalParametrizationBasePtr StateBlock::getLocalParametrization() const
-{
- return local_param_ptr_;
-}
-
-inline void StateBlock::removeLocalParametrization()
-{
- assert(local_param_ptr_ != nullptr && "state block without local parametrization");
- local_param_ptr_.reset();
- local_param_updated_.store(true);
-}
-
-inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param)
-{
- assert(_local_param != nullptr && "setting a null local parametrization");
- local_param_ptr_ = _local_param;
- local_param_updated_.store(true);
-}
-
-inline bool StateBlock::stateUpdated() const
-{
- return state_updated_.load();
-}
-
-inline bool StateBlock::fixUpdated() const
-{
- return fix_updated_.load();
-}
-
-inline bool StateBlock::localParamUpdated() const
-{
- return local_param_updated_.load();
-}
-
-inline void StateBlock::resetStateUpdated()
-{
- state_updated_.store(false);
-}
-
-inline void StateBlock::resetFixUpdated()
-{
- fix_updated_.store(false);
-}
-
-inline void StateBlock::resetLocalParamUpdated()
-{
- local_param_updated_.store(false);
-}
-
-}// namespace wolf
-
-#endif
diff --git a/include/base/state_homogeneous_3D.h b/include/base/state_homogeneous_3D.h
deleted file mode 100644
index adfda9018041d3a9d2469dc0aa16ea7e93ccdfe0..0000000000000000000000000000000000000000
--- a/include/base/state_homogeneous_3D.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * \file state_homogeneous_3D.h
- *
- * Created on: Mar 7, 2016
- * \author: jsola
- */
-
-#ifndef SRC_STATE_HOMOGENEOUS_3D_H_
-#define SRC_STATE_HOMOGENEOUS_3D_H_
-
-#include "base/state_block.h"
-#include "base/local_parametrization_homogeneous.h"
-
-namespace wolf {
-
-class StateHomogeneous3D : public StateBlock
-{
- public:
- StateHomogeneous3D(bool _fixed = false);
- StateHomogeneous3D(const Eigen::VectorXs _state, bool _fixed = false);
- virtual ~StateHomogeneous3D();
-};
-
-inline StateHomogeneous3D::StateHomogeneous3D(const Eigen::VectorXs _state, bool _fixed) :
- StateBlock(_state, _fixed)
-{
- 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)
-{
- local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
- state_ << 0, 0, 0, 1; // Set origin
-}
-
-inline StateHomogeneous3D::~StateHomogeneous3D()
-{
- // The local_param_ptr_ pointer is already removed by the base class
-}
-
-} // namespace wolf
-
-#endif /* SRC_STATE_HOMOGENEOUS_3D_H_ */
diff --git a/include/base/state_quaternion.h b/include/base/state_quaternion.h
deleted file mode 100644
index d990ce1f8c6ed89c3150e4958d7ad570b1e2f606..0000000000000000000000000000000000000000
--- a/include/base/state_quaternion.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * \file StateQuaternion.h
- *
- * Created on: Mar 7, 2016
- * author: jsola
- */
-
-#ifndef SRC_STATE_QUATERNION_H_
-#define SRC_STATE_QUATERNION_H_
-
-#include "base/state_block.h"
-#include "base/local_parametrization_quaternion.h"
-
-namespace wolf {
-
-class StateQuaternion : public StateBlock
-{
- public:
- StateQuaternion(bool _fixed = false);
- StateQuaternion(const Eigen::VectorXs& _state, bool _fixed = false);
- StateQuaternion(const Eigen::Quaternions& _quaternion, bool _fixed = false);
- virtual ~StateQuaternion();
-};
-
-inline StateQuaternion::StateQuaternion(const Eigen::Quaternions& _quaternion, bool _fixed) :
- StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
-{
-}
-
-inline StateQuaternion::StateQuaternion(const Eigen::VectorXs& _state, bool _fixed) :
- StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
-{
- assert(_state.size() == 4 && "The quaternion state vector must be of size 4");
-}
-
-inline StateQuaternion::StateQuaternion(bool _fixed) :
- StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
-{
- state_ = Eigen::Quaternions::Identity().coeffs();
- //
-}
-
-inline StateQuaternion::~StateQuaternion()
-{
- // The local_param_ptr_ pointer is already removed by the base class
-}
-
-} // namespace wolf
-
-#endif /* SRC_STATE_QUATERNION_H_ */
diff --git a/include/base/time_stamp.h b/include/base/time_stamp.h
deleted file mode 100644
index 02a5599af607af39d4b16bb3c05a35779cc7bb04..0000000000000000000000000000000000000000
--- a/include/base/time_stamp.h
+++ /dev/null
@@ -1,261 +0,0 @@
-
-#ifndef TIME_STAMP_H_
-#define TIME_STAMP_H_
-
-//wolf includes
-#include "base/wolf.h"
-
-//C, std
-#include <sys/time.h>
-#include <iostream>
-#include <iomanip>
-
-static const unsigned int NANOSECS = 1000000000;
-
-namespace wolf {
-
-/**
- * \brief TimeStamp implements basic functionalities for time stamps
- *
- * TimeStamp implements basic functionalities for time stamps
- */
-class TimeStamp
-{
- protected:
- unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
-
- public:
- /** \brief Constructor
- *
- * Constructor without arguments. Sets time stamp to now.
- *
- */
- TimeStamp();
-
- /** \brief Copy constructor
- *
- * Copy constructor
- *
- */
- TimeStamp(const TimeStamp& _ts);
-
- /** \brief Constructor with argument
- *
- * Constructor with arguments
- *
- */
- TimeStamp(const Scalar& _ts);
-
- /** \brief Constructor from sec and nsec
- *
- * Constructor from sec and nsec
- *
- */
- TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec);
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- ~TimeStamp();
-
- /** \brief Time stamp to now
- */
- void setToNow();
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a timeval struct
- */
- void set(const timeval& ts);
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds)
- *
- */
- void set(const unsigned long int& sec, const unsigned long int& nanosec);
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a scalar_t (seconds)
- *
- */
- void set(const Scalar& ts);
-
- /** \brief Get time stamp
- *
- * Returns time stamp
- *
- */
- Scalar get() const;
-
- /** \brief Get time stamp (only seconds)
- *
- * Returns seconds of time stamp
- *
- */
- unsigned long int getSeconds() const;
-
- /** \brief Get time stamp (only nano seconds)
- *
- * Returns nanoseconds part of time stamp
- *
- */
- unsigned long int getNanoSeconds() const;
-
- /** \brief Assignement operator
- *
- * Assignement operator
- *
- */
- void operator =(const Scalar& ts);
-
- /** \brief Assignement operator
- *
- * Assignement operator given a scalar_t (seconds)
- *
- */
- void operator =(const TimeStamp& ts);
-
- /** \brief comparison operator
- *
- * Comparison operator
- *
- */
- bool operator !=(const TimeStamp& ts) const;
- bool operator ==(const TimeStamp& ts) const;
- bool operator <(const TimeStamp& ts) const;
- bool operator >(const TimeStamp& ts) const;
-
- /** \brief comparison operator
- *
- * Comparison operator
- *
- */
- bool operator <=(const TimeStamp& ts) const;
- bool operator >=(const TimeStamp& ts) const;
-
- /** \brief Add-assign operator given a scalar_t (seconds)
- */
- void operator +=(const Scalar& dt);
-
- /** \brief Add-assign operator given a scalar_t (seconds)
- */
- TimeStamp operator +(const Scalar& dt) const;
-
- /** \brief Substraction-assign operator given a scalar_t (seconds)
- */
- void operator -=(const Scalar& dt);
-
- /** \brief difference operator
- *
- * difference operator that returns a scalar_t (seconds)
- *
- */
- TimeStamp operator -(const Scalar& ts) const;
-
- /** \brief difference operator
- *
- * difference operator that returns a Timestamp (seconds)
- *
- */
- Scalar operator -(const TimeStamp& ts) const;
-
- /** \brief Prints time stamp to a given ostream
- *
- * Prints time stamp to a given ostream
- *
- */
- void print(std::ostream & ost = std::cout) const;
-
- friend std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts);
-
-};
-
-inline void TimeStamp::set(const Scalar& ts)
-{
- time_stamp_nano_ = (ts > 0 ? (unsigned long int)(ts*NANOSECS) : 0);
-}
-
-inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec)
-{
- time_stamp_nano_ = sec*NANOSECS+nanosec;
-}
-
-inline void TimeStamp::set(const timeval& ts)
-{
- time_stamp_nano_ = (unsigned long int)(ts.tv_sec*NANOSECS) + (unsigned long int)(ts.tv_usec*1000);
-}
-
-inline Scalar TimeStamp::get() const
-{
- return ((Scalar)( time_stamp_nano_))*1e-9;
-}
-
-inline unsigned long int TimeStamp::getSeconds() const
-{
- return time_stamp_nano_ / NANOSECS;
-}
-
-inline unsigned long int TimeStamp::getNanoSeconds() const
-{
- return time_stamp_nano_ % NANOSECS;
-}
-
-inline void TimeStamp::operator =(const TimeStamp& ts)
-{
- time_stamp_nano_ = ts.time_stamp_nano_;
-}
-
-inline void TimeStamp::operator =(const Scalar& ts)
-{
- time_stamp_nano_ = (unsigned long int)(ts*NANOSECS);
-}
-
-inline bool TimeStamp::operator ==(const TimeStamp& ts) const
-{
- return (time_stamp_nano_ == ts.time_stamp_nano_);
-}
-
-inline bool TimeStamp::operator !=(const TimeStamp& ts) const
-{
- return (time_stamp_nano_ != ts.time_stamp_nano_);
-}
-
-inline bool TimeStamp::operator <(const TimeStamp& ts) const
-{
- return (time_stamp_nano_ < ts.time_stamp_nano_);
-}
-
-inline bool TimeStamp::operator >(const TimeStamp& ts) const
-{
- return (time_stamp_nano_ > ts.time_stamp_nano_);
-}
-
-inline bool TimeStamp::operator <=(const TimeStamp& ts) const
-{
- return (time_stamp_nano_ <= ts.time_stamp_nano_);
-}
-
-inline bool TimeStamp::operator >=(const TimeStamp& ts) const
-{
- return (time_stamp_nano_ >= ts.time_stamp_nano_);
-}
-
-inline void TimeStamp::operator +=(const Scalar& dt)
-{
- time_stamp_nano_ += (unsigned long int)(dt*NANOSECS);
-}
-
-inline Scalar TimeStamp::operator -(const TimeStamp& ts) const
-{
- return Scalar((long int)(time_stamp_nano_ - ts.time_stamp_nano_))*1e-9; // long int cast fix overflow in case of negative substraction result
-}
-
-static const TimeStamp InvalidStamp(-1,-1);
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/track_matrix.h b/include/base/track_matrix.h
deleted file mode 100644
index aaa7d7681048cddd53b6b5568605de35d1a29b0f..0000000000000000000000000000000000000000
--- a/include/base/track_matrix.h
+++ /dev/null
@@ -1,128 +0,0 @@
-/**
- * \file track_matrix.h
- *
- * Created on: Mar 24, 2018
- * \author: jsola
- */
-
-#ifndef TRACK_MATRIX_H_
-#define TRACK_MATRIX_H_
-
-#include "base/feature/feature_base.h"
-#include "base/capture/capture_base.h"
-
-#include <map>
-#include <vector>
-#include <list>
-#include <utility>
-
-namespace wolf
-{
-using std::map;
-using std::vector;
-using std::list;
-using std::pair;
-using std::shared_ptr;
-
-typedef map<TimeStamp, FeatureBasePtr> Track;
-typedef map<size_t, FeatureBasePtr > Snapshot;
-typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
-
-/** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
- * This class implements the following data structure:
- *
- * Snapshots at each capture Cx:
- * C1 C2 C3 C4 C5
- * Tracks for each matched feature:
- * f1 --- f3 --- f7 --- f11 -- f14 <-- track 1, of corresponding features in different captures
- * | | | | |
- * | f4 --- f8 --- f12 -- f15 <-- track 2
- * | | | |
- * | f5 --- f9 --- f13 <-- track 3
- * | | |
- * f2 --- f6 --- f10 <-- track 4
- *
- * Each 'f' is a feature pointer of the type FeatureBasePtr
- *
- * This structure allows accessing all the history of matched features with two different accesses:
- *
- * Track: track-wise: a track of features matches along a timely sequence of Captures, indexed by time stamp.
- *
- * map<TimeStamp ts, FeatureBasePtr f>
- *
- * Snapshot: capture-wise: a set of features tracked in a single Capture, indexed by track id:
- *
- * map<size_t track_id, FeatureBasePtr f>
- *
- * The class makes sure that both accesses are consistent each time some addition or removal of features is performed.
- *
- * The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in logarithmic time:
- *
- * Tracks are identified with the track ID in f->trackId()
- * Snapshots are identified with the Capture pointer in f->getCapture()
- *
- * these fields of FeatureBase are initialized each time a feature is added to the track matrix:
- *
- * add(Cap, track_id, f) will set f.capture_ptr = C and f.traci_id = traci_id.
- *
- * so e.g. given a feature f,
- *
-<<<<<<< HEAD
- * track (f->trackId()) ; // returns all the track where feature f is.
- * snapshot(f->getCapturePtr()) ; // returns all the features in the same capture of f.
-=======
- * getTrack (f->trackId()) ; // returns all the track where feature f is.
- * getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f.
->>>>>>> refs/remotes/origin/devel
- *
- */
-
-class TrackMatrix
-{
- public:
- TrackMatrix();
- virtual ~TrackMatrix();
-
- void newTrack (CaptureBasePtr _cap, FeatureBasePtr _ftr);
- void add (size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr);
- void remove (FeatureBasePtr _ftr);
- void remove (size_t _track_id);
- void remove (CaptureBasePtr _cap);
- map<size_t, Track > tracks () const;
- SizeStd numTracks ();
- SizeStd trackSize (size_t _track_id);
- Track track (size_t _track_id);
- Snapshot snapshot (CaptureBasePtr _capture);
- vector<FeatureBasePtr>
- trackAsVector(size_t _track_id);
- list<FeatureBasePtr>
- snapshotAsList(CaptureBasePtr _cap);
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
- FeatureBasePtr firstFeature(size_t _track_id);
- FeatureBasePtr lastFeature (size_t _track_id);
- FeatureBasePtr feature (size_t _track_id, CaptureBasePtr _cap);
- FeatureBasePtr feature (size_t _track_id, const TimeStamp& _ts); // return feat with time stamp >= _ts
- CaptureBasePtr firstCapture(size_t _track_id);
-
- Track trackAtKeyframes(size_t _track_id);
- bool markKeyframe(CaptureBasePtr _capture);
- bool unmarkKeyframe(FrameBasePtr _keyframe);
-
- private:
-
- static SizeStd track_id_count_;
-
- // Along track: maps of Feature pointers indexed by time stamp.
- // these tracks contain all features from all captures
- map<size_t, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
-
- // these tracks contain only features at captures belonging to a KF
- map<size_t, Track > tracks_kf_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
-
- // Across track: maps of Feature pointers indexed by track_Id.
- map<size_t, Snapshot > snapshots_; // map indexed by capture_Id of ( maps indexed by track_Id of ( features ) )
-};
-
-} /* namespace wolf */
-
-#endif /* TRACK_MATRIX_H_ */
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
deleted file mode 100644
index 6ceddf9e6a8742133b01ee9277369a0a3bcaa5be..0000000000000000000000000000000000000000
--- a/include/base/trajectory_base.h
+++ /dev/null
@@ -1,81 +0,0 @@
-
-#ifndef TRAJECTORY_BASE_H_
-#define TRAJECTORY_BASE_H_
-
-// Fwd refs
-namespace wolf{
-class Problem;
-class FrameBase;
-class TimeStamp;
-}
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-
-//std includes
-
-namespace wolf {
-
-//class TrajectoryBase
-class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
-{
- private:
- std::list<FrameBasePtr> frame_list_;
-
- protected:
- std::string frame_structure_; // Defines the structure of the Frames in the Trajectory.
- FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
-
- public:
- TrajectoryBase(const std::string& _frame_sturcture);
- virtual ~TrajectoryBase();
-
- // Properties
- std::string getFrameStructure() const;
-
- // Frames
- FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
- FrameBasePtrList& getFrameList();
- FrameBasePtr getLastFrame();
- FrameBasePtr getLastKeyFrame();
- FrameBasePtr findLastKeyFrame();
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
- void setLastKeyFrame(FrameBasePtr _key_frame_ptr);
- void sortFrame(FrameBasePtr _frm_ptr);
- void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
- FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
-
- // factors
- void getFactorList(FactorBasePtrList & _fac_list);
-
-};
-
-inline FrameBasePtrList& TrajectoryBase::getFrameList()
-{
- return frame_list_;
-}
-
-inline FrameBasePtr TrajectoryBase::getLastFrame()
-{
- return frame_list_.back();
-}
-
-inline FrameBasePtr TrajectoryBase::getLastKeyFrame()
-{
- return last_key_frame_ptr_;
-}
-
-inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr)
-{
- last_key_frame_ptr_ = _key_frame_ptr;
-}
-
-inline std::string TrajectoryBase::getFrameStructure() const
-{
- return frame_structure_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/wolf.h b/include/base/wolf.h
deleted file mode 100644
index 345312c32e507e9ed058eca824d4ab5c4f0834ad..0000000000000000000000000000000000000000
--- a/include/base/wolf.h
+++ /dev/null
@@ -1,368 +0,0 @@
-/**
- * \file wolf.h
- * \brief General typedefs for the Wolf project
- * \author Joan Sola
- * Created on: 28/05/2014
- */
-
-#ifndef WOLF_H_
-#define WOLF_H_
-
-// Enable project-specific definitions and macros
-#include "internal/config.h"
-#include "base/logging.h"
-
-//includes from Eigen lib
-#include <Eigen/Dense>
-#include <Eigen/Geometry>
-#include <Eigen/Sparse>
-#include <libgen.h>
-
-//includes from std lib
-#include <list>
-#include <map>
-#include <memory> // shared_ptr and weak_ptr
-
-// System specifics
-#include <sys/stat.h>
-
-namespace wolf {
-
-/**
- * \brief scalar type for the Wolf project
- *
- * Do NEVER forget to use Wolf scalar definitions when you code!!!
- *
- * Do NEVER use plain Eigen scalar types!!! (This is redundant with the above. But just to make sure you behave!)
- */
-typedef double Scalar;
-
-/**
- * \brief Vector and Matrices size type for the Wolf project
- *
- * We use the default defined in Eigen (std::ptrdiff_t, a signed integer)
- *
- */
-typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE SizeEigen;
-
-/**
- * \brief Other containers size type for the Wolf project
- *
- * We use the default defined in std (std::size_t, an unsigned integer)
- *
- */
-typedef std::size_t SizeStd;
-
-#define M_TORAD 0.017453292519943295769236907684886127134 // pi / 180
-#define M_TODEG 57.295779513082320876798154814105170332 // 180 / pi
-
-namespace Constants{
-
-// Wolf standard tolerance
-const Scalar EPS = 1e-8;
-// Wolf smmmmall tolerance
-const Scalar EPS_SMALL = 1e-16;
-}
-
-} // namespace wolf
-
-///////////////////////////////////////////
-// Construct types for any scalar defined in the typedef Scalar above
-////////////////////////////////////////////
-/** \brief Namespace extending Eigen definitions
- *
- * We redefine all algebra and gemoetry types to hold double or single precision floats.
- * The appended letter indicating this is 's', so that we have, e.g.,
- * - VectorXf Vector of floats - defined by Eigen
- * - VectorXd Vector of doubles - defined by Eigen
- * - VectorXs Vector of either double of float, depending on the type \b Scalar, defined by Wolf.
- *
- */
-namespace Eigen // Eigen namespace extension
-{
-// 1. Vectors and Matrices
-typedef Matrix<wolf::Scalar, 1, 1, RowMajor> Matrix1s; ///< 2x2 matrix of real Scalar type
-typedef Matrix<wolf::Scalar, 2, 2, RowMajor> Matrix2s; ///< 2x2 matrix of real Scalar type
-typedef Matrix<wolf::Scalar, 3, 3, RowMajor> Matrix3s; ///< 3x3 matrix of real Scalar type
-typedef Matrix<wolf::Scalar, 4, 4, RowMajor> Matrix4s; ///< 4x4 matrix of real Scalar type
-typedef Matrix<wolf::Scalar, 6, 6, RowMajor> Matrix6s; ///< 6x6 matrix of real Scalar type
-typedef Matrix<wolf::Scalar, 7, 7, RowMajor> Matrix7s; ///< 7x7 matrix of real Scalar type
-typedef Matrix<wolf::Scalar, Dynamic, Dynamic, RowMajor> MatrixXs; ///< variable size matrix of real Scalar type
-typedef Matrix<wolf::Scalar, 1, 1> Vector1s; ///< 1-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 2, 1> Vector2s; ///< 2-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 3, 1> Vector3s; ///< 3-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 4, 1> Vector4s; ///< 4-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 5, 1> Vector5s; ///< 5-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 6, 1> Vector6s; ///< 6-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 7, 1> Vector7s; ///< 7-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 8, 1> Vector8s; ///< 8-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 9, 1> Vector9s; ///< 9-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 10, 1> Vector10s; ///< 10-vector of real Scalar type
-typedef Matrix<wolf::Scalar, Dynamic, 1> VectorXs; ///< variable size vector of real Scalar type
-typedef Matrix<wolf::Scalar, 1, 2> RowVector2s; ///< 2-row-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 1, 3> RowVector3s; ///< 3-row-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 1, 4> RowVector4s; ///< 4-row-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 1, 7> RowVector7s; ///< 7-row-vector of real Scalar type
-typedef Matrix<wolf::Scalar, 1, Dynamic> RowVectorXs; ///< variable size row-vector of real Scalar type
-
-// 2. Quaternions and other rotation things
-typedef Quaternion<wolf::Scalar> Quaternions; ///< Quaternion of real Scalar type
-typedef AngleAxis<wolf::Scalar> AngleAxiss; ///< Angle-Axis of real Scalar type
-typedef Rotation2D<wolf::Scalar> Rotation2Ds; ///< Rotation2D of real Scalar type
-
-typedef Transform<wolf::Scalar,2,Affine> Affine2ds; ///< Affine2d of real Scalar type
-typedef Transform<wolf::Scalar,3,Affine> Affine3ds; ///< Affine3d of real Scalar type
-
-typedef Transform<wolf::Scalar,2,Isometry> Isometry2ds; ///< Isometry2d of real Scalar type
-typedef Transform<wolf::Scalar,3,Isometry> Isometry3ds; ///< Isometry3d of real Scalar type
-
-// 3. Sparse matrix
-typedef SparseMatrix<wolf::Scalar, RowMajor, int> SparseMatrixs;
-
-// 4. Arrays
-typedef Array<wolf::Scalar, Dynamic, Dynamic> ArrayXXs;
-typedef Array<wolf::Scalar, Dynamic, 1> ArrayXs;
-}
-
-namespace wolf {
-
-//////////////////////////////////////////////////////////
-/** Check Eigen Matrix sizes, both statically and dynamically
- *
- * Help:
- *
- * The WOLF project implements many template functions using Eigen Matrix and Quaternions, in different versions
- * (Static size, Dynamic size, Map, Matrix expression).
- *
- * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
- *
- * EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
- * EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
- * EIGEN_STATIC_ASSERT_VECTOR_ONLY
- *
- * but they do not work if the evaluated types are of dynamic size.
- *
- * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind:
- *
- * template<typename Derived>
- * inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){
- *
- * MatrixSizeCheck<3,1>::check(_v); /// We check here the size of the input parameter
- *
- * typedef typename Derived::Scalar T;
- *
- * ... code ...
- *
- * return M;
- * }
- *
- * The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ).
- * This check is performed statically or dynamically, depending on the type of argument provided.
- */
-template<int Size, int DesiredSize>
-struct StaticSizeCheck
-{
- template<typename T>
- StaticSizeCheck(const T&)
- {
- static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
- }
-};
-//
-template<int DesiredSize>
-struct StaticSizeCheck<Eigen::Dynamic, DesiredSize>
-{
- template<typename T>
- StaticSizeCheck(const T& t)
- {
- assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
- }
-};
-//
-template<int DesiredR, int DesiredC>
-struct MatrixSizeCheck
-{
- /** \brief Assert matrix size dynamically or statically
- *
- * @param t the variable for which we wish to assert the size.
- *
- * Usage: to assert that t is size (Rows x Cols)
- *
- * MatrixSizeCheck<Rows, Cols>::check(t);
- */
- template<typename T>
- static void check(const T& t)
- {
- StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
- StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
- }
-};
-//
-// End of check matrix sizes /////////////////////////////////////////////////
-
-/////////////////////////////////////////////////////////////////////////
-// TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
-/////////////////////////////////////////////////////////////////////////
-
-#define WOLF_PTR_TYPEDEFS(ClassName) \
- class ClassName; \
- typedef std::shared_ptr<ClassName> ClassName##Ptr; \
- typedef std::shared_ptr<const ClassName> ClassName##ConstPtr; \
- typedef std::weak_ptr<ClassName> ClassName##WPtr;
-
-#define WOLF_LIST_TYPEDEFS(ClassName) \
- class ClassName; \
- typedef std::list<ClassName##Ptr> ClassName##PtrList; \
- typedef ClassName##PtrList::iterator ClassName##Iter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter;
-
-#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
- struct StructName; \
- typedef std::shared_ptr<StructName> StructName##Ptr; \
- typedef std::shared_ptr<const StructName> StructName##ConstPtr; \
-
-// NodeBase
-WOLF_PTR_TYPEDEFS(NodeBase);
-
-// Problem
-WOLF_PTR_TYPEDEFS(Problem);
-
-// Hardware
-WOLF_PTR_TYPEDEFS(HardwareBase);
-
-// - Sensors
-WOLF_PTR_TYPEDEFS(SensorBase);
-WOLF_LIST_TYPEDEFS(SensorBase);
-
-// - - Intrinsics
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsBase);
-
-// - Processors
-WOLF_PTR_TYPEDEFS(ProcessorBase);
-WOLF_LIST_TYPEDEFS(ProcessorBase);
-
-// - ProcessorMotion
-WOLF_PTR_TYPEDEFS(ProcessorMotion);
-
-// - - Processor params
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsBase);
-
-// Trajectory
-WOLF_PTR_TYPEDEFS(TrajectoryBase);
-
-// - Frame
-WOLF_PTR_TYPEDEFS(FrameBase);
-WOLF_LIST_TYPEDEFS(FrameBase);
-
-// - Capture
-WOLF_PTR_TYPEDEFS(CaptureBase);
-WOLF_LIST_TYPEDEFS(CaptureBase);
-
-// - Feature
-WOLF_PTR_TYPEDEFS(FeatureBase);
-WOLF_LIST_TYPEDEFS(FeatureBase);
-
-// - Factor
-WOLF_PTR_TYPEDEFS(FactorBase);
-WOLF_LIST_TYPEDEFS(FactorBase);
-
-// Map
-WOLF_PTR_TYPEDEFS(MapBase);
-
-// - Landmark
-WOLF_PTR_TYPEDEFS(LandmarkBase);
-WOLF_LIST_TYPEDEFS(LandmarkBase);
-
-// - - State blocks
-WOLF_PTR_TYPEDEFS(StateBlock);
-WOLF_LIST_TYPEDEFS(StateBlock);
-WOLF_PTR_TYPEDEFS(StateAngle);
-WOLF_PTR_TYPEDEFS(StateQuaternion);
-
-// - - Local Parametrization
-WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
-
-// ==================================================
-// Some dangling functions
-
-inline bool file_exists(const std::string& _name)
-{
- struct stat buffer;
- return (stat (_name.c_str(), &buffer) == 0);
-}
-
-inline const Eigen::Vector3s gravity(void) {
- return Eigen::Vector3s(0,0,-9.806);
-}
-
-template <typename T, int N, int RC>
-bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M,
- const T eps = wolf::Constants::EPS)
-{
- return M.isApprox(M.transpose(), eps);
-}
-
-template <typename T, int N, int RC>
-bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
-{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M);
-
- if (eigensolver.info() == Eigen::Success)
- {
- // All eigenvalues must be >= 0:
- return (eigensolver.eigenvalues().array() >= eps).all();
- }
-
- return false;
-}
-
-template <typename T, int N, int RC>
-bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
-{
- return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
-}
-
-#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \
- assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
-
-#define WOLF_ASSERT_INFORMATION_MATRIX(x) \
- assert(isCovariance(x, Scalar(0.0)) && "Not an information matrix");
-
-template <typename T, int N, int RC>
-bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
-{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T,N,N,RC> > eigensolver(M);
-
- if (eigensolver.info() == Eigen::Success)
- {
- // All eigenvalues must be >= 0:
- Scalar epsilon = eps;
- while ((eigensolver.eigenvalues().array() < eps).any())
- {
- //std::cout << "----- any negative eigenvalue or too close to zero\n";
- //std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
- //std::cout << "previous determinant: " << M.determinant() << std::endl;
- M = eigensolver.eigenvectors() *
- eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
- eigensolver.eigenvectors().transpose();
- eigensolver.compute(M);
- //std::cout << "epsilon used: " << epsilon << std::endl;
- //std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
- //std::cout << "posterior determinant: " << M.determinant() << std::endl;
- epsilon *=10;
- }
- WOLF_ASSERT_COVARIANCE_MATRIX(M);
-
- return epsilon != eps;
- }
- else
- WOLF_ERROR("Couldn't compute covariance eigen decomposition");
-
- return false;
-}
-
-//===================================================
-
-} // namespace wolf
-
-#endif /* WOLF_H_ */
diff --git a/include/base/yaml/yaml_conversion.h b/include/base/yaml/yaml_conversion.h
deleted file mode 100644
index 0542cba4d870978b913f36c02f429491ee61d389..0000000000000000000000000000000000000000
--- a/include/base/yaml/yaml_conversion.h
+++ /dev/null
@@ -1,214 +0,0 @@
-/**
- * \file yaml_conversion.h
- *
- * Created on: May 12, 2016
- * \author: jsola
- */
-
-#ifndef YAML_YAML_CONVERSION_H_
-#define YAML_YAML_CONVERSION_H_
-
-// Yaml
-#include <yaml-cpp/yaml.h>
-
-// Eigen
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-// stl
-#include <iostream>
-
-namespace YAML
-{
-
-/**\brief Bridge YAML to and from Eigen::Matrix< >.
- *
- */
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct convert<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-{
- /** \brief Encode a YAML Sequence from an Eigen::Matrix< >
- */
- static Node encode(const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix)
- {
- Node node; //(NodeType::Sequence);
-
- int nValues = matrix.rows() * matrix.cols();
-
- for (int i = 0; i < nValues; ++i)
- {
- node.push_back(matrix(i / matrix.cols(), i % matrix.cols()));
- }
-
- return node;
- }
-
- /** \brief Decode a YAML sequence into a ````Eigen::Matrix<typename _Scalar, int _Rows, int _Cols>````
- *
- * Two YAML formats are accepted:
- *
- * - For matrices where at least one dimension is not Dynamic:
- * We use a single sequence of matrix entries: the matrix dimensions are inferred
- * ````
- * [ v1, v2, v3, ...]
- * ````
- * - For all kinds of matrices. We use a sequence of two sequences.
- * The first sequence is a vector of dimensions; the second sequence contains the matrix entries
- * ````
- * [ [ rows, cols ], [v1, v2, v3, ...] ]
- * ````
- */
- static bool decode(const Node& node, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix)
- {
- YAML::Node values;
-
- // ==========================================================================================
- if (node[0].Type() == NodeType::Sequence) // sizes given by YAML
- {
- if (node.size() == 2 && node[0].size() == 2 && node[1].Type() == NodeType::Sequence
- && node[1].size() == node[0][0].as<unsigned int>() * node[0][1].as<unsigned int>()) // YAML string is well formed
- {
- int rows = node[0][0].as<int>();
- int cols = node[0][1].as<int>();
- values = node[1];
- if (_Rows == Eigen::Dynamic && _Cols == Eigen::Dynamic) // full dynamic
- {
- matrix.resize(rows, cols);
- }
- else if (_Rows == Eigen::Dynamic) // rows dynamic
- {
- if (_Cols != cols)
- {
- std::cout << "Wrong number of cols" << std::endl;
- return false;
- }
- matrix.resize(rows, Eigen::NoChange);
- }
- else if (_Cols == Eigen::Dynamic) // cols dynamic
- {
- if (_Rows != rows)
- {
- std::cout << "Wrong number of rows" << std::endl;
- return false;
- }
- matrix.resize(Eigen::NoChange, cols);
- }
- else // fixed size
- {
- if (_Rows != rows || _Cols != cols)
- {
- std::cout << "Wrong size" << std::endl;
- return false;
- }
- }
- }
- else
- {
- std::cout << "Bad matrix specification" << std::endl;
- return false;
- }
- }
- else // sizes given by Matrix
- {
- // If full dynamic -> error
- if (_Rows == Eigen::Dynamic && _Cols == Eigen::Dynamic)
- {
- std::cout << "Bad yaml format. A full dynamic matrix requires [ [rows, cols], [... values ...] ]"
- << std::endl;
- return false;
- }
- values = node;
-
- // If one dimension is dynamic -> calculate and resize
-
- // _Rows is Dynamic
- if (_Rows == Eigen::Dynamic)
- {
- if (values.size() % _Cols != 0)
- {
- std::cout << "Input size of dynamic row matrix is not a multiple of fixed column size"
- << std::endl;
- return false;
- }
-
- int nDynRows = values.size() / _Cols;
- matrix.resize(nDynRows, Eigen::NoChange);
- }
-
- // _Cols is Dynamic
- if (_Cols == Eigen::Dynamic)
- {
- if (values.size() % _Rows != 0)
- {
- std::cout << "Input size of dynamic column matrix is not a multiple of fixed row size"
- << std::endl;
- return false;
- }
-
- int nDynCols = values.size() / _Rows;
- matrix.resize(Eigen::NoChange, nDynCols);
- }
- }
-
- // final check for good size
- if (values.size() != (unsigned int)(matrix.rows() * matrix.cols()))
- {
- std::cout << "Wrong input size" << std::endl;
- return false;
- }
- else // Fill the matrix
- {
- for (int i = 0; i < matrix.rows(); i++)
- for (int j = 0; j < matrix.cols(); j++)
- matrix(i, j) = values[(int)(i * matrix.cols() + j)].as<_Scalar>();
- }
- return true;
- }
-};
-
-/**\brief Bridge YAML <--> Eigen::Quaternion with real component last
- *
- * WARNING: Beware of Eigen constructor order!
- *
- * We use the x-y-z-w order, with the real part at the end. This is consistent with ROS Quaternion.msg,
- * which is good for compatibility against ROS messages and YAML configuration.
- *
- */
-template<typename _Scalar, int _Options>
-struct convert<Eigen::Quaternion<_Scalar, _Options> >
-{
- static Node encode(const Eigen::Quaternion<_Scalar, _Options>& quaternion)
- {
- Node node(NodeType::Sequence);
-
- node[0] = quaternion.x();
- node[1] = quaternion.y();
- node[2] = quaternion.z();
- node[3] = quaternion.w();
-
- return node;
- }
-
- static bool decode(const Node& node, Eigen::Quaternion<_Scalar, _Options>& quaternion)
- {
-
- int nSize = node.size(); // Sequence check is implicit
- if (nSize != 4)
- {
- std::cout << "Wrong quaternion input size!" << std::endl;
- return false;
- }
- else
- {
- quaternion.x() = node[0].as<_Scalar>();
- quaternion.y() = node[1].as<_Scalar>();
- quaternion.z() = node[2].as<_Scalar>();
- quaternion.w() = node[3].as<_Scalar>();
- }
- return true;
- }
-};
-
-} // namespace YAML
-
-#endif /* YAML_YAML_CONVERSION_H_ */
diff --git a/include/base/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
similarity index 96%
rename from include/base/capture/capture_gnss_fix.h
rename to include/gnss/capture/capture_gnss_fix.h
index 1902e6134d68a82ed10ef344e23522217e1a135f..2a00973a2cfb0e829c53ec48ed71ad30a918b0c2 100644
--- a/include/base/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -2,7 +2,7 @@
#define CAPTURE_GNSS_FIX_H_
//Wolf includes
-#include "base/feature/feature_gnss_fix.h"
+#include "gnss/feature/feature_gnss_fix.h"
#include "base/capture/capture_base.h"
//std includes
diff --git a/include/base/capture/capture_gnss_single_diff.h b/include/gnss/capture/capture_gnss_single_diff.h
similarity index 97%
rename from include/base/capture/capture_gnss_single_diff.h
rename to include/gnss/capture/capture_gnss_single_diff.h
index 0eba8c0c69c697df9356698ebe90c0d08d8fcc83..b8c080fb0066562d0ef3e74b479fe7722448bc57 100644
--- a/include/base/capture/capture_gnss_single_diff.h
+++ b/include/gnss/capture/capture_gnss_single_diff.h
@@ -2,7 +2,7 @@
#define CAPTURE_GNSS_SINGLE_DIFF_H_
//Wolf includes
-#include "base/feature/feature_gnss_single_diff.h"
+#include "gnss/feature/feature_gnss_single_diff.h"
#include "base/capture/capture_base.h"
//std includes
diff --git a/include/base/factor/factor_gnss_fix_2D.h b/include/gnss/factor/factor_gnss_fix_2D.h
similarity index 98%
rename from include/base/factor/factor_gnss_fix_2D.h
rename to include/gnss/factor/factor_gnss_fix_2D.h
index 70434ec608e060c8ac45c5010ea4a31c6a78bfd0..3bdaf6fa617ce2a6fdf5c96a520673d99ca78045 100644
--- a/include/base/factor/factor_gnss_fix_2D.h
+++ b/include/gnss/factor/factor_gnss_fix_2D.h
@@ -3,10 +3,10 @@
#define FACTOR_GNSS_FIX_2D_H_
//Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/sensor/sensor_gnss.h"
+#include "base/frame/frame_base.h"
+#include "gnss/sensor/sensor_gnss.h"
namespace wolf {
diff --git a/include/base/factor/factor_gnss_fix_3D.h b/include/gnss/factor/factor_gnss_fix_3D.h
similarity index 98%
rename from include/base/factor/factor_gnss_fix_3D.h
rename to include/gnss/factor/factor_gnss_fix_3D.h
index 343958d355d92d2f822708d6c36a13583ed4a90e..713ec5e26393c2673d39cf78ba451ff9bc0e3c26 100644
--- a/include/base/factor/factor_gnss_fix_3D.h
+++ b/include/gnss/factor/factor_gnss_fix_3D.h
@@ -3,9 +3,9 @@
#define FACTOR_GNSS_FIX_3D_H_
//Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
namespace wolf {
diff --git a/include/base/factor/factor_gnss_single_diff_2D.h b/include/gnss/factor/factor_gnss_single_diff_2D.h
similarity index 98%
rename from include/base/factor/factor_gnss_single_diff_2D.h
rename to include/gnss/factor/factor_gnss_single_diff_2D.h
index 9766685930d7a9e758a6823e02e8a021b660f60d..ed53f0073ed1ac9fcbe02488c33f1df06edce712 100644
--- a/include/base/factor/factor_gnss_single_diff_2D.h
+++ b/include/gnss/factor/factor_gnss_single_diff_2D.h
@@ -3,10 +3,10 @@
#define FACTOR_GNSS_SINGLE_DIFF_2D_H_
//Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/sensor/sensor_gnss.h"
+#include "base/frame/frame_base.h"
+#include "gnss/sensor/sensor_gnss.h"
namespace wolf {
diff --git a/include/base/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h
similarity index 94%
rename from include/base/feature/feature_gnss_fix.h
rename to include/gnss/feature/feature_gnss_fix.h
index 41b027936414bae092e2f589411c7083d3f56c48..2f7e8cd995a332aed2a0f91b0a69204bbadcfa12 100644
--- a/include/base/feature/feature_gnss_fix.h
+++ b/include/gnss/feature/feature_gnss_fix.h
@@ -2,7 +2,7 @@
#define FEATURE_GNSS_FIX_H_
//Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
//std includes
diff --git a/include/base/feature/feature_gnss_single_diff.h b/include/gnss/feature/feature_gnss_single_diff.h
similarity index 95%
rename from include/base/feature/feature_gnss_single_diff.h
rename to include/gnss/feature/feature_gnss_single_diff.h
index 2035510344842ee45abe6ac2ceaa47546b42fefb..f21c634b82fa5cdf5a6ac5258d3033324f6cfe38 100644
--- a/include/base/feature/feature_gnss_single_diff.h
+++ b/include/gnss/feature/feature_gnss_single_diff.h
@@ -2,7 +2,7 @@
#define FEATURE_GNSS_SINGLE_DIFF_H_
//Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
//std includes
diff --git a/include/base/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
similarity index 96%
rename from include/base/processor/processor_gnss_fix.h
rename to include/gnss/processor/processor_gnss_fix.h
index 396b07b1b211076dadc78c83ba306e51a91c2adb..eafeed0ceb5de1d0c7bcbdb750ad682ad8552e43 100644
--- a/include/base/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -3,7 +3,7 @@
// Wolf includes
#include "base/processor/processor_base.h"
-#include "base/capture/capture_gnss_fix.h"
+#include "gnss/capture/capture_gnss_fix.h"
// Std includes
diff --git a/include/base/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h
similarity index 94%
rename from include/base/processor/processor_gnss_single_diff.h
rename to include/gnss/processor/processor_gnss_single_diff.h
index 969df10ef8a11127baa6c452e36a07ecb56a158e..3f7a6702fe164c650e3cddb4d8e59d0773021815 100644
--- a/include/base/processor/processor_gnss_single_diff.h
+++ b/include/gnss/processor/processor_gnss_single_diff.h
@@ -3,7 +3,7 @@
// Wolf includes
#include "base/processor/processor_base.h"
-#include "base/capture/capture_gnss_single_diff.h"
+#include "gnss/capture/capture_gnss_single_diff.h"
// Std includes
diff --git a/include/base/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
similarity index 98%
rename from include/base/sensor/sensor_gnss.h
rename to include/gnss/sensor/sensor_gnss.h
index 80367f52dcfc0b5ea38ba122835f5e2143696df6..fe006440d56ecef2bd971272930c72e470d435c3 100644
--- a/include/base/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -2,7 +2,7 @@
#define SENSOR_GPS_H_
//wolf
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
// std
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index 20114da7f66f44b53dda372d726a78100a0f536b..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,820 +0,0 @@
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
-IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
- set(_WOLF_DEBUG true)
-ENDIF()
-
-option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-
-option(BUILD_EXAMPLES "Build examples" ON)
-set(BUILD_TESTS true)
-
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-#find dependencies.
-
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
-
-FIND_PACKAGE(Threads REQUIRED)
-
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
- FIND_PACKAGE(GLUT REQUIRED)
- FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
-IF(laser_scan_utils_FOUND)
- MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
-ENDIF(laser_scan_utils_FOUND)
-
-FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
-IF(raw_gps_utils_FOUND)
- MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision Utils
-FIND_PACKAGE(vision_utils QUIET)
-IF (vision_utils_FOUND)
- MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
- SET(PRINT_INFO_VU false)
- FIND_PACKAGE(OpenCV QUIET)
-ENDIF(vision_utils_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
-# YAML with yaml-cpp
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
-IF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-ELSEIF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library NOT FOUND!")
-ENDIF(YAMLCPP_FOUND)
-
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
- MESSAGE("glog Library FOUND: glog related sources will be built.")
- MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
- MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
- MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
-FIND_PATH(
- Suitesparse_INCLUDE_DIRS
- NAMES SuiteSparse_config.h
- PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND TRUE)
- MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND FALSE)
- MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
-
-# Define the directory where will be the configured config.h
-SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/internal)
-
-# Create the specified output directory if it does not exist.
-IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
- message(STATUS "Creating config output directory: ${WOLF_CONFIG_DIR}")
- file(MAKE_DIRECTORY "${WOLF_CONFIG_DIR}")
-ENDIF()
-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")
-
-#TEMPORAL INCLUDE UNTIL WE FIGURE OUT WHAT TO DO WITH FILES in src/temp
-
-include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-include_directories(../dummyInclude)
-
-# Include config.h directory at first.
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-INCLUDE_DIRECTORIES(.)
-
-# 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)
-
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-IF(laser_scan_utils_FOUND)
- INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-ENDIF(laser_scan_utils_FOUND)
-
-IF(raw_gps_utils_FOUND)
- INCLUDE_DIRECTORIES(${raw_gps_utils_INCLUDE_DIRS})
-ENDIF(raw_gps_utils_FOUND)
-
-IF(vision_utils_FOUND)
- INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(vision_utils_FOUND)
-
-# cereal
-IF(cereal_FOUND)
- INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-
-IF(Suitesparse_FOUND)
- INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(YAMLCPP_FOUND)
- INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
-IF(GLOG_FOUND)
-INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
-
-#headers
-SET(HDRS_BASE
-capture/capture_motion.h
- eigen_assert.h
- eigen_predicates.h
-landmark/landmark_match.h
- make_unique.h
- pinhole_tools.h
-processor/processor_capture_holder.h
-processor/processor_tracker_landmark.h
- )
-SET(HDRS
-<<<<<<< HEAD
- capture_pose.h
- capture_gnss_fix.h
- capture_gnss_single_diff.h
- capture_GPS_fix.h
- capture_IMU.h
- capture_odom_2D.h
- capture_odom_3D.h
- capture_void.h
- constraint_block_absolute.h
- constraint_container.h
- constraint_corner_2D.h
- constraint_AHP.h
- constraint_epipolar.h
- constraint_IMU.h
- constraint_fix_bias.h
- constraint_gnss_fix_2D.h
- constraint_gnss_fix_3D.h
- constraint_gnss_single_diff_2D.h
- constraint_GPS_2D.h
- constraint_GPS_pseudorange_3D.h
- constraint_GPS_pseudorange_2D.h
- constraint_odom_2D.h
- constraint_odom_2D_analytic.h
- constraint_odom_3D.h
- constraint_point_2D.h
- constraint_point_to_line_2D.h
- constraint_pose_2D.h
- constraint_pose_3D.h
- constraint_quaternion_absolute.h
- constraint_relative_2D_analytic.h
- feature_corner_2D.h
- feature_gnss_fix.h
- feature_gnss_single_diff.h
- feature_GPS_fix.h
- feature_GPS_pseudorange.h
- feature_IMU.h
- feature_odom_2D.h
- feature_polyline_2D.h
- feature_pose.h
-=======
-capture/capture_motion.h
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_AHP.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- temp/diff_drive_tools.h
- temp/diff_drive_tools.hpp
-feature/feature_corner_2D.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
->>>>>>> devel_refactor
- IMU_tools.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- local_parametrization_polyline_extreme.h
-<<<<<<< HEAD
- processor_frame_nearest_neighbor_filter.h
- processor_gnss_fix.h
- processor_gnss_single_diff.h
- processor_IMU.h
- test/processor_IMU_UnitTester.h
- processor_odom_2D.h
- processor_odom_3D.h
- processor_tracker_feature_dummy.h
- processor_tracker_landmark_dummy.h
- sensor_camera.h
- sensor_gnss.h
- sensor_GPS.h
- sensor_GPS_fix.h
- sensor_IMU.h
- sensor_odom_2D.h
- sensor_odom_3D.h
-=======
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
- test/processor_IMU_UnitTester.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark_dummy.h
-sensor/sensor_camera.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
- SET(HDRS_CAPTURE
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-capture/capture_velocity.h
-capture/capture_wheel_joint_position.h
- )
- SET(HDRS_CONSTRAINT
-factor/factor_autodiff_distance_3D.h
-factor/factor_AHP.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_diff_drive.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- )
- SET(HDRS_FEATURE
-feature/feature_corner_2D.h
-feature/feature_diff_drive.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- )
- SET(HDRS_LANDMARK
-landmark/landmark_match.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- )
- SET(HDRS_PROCESSOR
-processor/processor_capture_holder.h
-processor/processor_diff_drive.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark.h
-processor/processor_tracker_landmark_dummy.h
- )
- SET(HDRS_SENSOR
-sensor/sensor_camera.h
-sensor/sensor_diff_drive.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
->>>>>>> devel_refactor
- )
-# [Add generic derived header before this line]
-
-SET(HDRS_DTASSC
- data_association/matrix.h
- data_association/association_solver.h
- data_association/association_node.h
- data_association/association_tree.h
- data_association/association_nnls.h
- )
-
-SET(HDRS_CORE
-core/capture_base.h
-core/capture_buffer.h
-core/capture_pose.h
-core/capture_void.h
-core/factor_analytic.h
-core/factor_autodiff.h
-core/factor_base.h
-core/factory.h
-core/feature_base.h
-core/feature_match.h
-core/feature_pose.h
-core/frame_base.h
-core/hardware_base.h
-core/landmark_base.h
-core/local_parametrization_angle.h
-core/local_parametrization_base.h
-core/local_parametrization_homogeneous.h
-core/local_parametrization_quaternion.h
-core/logging.h
-core/map_base.h
-core/motion_buffer.h
-core/node_base.h
-core/problem.h
-core/processor_base.h
-core/factory.h
-core/processor_loopclosure_base.h
-core/processor_motion.h
-core/processor_tracker_feature.h
-core/processor_tracker.h
-core/rotations.h
-core/sensor_base.h
-core/factory.h
-core/singleton.h
-core/state_angle.h
-core/state_block.h
-core/state_homogeneous_3D.h
-core/state_quaternion.h
-core/three_D_tools.h
-core/time_stamp.h
-core/track_matrix.h
-core/trajectory_base.h
-core/wolf.h
- )
-SET(SRCS_CORE
-core/capture_base.cpp
-core/capture_pose.cpp
-core/capture_void.cpp
-core/factor_analytic.cpp
-core/factor_base.cpp
-core/feature_base.cpp
-core/feature_pose.cpp
-core/frame_base.cpp
-core/hardware_base.cpp
-core/landmark_base.cpp
-core/local_parametrization_base.cpp
-core/local_parametrization_homogeneous.cpp
-core/local_parametrization_quaternion.cpp
-core/map_base.cpp
-core/motion_buffer.cpp
-core/node_base.cpp
-core/problem.cpp
-core/processor_base.cpp
-core/processor_loopclosure_base.cpp
-core/processor_motion.cpp
-core/processor_tracker.cpp
-core/processor_tracker_feature.cpp
-core/sensor_base.cpp
-core/state_block.cpp
-core/time_stamp.cpp
-core/track_matrix.cpp
-core/trajectory_base.cpp
- )
-#sources
-SET(SRCS_BASE
-capture/capture_motion.cpp
-processor/processor_capture_holder.cpp
-processor/processor_tracker_landmark.cpp
- )
-
-SET(SRCS
-<<<<<<< HEAD
- capture_gnss_fix.cpp
- capture_GPS_fix.cpp
- capture_IMU.cpp
- capture_odom_2D.cpp
- capture_odom_3D.cpp
- capture_pose.cpp
- capture_void.cpp
- feature_corner_2D.cpp
- feature_GPS_fix.cpp
- feature_GPS_pseudorange.cpp
- feature_IMU.cpp
- feature_odom_2D.cpp
- feature_polyline_2D.cpp
- feature_pose.cpp
- landmark_corner_2D.cpp
- landmark_container.cpp
- landmark_line_2D.cpp
- landmark_polyline_2D.cpp
- local_parametrization_polyline_extreme.cpp
- processor_frame_nearest_neighbor_filter.cpp
- processor_gnss_fix.cpp
- processor_gnss_single_diff.cpp
- processor_IMU.cpp
- test/processor_IMU_UnitTester.cpp
- processor_odom_2D.cpp
- processor_odom_3D.cpp
- processor_tracker_feature_dummy.cpp
- processor_tracker_landmark_dummy.cpp
- sensor_camera.cpp
- sensor_gnss.cpp
- sensor_GPS.cpp
- sensor_GPS_fix.cpp
- sensor_IMU.cpp
- sensor_odom_2D.cpp
- sensor_odom_3D.cpp
-=======
- local_parametrization_polyline_extreme.cpp
- test/processor_IMU_UnitTester.cpp
- )
- SET(SRCS_CAPTURE
-capture/capture_GPS_fix.cpp
-capture/capture_IMU.cpp
-capture/capture_odom_2D.cpp
-capture/capture_odom_3D.cpp
-capture/capture_velocity.cpp
-capture/capture_wheel_joint_position.cpp
- )
- SET(SRCS_FEATURE
-feature/feature_corner_2D.cpp
-feature/feature_diff_drive.cpp
-feature/feature_GPS_fix.cpp
-feature/feature_GPS_pseudorange.cpp
-feature/feature_IMU.cpp
-feature/feature_odom_2D.cpp
-feature/feature_polyline_2D.cpp
- )
- SET(SRCS_LANDMARK
-landmark/landmark_corner_2D.cpp
-landmark/landmark_container.cpp
-landmark/landmark_line_2D.cpp
-landmark/landmark_polyline_2D.cpp
- )
- SET(SRCS_PROCESSOR
-processor/processor_frame_nearest_neighbor_filter.cpp
-processor/processor_diff_drive.cpp
-processor/processor_IMU.cpp
-processor/processor_odom_2D.cpp
-processor/processor_odom_3D.cpp
-processor/processor_tracker_feature_dummy.cpp
-processor/processor_tracker_landmark_dummy.cpp
- )
- SET(SRCS_SENSOR
-sensor/sensor_camera.cpp
-sensor/sensor_diff_drive.cpp
-sensor/sensor_GPS.cpp
-sensor/sensor_GPS_fix.cpp
-sensor/sensor_IMU.cpp
-sensor/sensor_odom_2D.cpp
-sensor/sensor_odom_3D.cpp
->>>>>>> devel_refactor
- )
-SET(SRCS_DTASSC
- data_association/association_solver.cpp
- data_association/association_node.cpp
- data_association/association_tree.cpp
- data_association/association_nnls.cpp
- )
-
-# Add the solver sub-directory
-add_subdirectory(solver)
-
-#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
- ceres_wrapper/sparse_utils.h
- #ceres_wrapper/solver_manager.h
- ceres_wrapper/ceres_manager.h
- #ceres_wrapper/qr_manager.h
- ceres_wrapper/cost_function_wrapper.h
- ceres_wrapper/create_numeric_diff_cost_function.h
- ceres_wrapper/local_parametrization_wrapper.h
- )
- SET(SRCS_WRAPPER
- #ceres_wrapper/solver_manager.cpp
- ceres_wrapper/ceres_manager.cpp
- #ceres_wrapper/qr_manager.cpp
- ceres_wrapper/local_parametrization_wrapper.cpp
- )
-ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- SET(HDRS ${HDRS}
-<<<<<<< HEAD
- capture_laser_2D.h
- sensor_laser_2D.h
- processor_tracker_feature_corner.h
- processor_tracker_landmark_corner.h
- processor_polyline.h
- )
- SET(SRCS ${SRCS}
- capture_laser_2D.cpp
- sensor_laser_2D.cpp
- processor_tracker_feature_corner.cpp
- processor_tracker_landmark_corner.cpp
- processor_polyline.cpp
-=======
-capture/capture_laser_2D.h
-sensor/sensor_laser_2D.h
-processor/processor_tracker_feature_corner.h
-processor/processor_tracker_landmark_corner.h
-processor/processor_tracker_landmark_polyline.h
- )
- SET(SRCS ${SRCS}
-capture/capture_laser_2D.cpp
-sensor/sensor_laser_2D.cpp
-processor/processor_tracker_feature_corner.cpp
-processor/processor_tracker_landmark_corner.cpp
-processor/processor_tracker_landmark_polyline.cpp
->>>>>>> devel_refactor
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_GPS.h
-processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
-capture/capture_GPS.cpp
-processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_image.h
-feature/feature_point_image.h
-landmark/landmark_AHP.h
-processor/processor_params_image.h
-processor/processor_tracker_feature_image.h
-processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
-landmark/landmark_point_3D.h
- )
- SET(SRCS ${SRCS}
-capture/capture_image.cpp
-feature/feature_point_image.cpp
-landmark/landmark_AHP.cpp
-processor/processor_tracker_feature_image.cpp
-processor/processor_tracker_landmark_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
-landmark/landmark_point_3D.cpp
- )
-ENDIF(vision_utils_FOUND)
-
-# Add the capture sub-directory
-# ADD_SUBDIRECTORY(captures)
-
-# Add the factors sub-directory
-# ADD_SUBDIRECTORY(factors)
-
-# Add the features sub-directory
-# ADD_SUBDIRECTORY(features)
-
-# Add the landmarks sub-directory
-# ADD_SUBDIRECTORY(landmarks)
-
-# Add the processors sub-directory
-# ADD_SUBDIRECTORY(processors)
-
-# Add the sensors sub-directory
-# ADD_SUBDIRECTORY(sensors)
-
-# Add the hello_wolf sub-directory
-ADD_SUBDIRECTORY(hello_wolf)
-
-IF (cereal_FOUND)
- ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
-IF (Suitesparse_FOUND)
- ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS ${HDRS}
- yaml/yaml_conversion.h
- )
-
- # sources
- SET(SRCS ${SRCS}
- yaml/processor_odom_3D_yaml.cpp
- yaml/processor_IMU_yaml.cpp
- yaml/sensor_camera_yaml.cpp
- yaml/sensor_odom_3D_yaml.cpp
- yaml/sensor_IMU_yaml.cpp
- )
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/processor_image_yaml.cpp
- )
- ENDIF(vision_utils_FOUND)
-ENDIF(YAMLCPP_FOUND)
-
-# create the shared library
-add_library(${PROJECT_NAME}_core SHARED
- ${SRCS_CORE}
- ${SRCS_BASE}
- )
-ADD_LIBRARY(${PROJECT_NAME}
- SHARED
- ${SRCS_BASE}
- ${SRCS}
- ${SRCS_CAPTURE}
- ${SRCS_CONSTRAINT}
- ${SRCS_FEATURE}
- ${SRCS_LANDMARK}
- ${SRCS_PROCESSOR}
- ${SRCS_SENSOR}
- #${SRCS_DTASSC}
- ${SRCS_SOLVER}
- ${SRCS_WRAPPER}
- )
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
-
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core)
-#Link the created libraries
-#=============================================================
-IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${raw_gps_utils_LIBRARY})
-ENDIF (raw_gps_utils_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
- IF (vision_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${vision_utils_LIBRARY})
- ENDIF (vision_utils_FOUND)
-ENDIF (OPENCV_FOUND)
-
-IF (YAMLCPP_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
-ENDIF (YAMLCPP_FOUND)
-
-IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
-
-#install library
-install(TARGETS ${PROJECT_NAME}_core DESTINATION lib/iri-algorithms EXPORT ${PROJECT_NAME}_core-targets)
-install(EXPORT ${PROJECT_NAME}_core-targets DESTINATION lib/iri-algorithms)
-#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
-
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
-
-#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS_CORE}
- DESTINATION include/iri-algorithms/wolf/core)
-#INSTALL(FILES ${HDRS_DTASSC}
-# DESTINATION include/iri-algorithms/wolf/data_association)
-INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/capture)
-INSTALL(FILES ${HDRS_CONSTRAINT}
- DESTINATION include/iri-algorithms/wolf/factor)
-INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/processor)
-INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/serialization)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
- DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
-
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-DESTINATION include/iri-algorithms/wolf/internal)
-
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
-export(PACKAGE ${PROJECT_NAME})
-
-#############
-## Testing ##
-#############
-IF (GLOG_FOUND)
- IF(BUILD_TESTS)
- MESSAGE("Building tests.")
- add_subdirectory(test)
- ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
-
-IF(BUILD_EXAMPLES)
- #Build examples
- MESSAGE("Building examples.")
- ADD_SUBDIRECTORY(examples)
-ENDIF(BUILD_EXAMPLES)
-
diff --git a/src/association/association_nnls.cpp b/src/association/association_nnls.cpp
deleted file mode 100644
index 5d8286a782576b248f367b7770df440e21dfe3f4..0000000000000000000000000000000000000000
--- a/src/association/association_nnls.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "base/association/association_nnls.h"
-
-namespace wolf
-{
-
-AssociationNNLS::AssociationNNLS() :
- max_dist_(MAX_DIST_DEFAULT)
-{
- //
-}
-
-AssociationNNLS::~AssociationNNLS()
-{
- //
-}
-
-void AssociationNNLS::setMaxDist(const double _max_dist)
-{
- max_dist_ = _max_dist;
-}
-
-void AssociationNNLS::reset()
-{
- nd_ = 0;
- nt_ = 0;
- scores_.clear();
- i_mask_.clear();
- j_mask_.clear();
-}
-
-void AssociationNNLS::resize(const unsigned int _n_det, const unsigned int _n_tar)
-{
- nd_ = _n_det; //detections
- nt_ = _n_tar; //targets
- scores_.resize(nd_, nt_);
- i_mask_.resize(nd_, false);
- j_mask_.resize(nt_, false);
-}
-
-//void AssociationNNLS::solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<unsigned int> & _unassoc)
-void AssociationNNLS::solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask)
-{
- bool min_found = true;
- double min_value;
- unsigned int ii, jj, ii_min, jj_min;
-
- //resize _associated_mask and resets it to false
- _associated_mask.resize(nd_,false);
-
- //find nearest neighbors by successive passing and masking through the scores_ matrix
- while(min_found)
- {
- min_found = false;
- min_value = max_dist_;
-
- for(ii = 0; ii< nd_; ii++)
- {
- if ( i_mask_[ii] == false)
- {
- for(jj = 0; jj< nt_; jj++)
- {
- if ( j_mask_[jj] == false)
- {
- if ( (scores_(ii,jj) < max_dist_) && (scores_(ii,jj) < min_value) )
- {
- min_value = scores_(ii,jj);
- min_found = true;
- ii_min = ii;
- jj_min = jj;
- }
- }
- }
- }
- }
-
- if (min_found)
- {
- i_mask_[ii_min] = true;
- j_mask_[jj_min] = true;
- _associated_mask.at(ii_min) = true;
- _pairs.push_back( std::pair<unsigned int, unsigned int>(ii_min, jj_min) );
- }
- }
-
- //set unassociated detections
-// for(ii = 0; ii< nd_; ii++)
-// {
-// if (i_mask_[ii] == false)
-// {
-// _unassoc.push_back(ii);
-// }
-// }
-
-}
-
-} // namespace wolf
diff --git a/src/association/association_node.cpp b/src/association/association_node.cpp
deleted file mode 100644
index 1321dac069e0e86c2bfb9c2df8060cc514d49797..0000000000000000000000000000000000000000
--- a/src/association/association_node.cpp
+++ /dev/null
@@ -1,195 +0,0 @@
-
-#include "base/association/association_node.h"
-
-AssociationNode::AssociationNode(const unsigned int _det_idx, const unsigned int _tar_idx, const double _prob, AssociationNode * _un_ptr, bool _is_root) :
- is_root_(_is_root),
- det_idx_(_det_idx),
- tar_idx_(_tar_idx),
- node_prob_(_prob),
- tree_prob_(1.)
-{
- up_node_ptr_ = _un_ptr;
-}
-
-AssociationNode::~AssociationNode()
-{
- //
-}
-
-bool AssociationNode::isRoot() const
-{
-// if ( up_node_ptr_ == NULL ) return true;
-// else return false;
- return is_root_;
-}
-
-bool AssociationNode::isTerminus() const
-{
- if ( node_list_.empty() ) return true;
- else return false;
-}
-
-unsigned int AssociationNode::getDetectionIndex() const
-{
- return det_idx_;
-}
-
-unsigned int AssociationNode::getTargetIndex() const
-{
- return tar_idx_;
-}
-
-double AssociationNode::getNodeProb() const
-{
- return node_prob_;
-}
-
-void AssociationNode::setNodeProb(double _np)
-{
- node_prob_ = _np;
-}
-
-double AssociationNode::getTreeProb() const
-{
- return tree_prob_;
-}
-
-AssociationNode* AssociationNode::upNode() const
-{
- //return &(*up_node_ptr_);
- return up_node_ptr_;
-}
-
-//double AssociationNode::computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const std::vector< std::vector<double> > & _stab) const
-double AssociationNode::computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const Matrixx<double> & _stab) const
-{
- double p_ij = 1.0;
-
- if ( _tj == _nt ) //Case void target -> unassociated detection
- {
- //Prob detection _di does not match to other targets than _tj
- for (unsigned int kk=0; kk<_nt; kk++)
- {
- //p_ij *= ( 1.0 - _stab.at(_di).at(kk) );
- p_ij *= ( 1.0 - _stab(_di,kk) );
- }
- }
- else //General case
- {
- //step 1. Positive matching _di with _tj
-// p_ij *= _stab.at(_di).at(_tj);
- p_ij *= _stab(_di,_tj);
-
- //step2. Prob detection _di does not match to other targets than _tj
- for (unsigned int kk=0; kk<_nt; kk++)
- {
-// if ( kk!=_tj ) p_ij *= 1 - _stab.at(_di).at(kk);
- if ( kk!=_tj ) p_ij *= 1 - _stab(_di,kk);
- }
-
- //step3. Prob target _tj does not match to other detections than _di
- for (unsigned int kk=0; kk<_nd; kk++)
- {
-// if ( kk!=_di ) p_ij *= 1 - _stab.at(kk).at(_tj);
- if ( kk!=_di ) p_ij *= 1 - _stab(kk,_tj);
- }
-
- //step4. Prob detection _di does not remain unassociated
- double p_un = 1.0;
- for (unsigned int kk=0; kk<_nt; kk++)
- {
- //p_un *= ( 1.0 - _stab.at(_di).at(kk) );
- p_un *= ( 1.0 - _stab(_di,kk) );
- }
- p_ij *= ( 1 - p_un );
- }
- return p_ij;
-}
-
-void AssociationNode::normalizeNodeProbs()
-{
- double pSum = 0;
- std::list<AssociationNode>::iterator it;
-
- for(it = node_list_.begin(); it != node_list_.end(); it++)
- pSum += it->getNodeProb();
-
- for(it = node_list_.begin(); it != node_list_.end(); it++)
- {
- it->setNodeProb(it->getNodeProb()/pSum);
- if (!isTerminus())
- it->normalizeNodeProbs();
- }
-}
-
-void AssociationNode::computeTreeProb(const double & _up_prob, std::list<AssociationNode*> & _tn_list)
-{
- std::list<AssociationNode>::iterator it;
-
- //compute joint probability
- tree_prob_ = _up_prob * node_prob_;
-
- //if terminus node, we have to add it to the terminus node list
- if ( isTerminus() )
- {
- _tn_list.push_back( &(*this) );
- }
- else //otherwise carry on recursivity
- {
- for(it = node_list_.begin(); it != node_list_.end(); it++)
- it->computeTreeProb(tree_prob_, _tn_list);
- }
-}
-
-//void AssociationNode::growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const std::vector< std::vector<double> > & _stab, std::vector<unsigned int> & _excluded)
-void AssociationNode::growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const Matrixx<double> & _stab, std::vector<unsigned int> & _excluded)
-{
- unsigned int tar_j; //target index (not target id!)
- double p_ij;//probability that detection i comes from target j
-
- //Recursive growing loop
- for (tar_j=0; tar_j<_nt+1; tar_j++) //for each target, including target void
- {
- //Carry on growing if target is void OR if target is not at excluded vector
- if ( (tar_j == _nt) || ( std::find(_excluded.begin(), _excluded.end(), tar_j) == _excluded.end() ) )
- {
- //compute probability from the score table
- p_ij = computeNodeProb(_nd, _nt, _det_i, tar_j, _stab); //std::cout << __LINE__ << ": p_ij: " << p_ij << std::endl;
-
- //Create node if prob is relevant, and carry on recursivity. Otherwise this current branch stops growing
- if (p_ij > PROB_ZERO_)
- {
- node_list_.push_back(AssociationNode(_det_i, tar_j, p_ij, this));
- if (_det_i+1 < _nd ) //check if there is more detections to carry on recursivity
- {
- _excluded.push_back(tar_j);//push back target to excluded vector
- node_list_.back().growTree(_nd, _nt, _det_i+1, _stab, _excluded); //recursivity
- _excluded.pop_back();//pop back target to excluded vector
- }
- }
- }
- }
-}
-
-void AssociationNode::destroyTree()
-{
- node_list_.clear();
-}
-
-void AssociationNode::printNode() const
-{
- std::cout << "D" << det_idx_ << ",T" << tar_idx_ << ", np=" << node_prob_ << ", tp=" << tree_prob_ << std::endl;
- //std::cout << "D" << det_idx_ << ",T" << tar_idx_ << ", np=" << node_prob_ << ", tp=" << tree_prob_ << ", this=" << this << ", up_ptr=" << up_node_ptr_ << std::endl;
-}
-
-void AssociationNode::printTree(const unsigned int _ntabs)
-{
- std::list<AssociationNode>::iterator it;
-
- for(unsigned int ii=0; ii<_ntabs; ii++) std::cout << "\t";
- printNode();
-
- //for (unsigned int ii=0; ii<node_list_.size(); ii++)
- // node_list_.at(ii).printTree(_ntabs+1);
- for(it = node_list_.begin(); it != node_list_.end(); it++) it->printTree(_ntabs+1);
-}
diff --git a/src/association/association_solver.cpp b/src/association/association_solver.cpp
deleted file mode 100644
index f640efa3e3c9b93b4144ab31d068ccdc23a71842..0000000000000000000000000000000000000000
--- a/src/association/association_solver.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-
-#include "base/association/association_solver.h"
-
-namespace wolf
-{
-
-AssociationSolver::AssociationSolver() :
- nd_(0),
- nt_(0)
-{
- //
-}
-
-AssociationSolver::~AssociationSolver()
-{
- //
-}
-
-unsigned int AssociationSolver::numDetections()
-{
- return nd_;
-}
-
-unsigned int AssociationSolver::numTargets()
-{
- return nt_;
-}
-
-void AssociationSolver::setScore(const unsigned int _det_i, const unsigned int _tar_j, const double _m_ij)
-{
- scores_(_det_i,_tar_j) = _m_ij;
-}
-
-double AssociationSolver::getScore(const unsigned int _det_i, const unsigned int _tar_j)
-{
- return scores_(_det_i,_tar_j);
-}
-
-void AssociationSolver::printScoreTable() const
-{
- scores_.print();
-}
-
-} //namespace wolf
diff --git a/src/association/association_tree.cpp b/src/association/association_tree.cpp
deleted file mode 100644
index b425f8e5db4a5392d303d87b06c7b65ae2c8c822..0000000000000000000000000000000000000000
--- a/src/association/association_tree.cpp
+++ /dev/null
@@ -1,186 +0,0 @@
-
-#include "base/association/association_tree.h"
-
-namespace wolf
-{
-
-AssociationTree::AssociationTree() :
- AssociationSolver(),
- root_(0,0,1, NULL, true)
-{
- //
-}
-
-AssociationTree::~AssociationTree()
-{
- //
-}
-
-void AssociationTree::reset()
-{
- nd_ = 0;
- nt_ = 0;
- scores_.clear();
- terminus_node_list_.clear();
- root_.destroyTree();
-}
-
-void AssociationTree::resize(const unsigned int _n_det, const unsigned int _n_tar)
-{
- nd_ = _n_det;
- nt_ = _n_tar;
- scores_.resize(nd_,nt_+1); //"+1" to account for void target, which manages unassociated detections
-}
-
-
-void AssociationTree::growTree()
-{
- std::vector<unsigned int> ex_vec;
-
- if ( nd_ != 0 ) //check if detections
- root_.growTree(nd_, nt_, 0,scores_, ex_vec);
-}
-
-void AssociationTree::computeTree()
-{
- if ( nd_ != 0 ) //check if detections
- root_.computeTreeProb(1., terminus_node_list_);
-}
-
-void AssociationTree::normalizeTree()
-{
- root_.normalizeNodeProbs();
-}
-
-void AssociationTree::chooseBestTerminus(std::list<AssociationNode*>::iterator & _best_node)
-{
- std::list<AssociationNode*>::iterator it;
- double bestProb = 0.;
- //double sum = 0;
-
- for (it = terminus_node_list_.begin(); it != terminus_node_list_.end(); it++)
- {
- if ( (*it)->getTreeProb() > bestProb )
- {
- _best_node = it;
- bestProb = (*it)->getTreeProb();
- }
-
- //debugging
- //sum += (*it)->getTreeProb();
- }
-}
-
-void AssociationTree::solve(std::map<unsigned int, unsigned int> & _pairs, std::vector<bool> & _associated_mask)
-{
- std::list<AssociationNode*>::iterator best_node;
-// bool rootReached = false;
- AssociationNode *anPtr;
-
- //grows tree exploring all likely hypothesis
- growTree();
-
- //computes tree probs
- computeTree();
-
- //normalizes tree probs
- normalizeTree();
-
- //if terminus_node_list_ is empty exit withou pairing
- if ( terminus_node_list_.empty() ) return;
-
- //choose best node based on best tree probability
- chooseBestTerminus(best_node);
-
- //resize _associated_mask and resets it to false
- _associated_mask.resize(nd_,false);
-
- //set pairs
- anPtr = *best_node; //init pointer
-// int ii=0;
- while( ! anPtr->isRoot() ) //set pairs
- {
-// if ( anPtr->getTargetIndex() == nt_) //detection with void target -> unassociated detection
-// {
-// _unassoc.push_back(anPtr->getDetectionIndex());
-// }
-// else
-// {
-// _pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
-// }
- if ( anPtr->getTargetIndex() < nt_ ) //association pair
- {
- _associated_mask.at(anPtr->getDetectionIndex()) = true;
- _pairs[anPtr->getDetectionIndex()] = anPtr->getTargetIndex();
- }
- anPtr = anPtr->upNode();
- }
-}
-
-void AssociationTree::solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask)
-{
- std::list<AssociationNode*>::iterator best_node;
-// bool rootReached = false;
- AssociationNode *anPtr;
-
- //grows tree exploring all likely hypothesis
- growTree();
-
- //computes tree probs
- computeTree();
-
- //normalizes tree probs
- normalizeTree();
-
- //if terminus_node_list_ is empty exit withou pairing
- if ( terminus_node_list_.empty() ) return;
-
- //choose best node based on best tree probability
- chooseBestTerminus(best_node);
-
- //resize _associated_mask and resets it to false
- _associated_mask.resize(nd_,false);
-
- //set pairs
- anPtr = *best_node; //init pointer
-// int ii=0;
- while( ! anPtr->isRoot() ) //set pairs
- {
-// if ( anPtr->getTargetIndex() == nt_) //detection with void target -> unassociated detection
-// {
-// _unassoc.push_back(anPtr->getDetectionIndex());
-// }
-// else
-// {
-// _pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
-// }
- if ( anPtr->getTargetIndex() < nt_ ) //association pair
- {
- _associated_mask.at(anPtr->getDetectionIndex()) = true;
- _pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
- }
- anPtr = anPtr->upNode();
- }
-}
-
-void AssociationTree::printTree()
-{
- if ( scores_.size() != 0 )
- {
- std::cout << "Nd: " << nd_ << "; Nt: " << nt_ << std::endl;
- root_.printTree();
- }
-}
-
-void AssociationTree::printTerminusNodes()
-{
- std::list<AssociationNode*>::iterator it;
- unsigned int ii;
-
- for (it = terminus_node_list_.begin(), ii=0; it != terminus_node_list_.end(); it++, ii++)
- {
- (*it)->printNode();
- }
-}
-
-} // namespace wolf
diff --git a/src/capture/CMakeLists.txt b/src/capture/CMakeLists.txt
deleted file mode 100644
index 57f34f813a42268bc2600eb53e65ba7b7f73f2ca..0000000000000000000000000000000000000000
--- a/src/capture/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.h
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.h
- )
-
-SET(SRCS_CAPTURE ${SRCS_CAPTURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.cpp
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_CAPTURE ${HDRS_CAPTURE} PARENT_SCOPE)
-SET(SRCS_CAPTURE ${SRCS_CAPTURE} PARENT_SCOPE)
diff --git a/src/capture/capture_GPS.cpp b/src/capture/capture_GPS.cpp
deleted file mode 100644
index 5c6520810890c564502bbb297705e96413289fb4..0000000000000000000000000000000000000000
--- a/src/capture/capture_GPS.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "base/capture/capture_GPS.h"
-
-namespace wolf {
-
-CaptureGPS::CaptureGPS(const TimeStamp &_ts, SensorBasePtr _sensor_ptr, rawgpsutils::SatellitesObs &_obs) :
- CaptureBase("GPS", _ts, _sensor_ptr),
- obs_(_obs)
-{
- //
-// std::cout << "CaptureGPS constructor.\t\tReceived " << obs_.measurements_.size() << " meas" << std::endl;
-// std::cout << obs_.toString() << std::endl;
-}
-
-CaptureGPS::~CaptureGPS()
-{
- //std::cout << "deleting CaptureGPS " << id() << std::endl;
-}
-
-rawgpsutils::SatellitesObs &CaptureGPS::getData()
-{
- return obs_;
-}
-
-} //namespace wolf
diff --git a/src/capture/capture_GPS_fix.cpp b/src/capture/capture_GPS_fix.cpp
deleted file mode 100644
index ccf9b8631bd0759448ec6c20da7b00b9f534087d..0000000000000000000000000000000000000000
--- a/src/capture/capture_GPS_fix.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "base/capture/capture_GPS_fix.h"
-
-namespace wolf {
-
-CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data) :
- CaptureBase("GPS FIX", _ts, _sensor_ptr),
- data_(_data)
-{
- //
-}
-
-CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase("GPS FIX", _ts, _sensor_ptr),
- data_(_data),
- data_covariance_(_data_covariance)
-{
- //
-}
-
-CaptureGPSFix::~CaptureGPSFix()
-{
- //std::cout << "Destroying GPS fix capture...\n";
-}
-
-void CaptureGPSFix::process()
-{
- // EXTRACT AND ADD FEATURES
- addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
-
- // ADD CONSTRAINT
- getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
-}
-
-} //namespace wolf
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp
deleted file mode 100644
index 6dbc07d59d21a3b33051d270825be9da060366c7..0000000000000000000000000000000000000000
--- a/src/capture/capture_IMU.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "base/capture/capture_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/state_quaternion.h"
-
-namespace wolf {
-
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _acc_gyro_data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr) :
- CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(6, false))
-{
- //
-}
-
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _acc_gyro_data,
- const Eigen::MatrixXs& _data_cov,
- const Vector6s& _bias,
- FrameBasePtr _origin_frame_ptr) :
- CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(_bias, false))
-{
- //
-}
-
-CaptureIMU::~CaptureIMU()
-{
- //
-}
-
-} //namespace wolf
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
deleted file mode 100644
index 003178c58cd7a4a85d1f86d74172c6c59302f6f6..0000000000000000000000000000000000000000
--- a/src/capture/capture_base.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-#include "base/capture/capture_base.h"
-#include "base/sensor/sensor_base.h"
-
-namespace wolf{
-
-using namespace Eigen;
-
-unsigned int CaptureBase::capture_id_count_ = 0;
-
-CaptureBase::CaptureBase(const std::string& _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- NodeBase("CAPTURE", _type),
- frame_ptr_(), // nullptr
- sensor_ptr_(_sensor_ptr),
- state_block_vec_(3),
- calib_size_(0),
- capture_id_(++capture_id_count_),
- time_stamp_(_ts)
-{
- if (_sensor_ptr)
- {
-
- if (_sensor_ptr->isExtrinsicDynamic())
- {
- assert(_p_ptr && "Pointer to dynamic position params is null!");
- assert(_o_ptr && "Pointer to dynamic orientation params is null!");
- // assign to Capture's members
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- }
- else if (_p_ptr || _o_ptr)
- {
- WOLF_ERROR("Provided dynamic sensor extrinsics but the sensor extrinsics are static");
- }
-
- if (_sensor_ptr->isIntrinsicDynamic())
- {
- assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
- // assign to Capture's member
- state_block_vec_[2] = _intr_ptr;
- }
- else if (_intr_ptr)
- {
- WOLF_ERROR("Provided dynamic sensor intrinsics but the sensor intrinsics are static");
- }
-
- getSensor()->setHasCapture();
- }
- else if (_p_ptr || _o_ptr || _intr_ptr)
- {
- WOLF_ERROR("Provided sensor parameters but no sensor pointer");
- }
- updateCalibSize();
-
- //WOLF_DEBUG("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
-}
-
-CaptureBase::~CaptureBase()
-{
- removeStateBlocks();
-}
-
-void CaptureBase::remove()
-{
- if (!is_removing_)
- {
- is_removing_ = true;
- CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
-
- // Remove State Blocks
- removeStateBlocks();
-
- // remove from upstream
- FrameBasePtr F = frame_ptr_.lock();
- if (F)
- {
- F->getCaptureList().remove(this_C);
- if (F->getCaptureList().empty() && F->getConstrainedByList().empty())
- F->remove(); // remove upstream
- }
-
- // remove downstream
- while (!feature_list_.empty())
- {
- feature_list_.front()->remove(); // remove downstream
- }
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(); // remove constrained by
- }
- }
-}
-
-FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
-{
- //std::cout << "Adding feature" << std::endl;
- feature_list_.push_back(_ft_ptr);
- _ft_ptr->setCapture(shared_from_this());
- _ft_ptr->setProblem(getProblem());
- return _ft_ptr;
-}
-
-void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
-{
- for (FeatureBasePtr feature_ptr : _new_ft_list)
- {
- feature_ptr->setCapture(shared_from_this());
- if (getProblem() != nullptr)
- feature_ptr->setProblem(getProblem());
- }
- feature_list_.splice(feature_list_.end(), _new_ft_list);
-}
-
-void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
-{
- for (auto f_ptr : getFeatureList())
- f_ptr->getFactorList(_fac_list);
-}
-
-FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setCaptureOther(shared_from_this());
- return _fac_ptr;
-}
-
-StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
-{
- if (getSensor())
- {
- if (_i < 2) // _i == 0 is position, 1 is orientation, 2 and onwards are intrinsics
- if (getSensor()->extrinsicsInCaptures())
- {
- assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
- }
- else
- return getSensor()->getStateBlockPtrStatic(_i);
-
- else // 2 and onwards are intrinsics
- if (getSensor()->intrinsicsInCaptures())
- {
- assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
- }
- else
- return getSensor()->getStateBlockPtrStatic(_i);
- }
- else // No sensor associated: assume sensor params are here
- {
- assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
- }
-}
-
-void CaptureBase::removeStateBlocks()
-{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlock(i, nullptr);
- }
- }
-}
-
-void CaptureBase::fix()
-{
- for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::unfix()
-{
- for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::fixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::unfixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::fixIntrinsics()
-{
- for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::unfixIntrinsics()
-{
- for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
- {
- for (auto sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
-}
-
-SizeEigen CaptureBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sb = getStateBlock(i);
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXs CaptureBase::getCalibration() const
-{
- Eigen::VectorXs calib(calib_size_);
- SizeEigen index = 0;
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
- {
- auto sb = getStateBlock(i);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
-void CaptureBase::setCalibration(const VectorXs& _calib)
-{
- updateCalibSize();
- assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
- SizeEigen index = 0;
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
- {
- auto sb = getStateBlock(i);
- if (sb && !sb->isFixed())
- {
- sb->setState(_calib.segment(index, sb->getSize()));
- index += sb->getSize();
- }
- }
-}
-
-} // namespace wolf
-
diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp
index b66af7277c9e878bde75cfde89f7cc0dc6d838e3..34f76594d5cff7256726e9eb663bf6ca0b1f0c1d 100644
--- a/src/capture/capture_gnss_fix.cpp
+++ b/src/capture/capture_gnss_fix.cpp
@@ -1,4 +1,4 @@
-#include "base/capture/capture_gnss_fix.h"
+#include "gnss/capture/capture_gnss_fix.h"
namespace wolf {
diff --git a/src/capture/capture_image.cpp b/src/capture/capture_image.cpp
deleted file mode 100644
index 1d8ac8b8d7b5959173948948e89d403470d34d45..0000000000000000000000000000000000000000
--- a/src/capture/capture_image.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-#include "base/capture/capture_image.h"
-
-namespace wolf {
-
-CaptureImage::CaptureImage(const TimeStamp& _ts, SensorCameraPtr _camera_ptr, cv::Mat _data_cv) :
- CaptureBase("IMAGE", _ts, _camera_ptr),
- frame_(_data_cv),
- grid_features_(nullptr),
- keypoints_(KeyPointVector()),
- descriptors_(cv::Mat()),
- matches_from_precedent_(DMatchVector()),
- matches_normalized_scores_(std::vector<Scalar>()),
- map_index_to_next_(std::map<int, int>())
-{
- //
-}
-
-CaptureImage::~CaptureImage()
-{
- //
-}
-
-const cv::Mat& CaptureImage::getImage() const
-{
- return frame_.getImage();
-}
-
-void CaptureImage::setDescriptors(const cv::Mat& _descriptors)
-{
- frame_.setDescriptors(_descriptors);
-}
-
-void CaptureImage::setKeypoints(const std::vector<cv::KeyPoint> &_keypoints)
-{
- frame_.setKeyPoints(_keypoints);
-}
-
-cv::Mat& CaptureImage::getDescriptors()
-{
- return frame_.getDescriptors();
-}
-
-std::vector<cv::KeyPoint>& CaptureImage::getKeypoints()
-{
- return frame_.getKeyPoints();
-}
-
-} // namespace wolf
diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp
deleted file mode 100644
index a2cdd817ebdfd3e73eb0de6d2e4e0183d157ce0a..0000000000000000000000000000000000000000
--- a/src/capture/capture_laser_2D.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "base/capture/capture_laser_2D.h"
-
-namespace wolf {
-
-CaptureLaser2D::CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges) :
- CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2D>(getSensor())), scan_(_ranges)
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
deleted file mode 100644
index 439228aa9b96d647f664241cbbf264cc1390e8eb..0000000000000000000000000000000000000000
--- a/src/capture/capture_motion.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/**
- * \file capture_motion2.cpp
- *
- * Created on: Oct 14, 2017
- * \author: jsola
- */
-
-#include "base/capture/capture_motion.h"
-
-namespace wolf
-{
-
-CaptureMotion::CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr) :
- CaptureBase(_type, _ts, _sensor_ptr),
- data_(_data),
- data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
- buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
- origin_frame_ptr_(_origin_frame_ptr)
-{
- //
-}
-
-CaptureMotion::CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr,
- StateBlockPtr _p_ptr ,
- StateBlockPtr _o_ptr ,
- StateBlockPtr _intr_ptr ) :
- CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
- data_(_data),
- data_cov_(_data_cov),
- buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
- origin_frame_ptr_(_origin_frame_ptr)
-{
- //
-}
-
-CaptureMotion::~CaptureMotion()
-{
- //
-}
-
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
-{
- VectorXs calib_preint = getCalibrationPreint();
- VectorXs delta_preint = getBuffer().get().back().delta_integr_;
- MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_;
- VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
- return delta_corrected;
-}
-
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts)
-{
- VectorXs calib_preint = getBuffer().getCalibrationPreint();
- Motion motion = getBuffer().getMotion(_ts);
- VectorXs delta_preint = motion.delta_integr_;
- MatrixXs jac_calib = motion.jacobian_calib_;
- VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
- return delta_corrected;
-}
-
-}
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
deleted file mode 100644
index 99b6e42812740e045613186ea1079a13d5b2f416..0000000000000000000000000000000000000000
--- a/src/capture/capture_odom_2D.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * capture_odom_2D.cpp
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
-
-#include "base/capture/capture_odom_2D.h"
-
-namespace wolf
-{
-
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- FrameBasePtr _origin_frame_ptr):
- CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _origin_frame_ptr)
-{
- //
-}
-
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr):
- CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _origin_frame_ptr)
-{
- //
-}
-
-CaptureOdom2D::~CaptureOdom2D()
-{
- //
-}
-
-} /* namespace wolf */
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp
deleted file mode 100644
index 26f4e6d6fd3697b4099084e327699bda8bb23150..0000000000000000000000000000000000000000
--- a/src/capture/capture_odom_3D.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * capture_odom_3D.cpp
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
-
-#include "base/capture/capture_odom_3D.h"
-
-namespace wolf
-{
-
-CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- FrameBasePtr _origin_frame_ptr):
- CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _origin_frame_ptr)
-{
- //
-}
-
-CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr):
- CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _origin_frame_ptr)
-{
- //
-}
-
-CaptureOdom3D::~CaptureOdom3D()
-{
- //
-}
-
-Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
-{
- VectorXs delta(7);
- delta.head(3) = _delta.head(3) + _delta_error.head(3);
- delta.tail(4) = (Quaternions(_delta.data()+3) * exp_q(_delta_error.tail(3))).coeffs();
- return delta;
-}
-
-} /* namespace wolf */
-
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
deleted file mode 100644
index 4f5cf88a4670649f8e61df9423353851797d424b..0000000000000000000000000000000000000000
--- a/src/capture/capture_pose.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "base/capture/capture_pose.h"
-
-namespace wolf{
-
-CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase("POSE", _ts, _sensor_ptr),
- data_(_data),
- data_covariance_(_data_covariance)
-{
- //
-}
-
-CapturePose::~CapturePose()
-{
- //
-}
-
-void CapturePose::emplaceFeatureAndFactor()
-{
- // Emplace feature
- FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
- addFeature(feature_pose);
-
- // Emplace factor
- if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
- feature_pose->addFactor(std::make_shared<FactorPose2D>(feature_pose));
- else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
- feature_pose->addFactor(std::make_shared<FactorPose3D>(feature_pose));
- else
- throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
-}
-
-} // namespace wolf
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
deleted file mode 100644
index 9c4e7675f67d8bcbc11c8b5a2d796a281bf20505..0000000000000000000000000000000000000000
--- a/src/capture/capture_velocity.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-#include "base/capture/capture_velocity.h"
-
-namespace wolf {
-
-CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr) :
- CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity,
- _delta_size, _delta_cov_size, _origin_frame_ptr)
-{
- //
-}
-
-CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
- const Eigen::MatrixXs& _velocity_cov,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- FrameBasePtr _origin_frame_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity, _velocity_cov,
- _delta_size, _delta_cov_size, _origin_frame_ptr,
- _p_ptr, _o_ptr, _intr_ptr)
-{
- //
-}
-
-const Eigen::VectorXs& CaptureVelocity::getVelocity() const
-{
- return getData();
-}
-
-const Eigen::MatrixXs& CaptureVelocity::getVelocityCov() const
-{
- return getDataCovariance();
-}
-
-} // namespace wolf
diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp
deleted file mode 100644
index 44bf8778833b24e0df94bd11ae3985df2d3bd46a..0000000000000000000000000000000000000000
--- a/src/capture/capture_void.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "base/capture/capture_void.h"
-
-namespace wolf {
-
-CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) :
- CaptureBase("VOID", _ts, _sensor_ptr)
-{
- //
-}
-
-CaptureVoid::~CaptureVoid()
-{
- //std::cout << "deleting CaptureVoid " << id() << std::endl;
-}
-
-} // namespace wolf
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
deleted file mode 100644
index 7b11d9c8de9e6189be3c9b6a4bef5864b392e9f6..0000000000000000000000000000000000000000
--- a/src/capture/capture_wheel_joint_position.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-#include "base/capture/capture_wheel_joint_position.h"
-#include "base/sensor/sensor_diff_drive.h"
-#include "base/rotations.h"
-
-namespace wolf {
-
-CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _positions,
- FrameBasePtr _origin_frame_ptr) :
- CaptureMotion("WHEEL JOINT POSITION", _ts, _sensor_ptr, _positions, Eigen::Matrix2s::Zero(), 3, 3,
- _origin_frame_ptr/*, nullptr, nullptr, std::make_shared<StateBlock>(3, false)*/)
-{
-//
-
- const IntrinsicsDiffDrive intrinsics =
- *(std::static_pointer_cast<SensorDiffDrive>(getSensor())->getIntrinsics());
-
- setDataCovariance(computeWheelJointPositionCov(getPositions(),
- intrinsics.left_resolution_,
- intrinsics.right_resolution_,
- intrinsics.left_gain_,
- intrinsics.right_gain_));
-}
-
-CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _positions,
- const Eigen::MatrixXs& _positions_cov,
- FrameBasePtr _origin_frame_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- CaptureMotion("WHEEL JOINT POSITION", _ts, _sensor_ptr, _positions, _positions_cov, 3, 3,
- _origin_frame_ptr, _p_ptr, _o_ptr, _intr_ptr)
-{
-//
-}
-
-Eigen::VectorXs CaptureWheelJointPosition::correctDelta(const VectorXs& _delta,
- const VectorXs& _delta_error)
-{
- Vector3s delta = _delta + _delta_error;
- delta(2) = pi2pi(delta(2));
- return delta;
-}
-
-const Eigen::VectorXs& CaptureWheelJointPosition::getPositions() const
-{
- return getData();
-}
-
-const Eigen::MatrixXs& CaptureWheelJointPosition::getPositionsCov() const
-{
- return getDataCovariance();
-}
-
-} // namespace wolf
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
deleted file mode 100644
index ff873304a360fe1da866ebdab63ffa44d08bc386..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ /dev/null
@@ -1,433 +0,0 @@
-#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
-#include "base/landmark/landmark_base.h"
-#include "base/make_unique.h"
-
-namespace wolf {
-
-CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
- const ceres::Solver::Options& _ceres_options) :
- SolverManager(_wolf_problem),
- ceres_options_(_ceres_options)
-{
- ceres::Covariance::Options covariance_options;
-#if CERES_VERSION_MINOR >= 13
- covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
- covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
-#elif CERES_VERSION_MINOR >= 10
- covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
-#else
- covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
-#endif
- covariance_options.num_threads = 1;
- covariance_options.apply_loss_function = false;
- //covariance_options.null_space_rank = -1;
- covariance_ = wolf::make_unique<ceres::Covariance>(covariance_options);
-
- ceres::Problem::Options problem_options;
- problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
- problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-
- ceres_problem_ = wolf::make_unique<ceres::Problem>(problem_options);
-}
-
-CeresManager::~CeresManager()
-{
- while (!fac_2_residual_idx_.empty())
- removeFactor(fac_2_residual_idx_.begin()->first);
-}
-
-std::string CeresManager::solveImpl(const ReportVerbosity report_level)
-{
- // Check
- #ifdef _WOLF_DEBUG
- check();
- #endif
-
- // run Ceres Solver
- ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_);
-
- std::string report;
-
- //return report
- if (report_level == ReportVerbosity::BRIEF)
- report = summary_.BriefReport();
- else if (report_level == ReportVerbosity::FULL)
- report = summary_.FullReport();
-
- return report;
-}
-
-void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
-{
- // update problem
- update();
-
- // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
- wolf_problem_->clearCovariance();
-
- // CREATE DESIRED COVARIANCES LIST
- std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
- std::vector<std::pair<const double*, const double*>> double_pairs;
-
- switch (_blocks)
- {
- case CovarianceBlocksToBeComputed::ALL:
- {
- // first create a vector containing all state blocks
- std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
- //frame state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
- if (fr_ptr->isKey())
- for (auto sb : fr_ptr->getStateBlockVec())
- if (sb)
- all_state_blocks.push_back(sb);
-
- // landmark state blocks
- for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- {
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
- all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
- }
- // double loop all against all (without repetitions)
- for (unsigned int i = 0; i < all_state_blocks.size(); i++)
- for (unsigned int j = i; j < all_state_blocks.size(); j++)
- {
- state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
- getAssociatedMemBlockPtr(all_state_blocks[j]));
- }
- break;
- }
- case CovarianceBlocksToBeComputed::ALL_MARGINALS:
- {
- // first create a vector containing all state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
- if (fr_ptr->isKey())
- for (auto sb : fr_ptr->getStateBlockVec())
- if (sb)
- for(auto sb2 : fr_ptr->getStateBlockVec())
- if (sb2)
- {
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
- if (sb == sb2)
- break;
- }
-
- // landmark state blocks
- for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- for (auto sb : l_ptr->getUsedStateBlockVec())
- for(auto sb2 : l_ptr->getUsedStateBlockVec())
- {
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
- if (sb == sb2)
- break;
- }
- // // loop all marginals (PO marginals)
- // for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
- // {
- // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
- // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
- // state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
- //
- // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i]->get());
- // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i+1]->get());
- // double_pairs.emplace_back(all_state_blocks[2*i+1]->get(),all_state_blocks[2*i+1]->get());
- // }
- break;
- }
- case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
- {
- //robot-robot
- auto last_key_frame = wolf_problem_->getLastKeyFrame();
-
- state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
- state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
- state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO());
-
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
- getAssociatedMemBlockPtr(last_key_frame->getP()));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
- getAssociatedMemBlockPtr(last_key_frame->getO()));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
- getAssociatedMemBlockPtr(last_key_frame->getO()));
-
- // landmarks
- std::vector<StateBlockPtr> landmark_state_blocks;
- for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- {
- // load state blocks vector
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
-
- for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
- {
- // robot - landmark
- state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
- state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
- getAssociatedMemBlockPtr((*state_it)));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
- getAssociatedMemBlockPtr((*state_it)));
-
- // landmark marginal
- for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
- {
- state_block_pairs.emplace_back(*state_it, *next_state_it);
- double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
- getAssociatedMemBlockPtr((*next_state_it)));
- }
- }
- }
- break;
- }
- }
- //std::cout << "pairs... " << double_pairs.size() << std::endl;
-
- // COMPUTE DESIRED COVARIANCES
- if (covariance_->Compute(double_pairs, ceres_problem_.get()))
- {
- // STORE DESIRED COVARIANCES
- for (unsigned int i = 0; i < double_pairs.size(); i++)
- {
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
- covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
- wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
- //std::cout << "getted covariance " << std::endl << cov << std::endl;
- }
- }
- else
- std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
-}
-
-void CeresManager::computeCovariances(const StateBlockPtrList& st_list)
-{
- //std::cout << "CeresManager: computing covariances..." << std::endl;
-
- // update problem
- update();
-
- // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
- wolf_problem_->clearCovariance();
-
- // CREATE DESIRED COVARIANCES LIST
- std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
- std::vector<std::pair<const double*, const double*>> double_pairs;
-
- // double loop all against all (without repetitions)
- for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
- for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
- {
- state_block_pairs.emplace_back(*st_it1, *st_it2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
- getAssociatedMemBlockPtr((*st_it2)));
- }
-
- //std::cout << "pairs... " << double_pairs.size() << std::endl;
-
- // COMPUTE DESIRED COVARIANCES
- if (covariance_->Compute(double_pairs, ceres_problem_.get()))
- // STORE DESIRED COVARIANCES
- for (unsigned int i = 0; i < double_pairs.size(); i++)
- {
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
- covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
- wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
- //std::cout << "getted covariance " << std::endl << cov << std::endl;
- }
- else
- std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
-}
-
-void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
-{
- assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
-
- auto cost_func_ptr = createCostFunction(fac_ptr);
- fac_2_costfunction_[fac_ptr] = cost_func_ptr;
-
- std::vector<Scalar*> res_block_mem;
- res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
- for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
- {
- res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
- }
-
- auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
-
- //std::cout << "Added residual block corresponding to constraint " << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl;
-
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
- assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map");
-
- fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
- loss_func_ptr,
- res_block_mem);
-
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
-}
-
-void CeresManager::removeFactor(const FactorBasePtr& _fac_ptr)
-{
- assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
-
- ceres_problem_->RemoveResidualBlock(fac_2_residual_idx_[_fac_ptr]);
- fac_2_residual_idx_.erase(_fac_ptr);
- fac_2_costfunction_.erase(_fac_ptr);
-
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
-}
-
-void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
-{
- // std::cout << "CeresManager::addStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
- ceres::LocalParameterization* local_parametrization_ptr = nullptr;
-
- if (state_ptr->hasLocalParametrization() &&
- state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
- {
- auto p = state_blocks_local_param_.emplace(state_ptr,
- std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
-
- local_parametrization_ptr = p.first->second.get();
- }
-
- ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
- state_ptr->getSize(),
- local_parametrization_ptr);
-
- if (state_ptr->isFixed())
- ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
-
- updateStateBlockStatus(state_ptr);
-}
-
-void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
-{
- //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
- assert(state_ptr);
- ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
- state_blocks_local_param_.erase(state_ptr);
-}
-
-void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
-{
- assert(state_ptr != nullptr);
- // std::cout << "CeresManager::updateStateBlockStatus " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
- if (state_ptr->isFixed())
- ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
- else
- ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
-}
-
-void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
-{
- assert(state_ptr != nullptr);
-
- /* in ceres the easiest way to update (add or remove) a local parameterization
- * of a state block (parameter block in ceres) is remove & add:
- * - the state block: The associated memory block (that identified the parameter_block) is and MUST be the same
- * - all involved factors (residual_blocks in ceres)
- */
-
- // get all involved factors
- FactorBasePtrList involved_factors;
- for (auto pair : fac_2_costfunction_)
- for (const StateBlockPtr& st : pair.first->getStateBlockPtrVector())
- if (st == state_ptr)
- {
- // store
- involved_factors.push_back(pair.first);
- break;
- }
-
- // Remove all involved factors (it does not remove any parameter block)
- for (auto fac : involved_factors)
- removeFactor(fac);
-
- // Remove state block (it removes all involved residual blocks but they just were removed)
- removeStateBlock(state_ptr);
-
- // Add state block
- addStateBlock(state_ptr);
-
- // Add all involved factors
- for (auto fac : involved_factors)
- addFactor(fac);
-}
-
-bool CeresManager::hasConverged()
-{
- return summary_.termination_type == ceres::CONVERGENCE;
-}
-
-SizeStd CeresManager::iterations()
-{
- return summary_.iterations.size();
-}
-
-Scalar CeresManager::initialCost()
-{
- return Scalar(summary_.initial_cost);
-}
-
-Scalar CeresManager::finalCost()
-{
- return Scalar(summary_.final_cost);
-}
-
-ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fac_ptr)
-{
- assert(_fac_ptr != nullptr);
-
- // analitic & autodiff jacobian
- if (_fac_ptr->getJacobianMethod() == JAC_ANALYTIC || _fac_ptr->getJacobianMethod() == JAC_AUTO)
- return std::make_shared<CostFunctionWrapper>(_fac_ptr);
-
- // numeric jacobian
- else if (_fac_ptr->getJacobianMethod() == JAC_NUMERIC)
- return createNumericDiffCostFunction(_fac_ptr);
-
- else
- throw std::invalid_argument( "Wrong Jacobian Method!" );
-}
-
-void CeresManager::check()
-{
- // Check numbers
- assert(ceres_problem_->NumResidualBlocks() == fac_2_costfunction_.size());
- assert(ceres_problem_->NumResidualBlocks() == fac_2_residual_idx_.size());
- assert(ceres_problem_->NumParameterBlocks() == state_blocks_.size());
-
- // Check parameter blocks
- for (auto&& state_block_pair : state_blocks_)
- assert(ceres_problem_->HasParameterBlock(state_block_pair.second.data()));
-
- // Check residual blocks
- for (auto&& fac_res_pair : fac_2_residual_idx_)
- {
- // costfunction - residual
- assert(fac_2_costfunction_.find(fac_res_pair.first) != fac_2_costfunction_.end());
- assert(fac_2_costfunction_[fac_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second));
-
- // factor - residual
- assert(fac_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor());
-
- // parameter blocks - state blocks
- std::vector<Scalar*> param_blocks;
- ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks);
- auto i = 0;
- for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
- {
- assert(getAssociatedMemBlockPtr(st) == param_blocks[i]);
- i++;
- }
- }
-}
-
-} // namespace wolf
-
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp
deleted file mode 100644
index 5cade15532096a249e66df8da32287fa1ecbf797..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/local_parametrization_wrapper.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "base/ceres_wrapper/local_parametrization_wrapper.h"
-
-namespace wolf {
-
-bool LocalParametrizationWrapper::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
-{
- Eigen::Map<const Eigen::VectorXs> x_raw_map((Scalar*)x_raw, GlobalSize());
- Eigen::Map<const Eigen::VectorXs> delta_raw_map((Scalar*)delta_raw, LocalSize());
- Eigen::Map<Eigen::VectorXs> x_plus_map((Scalar*)x_plus_delta_raw, GlobalSize());
- return local_parametrization_ptr_->plus(x_raw_map, delta_raw_map, x_plus_map);
-};
-
-bool LocalParametrizationWrapper::ComputeJacobian(const double* x, double* jacobian) const
-{
- Eigen::Map<const Eigen::VectorXs> x_map((Scalar*)x, GlobalSize());
- Eigen::Map<Eigen::MatrixXs> jacobian_map((Scalar*)jacobian, GlobalSize(), LocalSize());
- return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map);
-};
-
-} // namespace wolf
-
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
deleted file mode 100644
index f2e40fac0122c6eb1599cbe27a62659f439a89f0..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/qr_manager.cpp
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
- * qr_manager.cpp
- *
- * Created on: Jun 7, 2017
- * Author: jvallve
- */
-
-#include "qr_manager.h"
-
-namespace wolf {
-
-QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) :
- SolverManager(_wolf_problem),
- A_(), // empty matrix
- b_(),
- any_state_block_removed_(false),
- new_state_blocks_(0),
- N_batch_(_N_batch),
- pending_changes_(false)
-{
- //
-}
-
-QRManager::~QRManager()
-{
- sb_2_col_.clear();
- fac_2_row_.clear();
-}
-
-std::string QRManager::solve(const unsigned int& _report_level)
-{
- // check for update notifications
- update();
-
- // Decomposition
- if (!computeDecomposition())
- return std::string("decomposition failed\n");
-
- // Solve
- Eigen::VectorXs x_incr = solver_.solve(-b_);
- b_.setZero();
-
- // update state blocks
- for (auto sb_pair : sb_2_col_)
- sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false);
-
- if (_report_level == 1)
- return std::string("Success!\n");
- else if (_report_level == 2)
- return std::string("Success!\n");
-
- return std::string();
-}
-
-void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
-{
- // TODO
-}
-
-void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
-{
- //std::cout << "computing covariances.." << std::endl;
- update();
- computeDecomposition();
-
-// std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
-// std::cout << Eigen::MatrixXs(solver_.matrixR()) << std::endl;
-
- covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols()));
-
- Eigen::SparseMatrix<Scalar, Eigen::ColMajor> I(A_.cols(),A_.cols());
- I.setIdentity();
- Eigen::SparseMatrix<Scalar, Eigen::ColMajor> iR = covariance_solver_.solve(I);
- Eigen::MatrixXs Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
-
-// std::cout << "A' A = \n" << Eigen::MatrixXs(A_.transpose() * A_)<< std::endl;
-// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXs(P * iR * iR.transpose() * P.transpose()) << std::endl;
-// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXs(Sigma_full * A_.transpose() * A_) << std::endl;
-// std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl;
-// std::cout << "Sigma = \n" << Sigma_full << std::endl;
-
- // STORE DESIRED COVARIANCES
- for (auto sb_row = _sb_list.begin(); sb_row != _sb_list.end(); sb_row++)
- for (auto sb_col = sb_row; sb_col!=_sb_list.end(); sb_col++)
- {
- //std::cout << "cov state block " << sb_col->get() << std::endl;
- assert(sb_2_col_.find(*sb_col) != sb_2_col_.end() && "state block not found");
- //std::cout << "block: " << sb_2_col_[*sb_row] << "," << sb_2_col_[*sb_col] << std::endl << Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()) << std::endl;
- wolf_problem_->addCovarianceBlock(*sb_row, *sb_col, Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()));
- }
-}
-
-bool QRManager::computeDecomposition()
-{
- if (pending_changes_)
- {
- // Rebuild problem
- if (any_state_block_removed_)
- {
- // rebuild maps
- unsigned int state_size = 0;
- for (auto sb_pair : sb_2_col_)
- {
- sb_2_col_[sb_pair.first] = state_size;
- state_size += sb_pair.first->getSize();
- }
-
- unsigned int meas_size = 0;
- for (auto fac_pair : fac_2_row_)
- {
- fac_2_row_[fac_pair.first] = meas_size;
- meas_size += fac_pair.first->getSize();
- }
-
- // resize and setZero A, b
- A_.resize(meas_size,state_size);
- b_.resize(meas_size);
- }
-
- if (any_state_block_removed_ || new_state_blocks_ >= N_batch_)
- {
- // relinearize all factors
- for (auto fac_pair : fac_2_row_)
- relinearizeFactor(fac_pair.first);
-
- any_state_block_removed_ = false;
- new_state_blocks_ = 0;
- }
-
- // Decomposition
- solver_.compute(A_);
- if (solver_.info() != Eigen::Success)
- return false;
- }
-
- pending_changes_ = false;
-
- return true;
-}
-
-void QRManager::addFactor(FactorBasePtr _fac_ptr)
-{
- //std::cout << "add factor " << _fac_ptr->id() << std::endl;
- assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor");
- fac_2_row_[_fac_ptr] = A_.rows();
- A_.conservativeResize(A_.rows() + _fac_ptr->getSize(), A_.cols());
- b_.conservativeResize(b_.size() + _fac_ptr->getSize());
-
- assert(A_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad A number of rows");
- assert(b_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad b number of rows");
-
- relinearizeFactor(_fac_ptr);
-
- pending_changes_ = true;
-}
-
-void QRManager::removeFactor(FactorBasePtr _fac_ptr)
-{
- //std::cout << "remove factor " << _fac_ptr->id() << std::endl;
- assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor");
- eraseBlockRow(A_, fac_2_row_[_fac_ptr], _fac_ptr->getSize());
- b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()).setZero();
- fac_2_row_.erase(_fac_ptr);
- pending_changes_ = true;
-}
-
-void QRManager::addStateBlock(StateBlockPtr _st_ptr)
-{
- //std::cout << "add state block " << _st_ptr.get() << std::endl;
- assert(sb_2_col_.find(_st_ptr) == sb_2_col_.end() && "adding existing state block");
- sb_2_col_[_st_ptr] = A_.cols();
- A_.conservativeResize(A_.rows(), A_.cols() + _st_ptr->getSize());
-
- new_state_blocks_++;
- pending_changes_ = true;
-}
-
-void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
-{
- //std::cout << "remove state block " << _st_ptr.get() << std::endl;
- assert(sb_2_col_.find(_st_ptr) != sb_2_col_.end() && "removing unknown state block");
- eraseBlockCol(A_, sb_2_col_[_st_ptr], _st_ptr->getSize());
-
- // flag to rebuild problem
- any_state_block_removed_ = true;
- // TODO: insert identity while problem is not re-built?
-
- sb_2_col_.erase(_st_ptr);
- pending_changes_ = true;
-}
-
-void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
-{
- //std::cout << "update state block " << _st_ptr.get() << std::endl;
- if (_st_ptr->isFixed())
- {
- if (sb_2_col_.find(_st_ptr) != sb_2_col_.end())
- removeStateBlock(_st_ptr);
- }
- else
- if (sb_2_col_.find(_st_ptr) == sb_2_col_.end())
- addStateBlock(_st_ptr);
-}
-
-void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
-{
- // evaluate factor
- std::vector<const Scalar*> fac_states_ptr;
- for (auto sb : _fac_ptr->getStateBlockPtrVector())
- fac_states_ptr.push_back(sb->get());
- Eigen::VectorXs residual(_fac_ptr->getSize());
- std::vector<Eigen::MatrixXs> jacobians;
- _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
-
- // Fill jacobians
- Eigen::SparseMatrixs A_block_row(_fac_ptr->getSize(), A_.cols());
- for (auto i = 0; i < jacobians.size(); i++)
- if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
- {
- assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
- assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
- // insert since A_block_row has just been created so it's empty for sure
- insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
- }
- assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
-
- // Fill residual
- b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual;
-}
-
-} /* namespace wolf */
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
deleted file mode 100644
index 06ae2d113c1a4c4a988fa6a684be614c90ed4d9a..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/solver_manager.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-#include "base/solver/solver_manager.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
-#include "base/landmark/landmark_base.h"
-
-namespace wolf {
-
-SolverManager::SolverManager(ProblemPtr _wolf_problem) :
- wolf_problem_(_wolf_problem)
-{
-}
-
-SolverManager::~SolverManager()
-{
-}
-
-void SolverManager::update()
-{
- //std::cout << "SolverManager: updating... " << std::endl;
- //std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl;
- //std::cout << wolf_problem_->getFactorNotificationList().size() << " factor notifications" << std::endl;
-
- // REMOVE CONSTRAINTS
- auto fac_notification_it = wolf_problem_->getFactorNotificationList().begin();
- while ( fac_notification_it != wolf_problem_->getFactorNotificationList().end() )
- if (fac_notification_it->notification_ == REMOVE)
- {
- removeFactor(fac_notification_it->factor_ptr_);
- fac_notification_it = wolf_problem_->getFactorNotificationList().erase(fac_notification_it);
- }
- else
- fac_notification_it++;
-
- // REMOVE STATE BLOCKS
- auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin();
- while ( state_notification_it != wolf_problem_->getStateBlockNotificationList().end() )
- if (state_notification_it->notification_ == REMOVE)
- {
- removeStateBlock(state_notification_it->state_block_ptr_);
- state_notification_it = wolf_problem_->getStateBlockNotificationList().erase(state_notification_it);
- }
- else
- state_notification_it++;
-
- // ADD/UPDATE STATE BLOCKS
- while (!wolf_problem_->getStateBlockNotificationList().empty())
- {
- switch (wolf_problem_->getStateBlockNotificationList().front().notification_)
- {
- case ADD:
- {
- addStateBlock(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
- if (wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_->isFixed())
- updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- case UPDATE:
- {
- updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- default:
- throw std::runtime_error("SolverManager::update: State Block notification must be ADD, UPATE or REMOVE.");
- }
- wolf_problem_->getStateBlockNotificationList().pop_front();
- }
- // ADD CONSTRAINTS
- while (!wolf_problem_->getFactorNotificationList().empty())
- {
- switch (wolf_problem_->getFactorNotificationList().front().notification_)
- {
- case ADD:
- {
- addFactor(wolf_problem_->getFactorNotificationList().front().factor_ptr_);
- break;
- }
- default:
- throw std::runtime_error("SolverManager::update: Factor notification must be ADD or REMOVE.");
- }
- wolf_problem_->getFactorNotificationList().pop_front();
- }
-
- assert(wolf_problem_->getFactorNotificationList().empty() && "wolf problem's factors notification list not empty after update");
- assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
-}
-
-ProblemPtr SolverManager::getProblem()
-{
- return wolf_problem_;
-}
-
-} // namespace wolf
-
diff --git a/src/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
index 2fae213bcb4493647a2e35b963af6c89bdb34dfc..d9eec5b00f275b9ed4fc1fd2f309a73daa8a171e 100644
--- a/src/examples/solver/test_ccolamd.cpp
+++ b/src/examples/solver/test_ccolamd.cpp
@@ -6,7 +6,7 @@
*/
// Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//std includes
#include <cstdlib>
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index 93b39fe6572185da38ef7fccb8cb48b6302bab1c..dc4094304626dda2d5f044576da3cbaed648d377 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -17,7 +17,7 @@
#include <queue>
//Wolf includes
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/factor/factor_base.h"
#include "base/sensor/sensor_laser_2D.h"
#include "wolf_manager.h"
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index 10824d51e65440e25dd4a2e5b93b810d5f0a2372..9174dc94f6d30e66ae586d97ecc53896e0f2522a 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -17,7 +17,7 @@
#include "glog/logging.h"
//Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/processor/processor_tracker_landmark_corner.h"
#include "base/processor/processor_odom_2D.h"
#include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index d4afdc7d00f1805782648bf826159d5ea955d288..d12f491de7ce0895ca301498454e7f3d451bbd4e 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -17,7 +17,7 @@
#include "glog/logging.h"
//Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/processor/processor_tracker_landmark_corner.h"
#include "base/processor/processor_odom_2D.h"
#include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 9e9ca5390a97a42ec61f7ad2a76081fa40e72b5c..136ba285e0d9fb2c62b4801dc75e597dcdd257ff 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -17,7 +17,7 @@
#include "glog/logging.h"
//Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/processor/processor_tracker_landmark_polyline.h"
#include "base/processor/processor_odom_2D.h"
#include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 306c7dfdc0fae360e7ec0d68a763cebf278d4182..23980071f58e0e30bc3eeebef8c6d51106e2adf2 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -6,8 +6,8 @@
*/
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_diff_drive.h"
#include "base/capture/capture_wheel_joint_position.h"
#include "base/processor/processor_diff_drive.h"
diff --git a/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
index 1c01f5e1361a9ee43388a03c39928a405342f244..d24a715cbc8d65f2b86d67e3931b9c1199f22398 100644
--- a/src/examples/test_eigen_quaternion.cpp
+++ b/src/examples/test_eigen_quaternion.cpp
@@ -6,7 +6,7 @@
#include <eigen3/Eigen/Geometry>
//Wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
int main()
{
diff --git a/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp
index b3938285e114b46926da4d5b18255e10e4130be6..2c92e39d12d6ec6962869eece67bdcfed2d5bc08 100644
--- a/src/examples/test_factor_AHP.cpp
+++ b/src/examples/test_factor_AHP.cpp
@@ -1,8 +1,8 @@
#include "base/pinhole_tools.h"
#include "base/landmark/landmark_AHP.h"
#include "base/factor/factor_AHP.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/sensor/sensor_camera.h"
#include "base/capture/capture_image.h"
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index 5dde0e16a573a9b8e4e5fb90d6b65e905dcb18e6..3d0483e9647f9e3203ab3bb89036de55e0f9adc9 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -3,11 +3,11 @@
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
#include "base/capture/capture_pose.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/ceres_wrapper/ceres_manager.h"
//#define DEBUG_RESULTS
diff --git a/src/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
index 621f651866200d68a0b3061cb699c6f56d896954..e095e561d3348cb3ff4445c5f874e8f34ca637e9 100644
--- a/src/examples/test_faramotics_simulation.cpp
+++ b/src/examples/test_faramotics_simulation.cpp
@@ -16,10 +16,10 @@
#include "unistd.h"
// wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/feature/feature_base.h"
#include "base/landmark/landmark_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
//faramotics includes
#include "faramotics/dynamicSceneRender.h"
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index dff0efd51fe4a71d8d9dc22b6630f4b50fa62946..dab6894d7a558cf53502ffc58c6a407a3fa4bbe5 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -7,8 +7,8 @@
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 4415b685cecace3b3aa348d6ca4adfd748219acd..43a54d654c1611e7d9c5a42ef326c382f2dd9566 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -7,8 +7,8 @@
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index e7ed6a9157f104629a2d6f41d4277e4450763672..4b923de89990eac72ffc20df61b13e28dbb76b66 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -2,12 +2,12 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/processor/processor_odom_3D.h"
#include "base/ceres_wrapper/ceres_manager.h"
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index f134ccc124603660a1d687fe5fa3357427677415..e542e18e0ffac35b9c398ba01ab23bd7fc2e13a1 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -2,12 +2,12 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/processor/processor_odom_3D.h"
#include "base/ceres_wrapper/ceres_manager.h"
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index 7dedb1200e1fcfa14483a848bf5b3032aac3ccd5..aa74ae9e28d8a692694ccc3f1d14c9dcf9dc819a 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -5,12 +5,12 @@
* \author: jsola
*/
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/map_base.h"
#include "base/landmark/landmark_polyline_2D.h"
#include "base/landmark/landmark_AHP.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/yaml/yaml_conversion.h"
#include <iostream>
diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
index 0e397cac34729accf6ecd24da3829f011044800a..1fbed214fb99a53c9c4d509ccc3e47d27c744cb1 100644
--- a/src/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -7,10 +7,10 @@
//Wolf
#include "base/capture/capture_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index 7dc68ebd7d36b75ee7dc15299495ede4bc6f8fe2..33ec4cabb2580353719cf4d50ed14976e626cfaa 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -9,10 +9,10 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
diff --git a/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
index 6cf92cef243dde6230b39220224c1a2a87b18724..22c797d6d0726e600427680bd475dede67ebc5b8 100644
--- a/src/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -9,10 +9,10 @@
#include "base/capture/capture_IMU.h"
#include "base/sensor/sensor_IMU.h"
#include <test/processor_IMU_UnitTester.h>
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index ffeb5ce92272894a19ec5c7161ee354412a7072a..3c5bc0afed0a39fe7a9ca1f8560a26410602855a 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -6,7 +6,7 @@
*/
#include "base/capture/capture_IMU.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_3D.h"
#include "base/map_base.h"
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index 44d3e44f200b5a0b0858c181c160d96e284cc051..89a213f43bdf3a300bf49463a18176f2d1c43737 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -9,10 +9,10 @@
#include <iostream>
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/processor/processor_tracker_feature_dummy.h"
#include "base/capture/capture_void.h"
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index 81900c7ef6077fe947f09c9f59a42d094c268a40..1702229762a5aa413678f6937a8e78d4b7b921df 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -9,10 +9,10 @@
#include <iostream>
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/processor/processor_tracker_landmark_dummy.h"
#include "base/capture/capture_void.h"
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index d8b22c18d234ff658b22179060a6008ef663e117..be7df536e971273559252aeff8dc51bd808bb14b 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -4,9 +4,9 @@
#include "base/processor/processor_tracker_landmark_image.h"
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
#include "base/processor/processor_odom_3D.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/sensor/sensor_camera.h"
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 142b9980846a38c123e01a61c4df2513f724729e..dd8b88f79643ebc894aa4907f9ec58d4c1159396 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -5,8 +5,8 @@
* \author: jtarraso
*/
-#include "base/wolf.h"
-#include "base/frame_base.h"
+#include "base/common/wolf.h"
+#include "base/frame/frame_base.h"
#include "base/pinhole_tools.h"
#include "base/sensor/sensor_camera.h"
#include "base/rotations.h"
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index e046765f0bd18cb289a4137f452c581440ab08c8..c65e17c714f887541f5cfd81409fab4cca02964d 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -6,9 +6,9 @@
*/
// Wolf includes
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/frame_base.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/frame/frame_base.h"
#include "base/trajectory_base.h"
// STL includes
diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
index 5885553ed2fbbed5b6769d1f361c84cb9195509f..9bdcd7c4a15a6f188c25e9f6bad886ea099a2311 100644
--- a/src/examples/test_state_quaternion.cpp
+++ b/src/examples/test_state_quaternion.cpp
@@ -5,8 +5,8 @@
* \author: jsola
*/
-#include "base/frame_base.h"
-#include "base/state_quaternion.h"
+#include "base/frame/frame_base.h"
+#include "base/state_block/state_quaternion.h"
#include "base/time_stamp.h"
#include <iostream>
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index b35d0dc1b335d88759ecdcd3b4c277a309b58636..728e5ef4dee16d107deebb6fbaaba2db0f8e3a55 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -17,7 +17,7 @@
#include "base/processor/processor_odom_3D.h"
#include "../processor_image_feature.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/factory.h"
diff --git a/src/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp
index b516acd098a93ce8c060e0c380555ec29041be3a..e188ff81594c2888e212372e6d945683e8af4703 100644
--- a/src/examples/test_wolf_logging.cpp
+++ b/src/examples/test_wolf_logging.cpp
@@ -5,7 +5,7 @@
* \author: Jeremie Deray
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/logging.h"
int main(int, char*[])
diff --git a/src/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp
index a2e3ccef5d5534daf5be8b1a360e2c30de6b3e6d..ff78c97b0866ab66440ba91d20d8d0db503aed26 100644
--- a/src/examples/test_wolf_root.cpp
+++ b/src/examples/test_wolf_root.cpp
@@ -6,7 +6,7 @@
*/
//Wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//std
#include <iostream>
diff --git a/src/factor/CMakeLists.txt b/src/factor/CMakeLists.txt
deleted file mode 100644
index 9c575b51f3e266d23e243f6713356516d70901ab..0000000000000000000000000000000000000000
--- a/src/factor/CMakeLists.txt
+++ /dev/null
@@ -1,27 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT}
- ${CMAKE_CURRENT_SOURCE_DIR}/factor_autodiff_distance_3D.h
- ${CMAKE_CURRENT_SOURCE_DIR}/factor_autodiff_trifocal.h
- ${CMAKE_CURRENT_SOURCE_DIR}/factor_diff_drive.h
- )
-
-SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT}
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT} PARENT_SCOPE)
-SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} PARENT_SCOPE)
-
-
\ No newline at end of file
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
deleted file mode 100644
index c86bec4aa9b35d7e7879f5a6c7f53c9181e78570..0000000000000000000000000000000000000000
--- a/src/factor/factor_analytic.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-#include "base/factor/factor_analytic.h"
-#include "base/state_block.h"
-
-namespace wolf {
-
-FactorAnalytic::FactorAnalytic(const std::string& _tp,
- bool _apply_loss_function, FactorStatus _status,
- StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
- state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
- _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
- _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
- _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
- _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
- _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
- _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
- _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
- _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
- _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
-{
- resizeVectors();
-}
-
-FactorAnalytic::FactorAnalytic(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status,
- StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
- state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
- _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
- _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
- _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
- _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
- _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
- _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
- _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
- _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
- _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
-{
- resizeVectors();
-}
-/*
-FactorAnalytic::FactorAnalytic(FactorType _tp, const LandmarkBasePtr& _landmark_ptr, const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status,
- StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase( _tp, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
-{
- resizeVectors();
-}
-*/
-std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector() const
-{
- return state_ptr_vector_;
-}
-
-std::vector<unsigned int> FactorAnalytic::getStateSizes() const
-{
- return state_block_sizes_vector_;
-}
-
-JacobianMethod FactorAnalytic::getJacobianMethod() const
-{
- return JAC_ANALYTIC;
-}
-
-void FactorAnalytic::resizeVectors()
-{
- assert(state_ptr_vector_[0] != nullptr && "at least one not null state block pointer required");
-
- for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++)
- if (state_ptr_vector_.at(ii) == nullptr)
- {
- state_ptr_vector_.resize(ii);
- state_block_sizes_vector_.resize(ii);
- break;
- }
-}
-
-} // namespace wolf
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
deleted file mode 100644
index e3dc7b1ab767657d613dc99c9f41925cf6a290da..0000000000000000000000000000000000000000
--- a/src/factor/factor_base.cpp
+++ /dev/null
@@ -1,149 +0,0 @@
-#include "base/factor/factor_base.h"
-#include "base/frame_base.h"
-#include "base/landmark/landmark_base.h"
-
-namespace wolf {
-
-unsigned int FactorBase::factor_id_count_ = 0;
-
-FactorBase::FactorBase(const std::string& _tp,
- bool _apply_loss_function,
- FactorStatus _status) :
- NodeBase("CONSTRAINT", _tp),
- feature_ptr_(), // nullptr
- factor_id_(++factor_id_count_),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
- frame_other_ptr_(), // nullptr
- feature_other_ptr_(), // nullptr
- landmark_other_ptr_(), // nullptr
- processor_ptr_() // nullptr
-{
-// std::cout << "constructed +c" << id() << std::endl;
-}
-
-FactorBase::FactorBase(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- NodeBase("CONSTRAINT", _tp),
- feature_ptr_(),
- factor_id_(++factor_id_count_),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
- frame_other_ptr_(_frame_other_ptr),
- capture_other_ptr_(_capture_other_ptr),
- feature_other_ptr_(_feature_other_ptr),
- landmark_other_ptr_(_landmark_other_ptr),
- processor_ptr_(_processor_ptr)
-{
-// std::cout << "constructed +c" << id() << std::endl;
-}
-
-void FactorBase::remove()
-{
- if (!is_removing_)
- {
- is_removing_ = true;
- FactorBasePtr this_c = shared_from_this(); // keep this alive while removing it
- FeatureBasePtr f = feature_ptr_.lock();
- if (f)
- {
- f->getFactorList().remove(shared_from_this()); // remove from upstream
- if (f->getFactorList().empty() && f->getConstrainedByList().empty())
- f->remove(); // remove upstream
- }
- // add factor to be removed from solver
- if (getProblem() != nullptr)
- getProblem()->removeFactor(shared_from_this());
-
- // remove other: {Frame, Capture, Feature, Landmark}
- FrameBasePtr frm_o = frame_other_ptr_.lock();
- if (frm_o)
- {
- frm_o->getConstrainedByList().remove(shared_from_this());
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
- frm_o->remove();
- }
-
- CaptureBasePtr cap_o = capture_other_ptr_.lock();
- if (cap_o)
- {
- cap_o->getConstrainedByList().remove(shared_from_this());
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
- cap_o->remove();
- }
-
- FeatureBasePtr ftr_o = feature_other_ptr_.lock();
- if (ftr_o)
- {
- ftr_o->getConstrainedByList().remove(shared_from_this());
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
- ftr_o->remove();
- }
-
- LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
- if (lmk_o)
- {
- lmk_o->getConstrainedByList().remove(shared_from_this());
- if (lmk_o->getConstrainedByList().empty())
- lmk_o->remove();
- }
-
- // std::cout << "Removed c" << id() << std::endl;
- }
-}
-
-const Eigen::VectorXs& FactorBase::getMeasurement() const
-{
- return getFeature()->getMeasurement();
-}
-
-const Eigen::MatrixXs& FactorBase::getMeasurementCovariance() const
-{
- return getFeature()->getMeasurementCovariance();
-}
-
-const Eigen::MatrixXs& FactorBase::getMeasurementSquareRootInformationUpper() const
-{
- return getFeature()->getMeasurementSquareRootInformationUpper();
-}
-
-CaptureBasePtr FactorBase::getCapture() const
-{
- return getFeature()->getCapture();
-}
-
-void FactorBase::setStatus(FactorStatus _status)
-{
- if (getProblem() == nullptr)
- std::cout << "factor not linked with 'top', only status changed" << std::endl;
- else if (_status != status_)
- {
- if (_status == FAC_ACTIVE)
- getProblem()->addFactor(shared_from_this());
- else if (_status == FAC_INACTIVE)
- getProblem()->removeFactor(shared_from_this());
- }
- status_ = _status;
-}
-
-void FactorBase::setApplyLossFunction(const bool _apply)
-{
- if (apply_loss_function_ != _apply)
- {
- if (getProblem() == nullptr)
- std::cout << "factor not linked with Problem, apply loss function change not notified" << std::endl;
- else
- {
- FactorBasePtr this_c = shared_from_this();
- getProblem()->removeFactor(this_c);
- getProblem()->addFactor(this_c);
- }
- }
-}
-
-} // namespace wolf
diff --git a/src/feature/CMakeLists.txt b/src/feature/CMakeLists.txt
deleted file mode 100644
index 316731a39f5dd8e3721e70d6b23ceec4186e523e..0000000000000000000000000000000000000000
--- a/src/feature/CMakeLists.txt
+++ /dev/null
@@ -1,25 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_FEATURE ${HDRS_FEATURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.h
- )
-
-SET(SRCS_FEATURE ${SRCS_FEATURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_FEATURE ${HDRS_FEATURE} PARENT_SCOPE)
-SET(SRCS_FEATURE ${SRCS_FEATURE} PARENT_SCOPE)
-
\ No newline at end of file
diff --git a/src/feature/feature_GPS_fix.cpp b/src/feature/feature_GPS_fix.cpp
deleted file mode 100644
index d4b5a12f241b5055e037f4cbd893051687dfb25e..0000000000000000000000000000000000000000
--- a/src/feature/feature_GPS_fix.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "base/feature/feature_GPS_fix.h"
-
-namespace wolf {
-
-FeatureGPSFix::FeatureGPSFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("GPS FIX", _measurement, _meas_covariance)
-{
- //
-}
-
-FeatureGPSFix::~FeatureGPSFix()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_GPS_pseudorange.cpp b/src/feature/feature_GPS_pseudorange.cpp
deleted file mode 100644
index 7979f30e45768cc0ebb9a4b1f8b2c376a4dde601..0000000000000000000000000000000000000000
--- a/src/feature/feature_GPS_pseudorange.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#include "base/feature/feature_GPS_pseudorange.h"
-
-namespace wolf {
-
-FeatureGPSPseudorange::FeatureGPSPseudorange(Eigen::Vector3s &_sat_position, Scalar _pseudorange, Scalar _covariance) :
- FeatureBase("GPS PR", Eigen::VectorXs::Constant(1,_pseudorange), Eigen::MatrixXs::Identity(1,1)*_covariance),
- sat_position_(_sat_position),
- pseudorange_(_pseudorange)
-{
-// std::cout << "FeatureGPSPseudorange() " << std::setprecision(12)
-// << " --pr=" << pseudorange_
-// << "\t--pos(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")"
-// << " --covariance =" << getMeasurementCovariance()
-// << std::endl;
-}
-
-FeatureGPSPseudorange::~FeatureGPSPseudorange()
-{
-
-}
-
-Scalar FeatureGPSPseudorange::getPseudorange() const
-{
- return pseudorange_;
-}
-
-const Eigen::Vector3s &FeatureGPSPseudorange::getSatPosition() const
-{
- return sat_position_;
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
deleted file mode 100644
index b35baf2d0a7dc9191e0a17886aea0a6134b060df..0000000000000000000000000000000000000000
--- a/src/feature/feature_IMU.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "base/feature/feature_IMU.h"
-
-namespace wolf {
-
-FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::Vector6s& _bias,
- const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
- CaptureMotionPtr _cap_imu_ptr) :
- FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance),
- acc_bias_preint_(_bias.head<3>()),
- gyro_bias_preint_(_bias.tail<3>()),
- jacobian_bias_(_dD_db_jacobians)
-{
- if (_cap_imu_ptr)
- this->setCapture(_cap_imu_ptr);
-}
-
-FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
- FeatureBase("IMU", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()),
- acc_bias_preint_ (_cap_imu_ptr->getCalibrationPreint().head<3>()),
- gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
- jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
-{
- this->setCapture(_cap_imu_ptr);
-}
-
-FeatureIMU::~FeatureIMU()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
deleted file mode 100644
index b47c956b9f39e574f8a8c019788669bc024efbe7..0000000000000000000000000000000000000000
--- a/src/feature/feature_base.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-#include "base/feature/feature_base.h"
-#include "base/factor/factor_base.h"
-#include "base/capture/capture_base.h"
-
-namespace wolf {
-
-unsigned int FeatureBase::feature_id_count_ = 0;
-
-FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- NodeBase("FEATURE", _type),
- capture_ptr_(),
- feature_id_(++feature_id_count_),
- track_id_(0),
- landmark_id_(0),
- measurement_(_measurement)
-{
- setMeasurementCovariance(_meas_covariance);
-// std::cout << "constructed +f" << id() << std::endl;
-}
-
-FeatureBase::~FeatureBase()
-{
-// std::cout << "destructed -f" << id() << std::endl;
-}
-
-void FeatureBase::remove()
-{
- if (!is_removing_)
- {
- is_removing_ = true;
- FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
-
- // remove from upstream
- CaptureBasePtr C = capture_ptr_.lock();
- if (C)
- {
- C->getFeatureList().remove(this_f); // remove from upstream
- if (C->getFeatureList().empty())
- C->remove(); // remove upstream
- }
-
- // remove downstream
- while (!factor_list_.empty())
- {
- factor_list_.front()->remove(); // remove downstream
- }
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(); // remove constrained
- }
- }
-}
-
-FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
-{
- factor_list_.push_back(_co_ptr);
- _co_ptr->setFeature(shared_from_this());
- _co_ptr->setProblem(getProblem());
- // add factor to be added in solver
- if (getProblem() != nullptr)
- {
- if (_co_ptr->getStatus() == FAC_ACTIVE)
- getProblem()->addFactor(_co_ptr);
- }
- else
- WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM.");
- return _co_ptr;
-}
-
-FrameBasePtr FeatureBase::getFrame() const
-{
- return capture_ptr_.lock()->getFrame();
-}
-
-FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFeatureOther(shared_from_this());
- return _fac_ptr;
-}
-
-FactorBasePtrList& FeatureBase::getFactorList()
-{
- return factor_list_;
-}
-
-void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
-{
- _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
-}
-
-void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
-{
- WOLF_ASSERT_COVARIANCE_MATRIX(_meas_cov);
-
- // set (ensuring symmetry)
- measurement_covariance_ = _meas_cov.selfadjointView<Eigen::Upper>();
-
- // compute square root information upper matrix
- measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
-}
-
-void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
-{
- WOLF_ASSERT_INFORMATION_MATRIX(_meas_info);
-
- // set (ensuring symmetry)
- measurement_covariance_ = _meas_info.inverse().selfadjointView<Eigen::Upper>();
-
- // Avoid singular covariance
- makePosDef(measurement_covariance_);
-
- // compute square root information upper matrix
- measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
-}
-
-void FeatureBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto ctr : factor_list_)
- ctr->setProblem(_problem);
-}
-
-Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const
-{
- // impose symmetry
- Eigen::MatrixXs info = _info.selfadjointView<Eigen::Upper>();
-
- // Normal Cholesky factorization
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(info);
- Eigen::MatrixXs R = llt_of_info.matrixU();
-
- // Good factorization
- if (info.isApprox(R.transpose() * R, Constants::EPS))
- return R;
-
- // Not good factorization: SelfAdjointEigenSolver
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(info);
- Eigen::VectorXs eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
-
- R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
-
- return R;
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_corner_2D.cpp b/src/feature/feature_corner_2D.cpp
deleted file mode 100644
index 27803ef4a9902ec53b6689a6f7fb4ea1c5fddb57..0000000000000000000000000000000000000000
--- a/src/feature/feature_corner_2D.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-
-#include "base/feature/feature_corner_2D.h"
-
-namespace wolf {
-
-FeatureCorner2D::FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance) :
- FeatureBase("CORNER 2D", _measurement, _meas_covariance)
-{
- //std::cout << "feature: "<< _measurement.transpose() << std::endl;
-}
-
-FeatureCorner2D::~FeatureCorner2D()
-{
- //
-}
-
-Scalar FeatureCorner2D::getAperture() const
-{
- return measurement_(3);
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
deleted file mode 100644
index c2b8d9ed1172314d3a761c1c06d9c50fb2f3e758..0000000000000000000000000000000000000000
--- a/src/feature/feature_diff_drive.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "base/feature/feature_diff_drive.h"
-
-namespace wolf {
-
-FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::VectorXs& _diff_drive_factors,
- const Eigen::MatrixXs& _jacobian_diff_drive_factors) :
- FeatureBase("DIFF DRIVE", _delta_preintegrated, _delta_preintegrated_covariance),
- diff_drive_factors_(_diff_drive_factors),
- jacobian_diff_drive_factors_(_jacobian_diff_drive_factors)
-{
- //
-}
-
-const Eigen::MatrixXs& FeatureDiffDrive::getJacobianFactor() const
-{
- return jacobian_diff_drive_factors_;
-}
-
-} /* namespace wolf */
diff --git a/src/feature/feature_line_2D.cpp b/src/feature/feature_line_2D.cpp
deleted file mode 100644
index c69ba2c1ddf6922773f473c520ed07c4f6d2df57..0000000000000000000000000000000000000000
--- a/src/feature/feature_line_2D.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-
-#include "base/feature/feature_corner_2D.h"
-
-namespace wolf {
-
-FeatureLine2D::FeatureLine2D(const Eigen::Vector3s & _line_homogeneous_params,
- const Eigen::Matrix3s & _params_covariance, Eigen::Vector3s & _point1, Eigen::Vector3s & _point2) :
- FeatureBase("LINE_2D", _line_homogeneous_params, _params_covariance),
- first_point_(_point1),
- last_point_(_point2)
-{
- //
-}
-
-FeatureLine2D::~FeatureLine2D()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
deleted file mode 100644
index 3e01bd662cf69b07230a6c7f939c4dd18a754a07..0000000000000000000000000000000000000000
--- a/src/feature/feature_motion.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * feature_motion.cpp
- *
- * Created on: Aug 11, 2017
- * Author: jsola
- */
-
-#include "feature_motion.h"
-
-namespace wolf
-{
-
-FeatureMotion::FeatureMotion(const std::string& _type, const CaptureMotionPtr& _capture_motion) :
- FeatureBase(_type, _capture_motion->getDeltaPreint(), _capture_motion->getDeltaPreintCov()),
- calib_preint_(_capture_motion->getCalibrationPreint()),
- jacobian_calib_(_capture_motion->getJacobianCalib())
-{
- //
-}
-
-FeatureMotion::~FeatureMotion()
-{
- //
-}
-
-} /* namespace wolf */
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
deleted file mode 100644
index 472072e54eca2369c43b6e8cbb2e8c09d66f86e2..0000000000000000000000000000000000000000
--- a/src/feature/feature_odom_2D.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "base/feature/feature_odom_2D.h"
-
-namespace wolf {
-
-FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("ODOM 2D", _measurement, _meas_covariance)
-{
- //std::cout << "New FeatureOdom2D: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
-}
-
-FeatureOdom2D::~FeatureOdom2D()
-{
- //
-}
-
-void FeatureOdom2D::findFactors()
-{
-
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_point_image.cpp b/src/feature/feature_point_image.cpp
deleted file mode 100644
index e7d3781b6935b78cff79a0b25ee8998e3a7c6d44..0000000000000000000000000000000000000000
--- a/src/feature/feature_point_image.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-
-#include "base/feature/feature_point_image.h"
-
-namespace wolf {
-
-FeaturePointImage::~FeaturePointImage()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_polyline_2D.cpp b/src/feature/feature_polyline_2D.cpp
deleted file mode 100644
index 0f14a701c40b55aacdc042cfd2634dc9392ea180..0000000000000000000000000000000000000000
--- a/src/feature/feature_polyline_2D.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-/**
- * \file feature_polyline.cpp
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#include "base/feature/feature_polyline_2D.h"
-
-namespace wolf
-{
-
-} /* namespace wolf */
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
deleted file mode 100644
index 15d40e60a99284e14275a37e191b10c12c99ab04..0000000000000000000000000000000000000000
--- a/src/feature/feature_pose.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "base/feature/feature_pose.h"
-
-namespace wolf {
-
-FeaturePose::FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("POSE", _measurement, _meas_covariance)
-{
- //
-}
-
-FeaturePose::~FeaturePose()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
deleted file mode 100644
index 32c65515203711405d50b7b95946a76d3dbe10bd..0000000000000000000000000000000000000000
--- a/src/frame_base.cpp
+++ /dev/null
@@ -1,383 +0,0 @@
-
-#include "base/frame_base.h"
-#include "base/factor/factor_base.h"
-#include "base/trajectory_base.h"
-#include "base/capture/capture_base.h"
-#include "base/state_block.h"
-#include "base/state_angle.h"
-#include "base/state_quaternion.h"
-
-namespace wolf {
-
-unsigned int FrameBase::frame_id_count_ = 0;
-
-FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "Base"),
- trajectory_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- frame_id_(++frame_id_count_),
- type_(NON_KEY_FRAME),
- time_stamp_(_ts)
-{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _v_ptr;
-}
-
-FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "Base"),
- trajectory_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- frame_id_(++frame_id_count_),
- type_(_tp),
- time_stamp_(_ts)
-{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _v_ptr;
-}
-
-FrameBase::~FrameBase()
-{
- if ( isKey() )
- removeStateBlocks();
-}
-
-void FrameBase::remove()
-{
- if (!is_removing_)
- {
- is_removing_ = true;
- FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
-
- // remove from upstream
- TrajectoryBasePtr T = trajectory_ptr_.lock();
- if (T)
- {
- T->getFrameList().remove(this_F); // remove from upstream
- }
-
- // remove downstream
- while (!capture_list_.empty())
- {
- capture_list_.front()->remove(); // remove downstream
- }
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(); // remove constrained
- }
-
- // Remove Frame State Blocks
- if ( isKey() )
- removeStateBlocks();
-
- if (getTrajectory()->getLastKeyFrame()->id() == this_F->id())
- getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame());
-
-// std::cout << "Removed F" << id() << std::endl;
- }
-}
-
-void FrameBase::setTimeStamp(const TimeStamp& _ts)
-{
- time_stamp_ = _ts;
- if (isKey() && getTrajectory() != nullptr)
- getTrajectory()->sortFrame(shared_from_this());
-}
-
-void FrameBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
- {
- for (StateBlockPtr sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
-}
-
-void FrameBase::removeStateBlocks()
-{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- StateBlockPtr sbp = getStateBlock(i);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlock(i, nullptr);
- }
- }
-}
-
-void FrameBase::setKey()
-{
- if (type_ != KEY_FRAME)
- {
- type_ = KEY_FRAME;
- registerNewStateBlocks();
-
- if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_)
- getTrajectory()->setLastKeyFrame(shared_from_this());
-
- getTrajectory()->sortFrame(shared_from_this());
-
-// WOLF_DEBUG("Set KF", this->id());
- }
-}
-
-void FrameBase::fix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
-}
-
-void FrameBase::unfix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
-}
-
-bool FrameBase::isFixed() const
-{
- bool fixed = true;
- for (auto sb : getStateBlockVec())
- {
- if (sb)
- fixed &= sb->isFixed();
- }
- return fixed;
-}
-
-void FrameBase::setState(const Eigen::VectorXs& _state)
-{
- int size = 0;
- for(unsigned int i = 0; i<state_block_vec_.size(); i++){
- size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize());
- }
-
- //State Vector size must be lower or equal to frame state size :
- // example : PQVBB -> if we initialize only position and orientation due to use of processorOdom3D
- assert(_state.size() <= size && "In FrameBase::setState wrong state size");
-
- unsigned int index = 0;
- const unsigned int _st_size = _state.size();
-
- //initialize the FrameBase StateBlocks while StateBlocks list's end not reached and input state_size can go further
- for (StateBlockPtr sb : state_block_vec_)
- if (sb && (index < _st_size))
- {
- sb->setState(_state.segment(index, sb->getSize()), isKey());
- index += sb->getSize();
- }
-}
-
-Eigen::VectorXs FrameBase::getState() const
-{
- Eigen::VectorXs state;
-
- getState(state);
-
- return state;
-}
-
-void FrameBase::getState(Eigen::VectorXs& _state) const
-{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
-
- _state = Eigen::VectorXs(size);
-
- SizeEigen index = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- {
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
-}
-
-unsigned int FrameBase::getSize() const
-{
- unsigned int size = 0;
- for (auto st : state_block_vec_)
- if (st)
- size += st->getSize();
- return size;
-}
-
-bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
-{
- return getProblem()->getFrameCovariance(shared_from_this(), _cov);
-}
-
-FrameBasePtr FrameBase::getPreviousFrame() const
-{
- //std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
- if (getTrajectory() == nullptr)
- //std::cout << "This Frame is not linked to any trajectory" << std::endl;
-
- assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
-
- //look for the position of this node in the upper list (frame list of trajectory)
- for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ )
- {
- if ( this->frame_id_ == (*f_it)->id() )
- {
- f_it++;
- if (f_it != getTrajectory()->getFrameList().rend())
- {
- //std::cout << "previous frame found!" << std::endl;
- return *f_it;
- }
- else
- {
- //std::cout << "previous frame not found!" << std::endl;
- return nullptr;
- }
- }
- }
- //std::cout << "previous frame not found!" << std::endl;
- return nullptr;
-}
-
-FrameBasePtr FrameBase::getNextFrame() const
-{
- //std::cout << "finding next frame of " << this->frame_id_ << std::endl;
- auto f_it = getTrajectory()->getFrameList().rbegin();
- f_it++; //starting from second last frame
-
- //look for the position of this node in the frame list of trajectory
- while (f_it != getTrajectory()->getFrameList().rend())
- {
- if ( this->frame_id_ == (*f_it)->id())
- {
- f_it--;
- return *f_it;
- }
- f_it++;
- }
- std::cout << "next frame not found!" << std::endl;
- return nullptr;
-}
-
-CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
-{
- capture_list_.push_back(_capt_ptr);
- _capt_ptr->setFrame(shared_from_this());
- _capt_ptr->setProblem(getProblem());
- _capt_ptr->registerNewStateBlocks();
- return _capt_ptr;
-}
-
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
-{
- for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
- return capture_ptr;
- return nullptr;
-}
-
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr)
-{
- for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensor() == _sensor_ptr)
- return capture_ptr;
- return nullptr;
-}
-
-CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
-{
- CaptureBasePtrList captures;
- for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensor() == _sensor_ptr)
- captures.push_back(capture_ptr);
- return captures;
-}
-
-void FrameBase::unlinkCapture(CaptureBasePtr _cap_ptr)
-{
- _cap_ptr->unlinkFromFrame();
- capture_list_.remove(_cap_ptr);
-}
-
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
-{
- for (const FactorBasePtr& constaint_ptr : getConstrainedByList())
- if (constaint_ptr->getProcessor() == _processor_ptr && constaint_ptr->getType() == type)
- return constaint_ptr;
- return nullptr;
-}
-
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
-{
- for (const FactorBasePtr& constaint_ptr : getConstrainedByList())
- if (constaint_ptr->getProcessor() == _processor_ptr)
- return constaint_ptr;
- return nullptr;
-}
-
-void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
-{
- for (auto c_ptr : getCaptureList())
- c_ptr->getFactorList(_fac_list);
-}
-
-FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFrameOther(shared_from_this());
- return _fac_ptr;
-}
-
-FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
-{
- assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
- StateBlockPtr v_ptr ( nullptr );
- FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) );
- f->setType("PO 2D");
- return f;
-}
-FrameBasePtr FrameBase::create_PO_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
-{
- assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
- StateBlockPtr v_ptr ( nullptr );
- FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) );
- f->setType("PO 3D");
- return f;
-}
-FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
-{
- assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
- StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
- FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) );
- f->setType("POV 3D");
- return f;
-}
-
-} // namespace wolf
-
-#include "base/factory.h"
-namespace wolf
-{
-namespace{ const bool WOLF_UNUSED Frame_PO_2D_Registered = FrameFactory::get().registerCreator("PO 2D", FrameBase::create_PO_2D ); }
-namespace{ const bool WOLF_UNUSED Frame_PO_3D_Registered = FrameFactory::get().registerCreator("PO 3D", FrameBase::create_PO_3D ); }
-namespace{ const bool WOLF_UNUSED Frame_POV_3D_Registered = FrameFactory::get().registerCreator("POV 3D", FrameBase::create_POV_3D); }
-} // namespace wolf
diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp
deleted file mode 100644
index 945b412c87cbab6be87a8d7d5536d2850996a14f..0000000000000000000000000000000000000000
--- a/src/hardware_base.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-#include "base/hardware_base.h"
-#include "base/sensor/sensor_base.h"
-
-namespace wolf {
-
-HardwareBase::HardwareBase() :
- NodeBase("HARDWARE", "Base")
-{
-// std::cout << "constructed H" << std::endl;
-}
-
-HardwareBase::~HardwareBase()
-{
-// std::cout << "destructed -H" << std::endl;
-}
-
-SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
-{
- sensor_list_.push_back(_sensor_ptr);
- _sensor_ptr->setProblem(getProblem());
- _sensor_ptr->setHardware(shared_from_this());
-
- _sensor_ptr->registerNewStateBlocks();
-
- return _sensor_ptr;
-}
-
-} // namespace wolf
diff --git a/src/landmark/CMakeLists.txt b/src/landmark/CMakeLists.txt
deleted file mode 100644
index bdfe8f65b174218926f12aa3d3c8211f4e069569..0000000000000000000000000000000000000000
--- a/src/landmark/CMakeLists.txt
+++ /dev/null
@@ -1,24 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_LANDMARK ${HDRS_LANDMARK}
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_3D.h
- )
-
-SET(SRCS_LANDMARK ${SRCS_LANDMARK}
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_3D.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_LANDMARK ${HDRS_LANDMARK} PARENT_SCOPE)
-SET(SRCS_LANDMARK ${SRCS_LANDMARK} PARENT_SCOPE)
\ No newline at end of file
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
deleted file mode 100644
index 1daa37470b0c1ce87d47e4d174344fd09e471d9d..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_AHP.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "base/landmark/landmark_AHP.h"
-
-#include "base/state_homogeneous_3D.h"
-#include "base/factory.h"
-#include "base/yaml/yaml_conversion.h"
-
-namespace wolf {
-
-/* Landmark - Anchored Homogeneous Point*/
-LandmarkAHP::LandmarkAHP(Eigen::Vector4s _position_homogeneous,
- FrameBasePtr _anchor_frame,
- SensorBasePtr _anchor_sensor,
- cv::Mat _2D_descriptor) :
- LandmarkBase("AHP", std::make_shared<StateHomogeneous3D>(_position_homogeneous)),
- cv_descriptor_(_2D_descriptor.clone()),
- anchor_frame_(_anchor_frame),
- anchor_sensor_(_anchor_sensor)
-{
-}
-
-LandmarkAHP::~LandmarkAHP()
-{
- //
-}
-
-YAML::Node LandmarkAHP::saveToYaml() const
-{
- // First base things
- YAML::Node node = LandmarkBase::saveToYaml();
-
- // Then add specific things
- std::vector<int> v;
- LandmarkAHP::cv_descriptor_.copyTo(v);
- node["descriptor"] = v;
- return node;
-}
-
-Eigen::Vector3s LandmarkAHP::getPointInAnchorSensor() const
-{
- Eigen::Map<const Eigen::Vector4s> hmg_point(getP()->getState().data());
- return hmg_point.head<3>()/hmg_point(3);
-}
-
-Eigen::Vector3s LandmarkAHP::point() const
-{
- using namespace Eigen;
- Transform<Scalar,3,Affine> T_w_r
- = Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
- * Quaternions(getAnchorFrame()->getO()->getState().data());
- Transform<Scalar,3,Affine> T_r_c
- = Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
- * Quaternions(getAnchorSensor()->getO()->getState().data());
- Vector4s point_hmg_c = getP()->getState();
- Vector4s point_hmg = T_w_r * T_r_c * point_hmg_c;
- return point_hmg.head<3>()/point_hmg(3);
-}
-
-LandmarkBasePtr LandmarkAHP::create(const YAML::Node& _node)
-{
- unsigned int id = _node["id"] .as< unsigned int >();
- Eigen::VectorXs pos_homog = _node["position"] .as< Eigen::VectorXs >();
- std::vector<int> v = _node["descriptor"] .as< std::vector<int> >();
- cv::Mat desc(v);
-
- LandmarkBasePtr lmk = std::make_shared<LandmarkAHP>(pos_homog, nullptr, nullptr, desc);
- lmk->setId(id);
- return lmk;
-}
-
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_ahp = LandmarkFactory::get().registerCreator("AHP", LandmarkAHP::create);
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
deleted file mode 100644
index 705cb97b45ef6796d1441630c010e366f657d281..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_base.cpp
+++ /dev/null
@@ -1,170 +0,0 @@
-
-#include "base/landmark/landmark_base.h"
-#include "base/factor/factor_base.h"
-#include "base/map_base.h"
-#include "base/state_block.h"
-#include "base/yaml/yaml_conversion.h"
-
-namespace wolf {
-
-unsigned int LandmarkBase::landmark_id_count_ = 0;
-
-LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
- NodeBase("LANDMARK", _type),
- map_ptr_(),
- state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
- landmark_id_(++landmark_id_count_)
-{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
-
-// std::cout << "constructed +L" << id() << std::endl;
-}
-
-LandmarkBase::~LandmarkBase()
-{
- removeStateBlocks();
-// std::cout << "destructed -L" << id() << std::endl;
-}
-
-void LandmarkBase::remove()
-{
- if (!is_removing_)
- {
- is_removing_ = true;
- LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
-
- // remove from upstream
- auto M = map_ptr_.lock();
- if (M)
- M->getLandmarkList().remove(shared_from_this());
-
- // remove constrained by
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove();
- }
-
- // Remove State Blocks
- removeStateBlocks();
- }
-}
-
-void LandmarkBase::fix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
-}
-
-void LandmarkBase::unfix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
-}
-
-bool LandmarkBase::isFixed() const
-{
- bool fixed = true;
- for (auto sb : getStateBlockVec())
- {
- if (sb)
- fixed &= sb->isFixed();
- }
- return fixed;
-}
-
-std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
-{
- std::vector<StateBlockPtr> used_state_block_vec(0);
- for (auto sbp : state_block_vec_)
- if (sbp)
- used_state_block_vec.push_back(sbp);
- return used_state_block_vec;
-}
-
-void LandmarkBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
- {
- for (auto sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
-}
-
-bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
-{
- return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
-}
-
-void LandmarkBase::removeStateBlocks()
-{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlock(i, nullptr);
- }
- }
-}
-
-Eigen::VectorXs LandmarkBase::getState() const
-{
- Eigen::VectorXs state;
-
- getState(state);
-
- return state;
-}
-
-void LandmarkBase::getState(Eigen::VectorXs& _state) const
-{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
-
- _state = Eigen::VectorXs(size);
-
- SizeEigen index = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- {
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
-}
-
-YAML::Node LandmarkBase::saveToYaml() const
-{
- YAML::Node node;
- node["id"] = landmark_id_;
- node["type"] = node_type_;
- if (getP() != nullptr)
- {
- node["position"] = getP()->getState();
- node["position fixed"] = getP()->isFixed();
- }
- if (getO() != nullptr)
- {
- node["orientation"] = getO()->getState();
- node["orientation fixed"] = getO()->isFixed();
- }
- return node;
-}
-
-FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setLandmarkOther(shared_from_this());
- return _fac_ptr;
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
deleted file mode 100644
index 3a424869ba5a8775bda6025b7a83aeadda89009a..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_container.cpp
+++ /dev/null
@@ -1,192 +0,0 @@
-
-#include "base/landmark/landmark_container.h"
-#include "base/state_block.h"
-
-namespace wolf {
-
-LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _witdh, const Scalar& _length) :
- LandmarkBase("CONTAINER", _p_ptr, _o_ptr),
- corners_(3,4)
-{
- Eigen::VectorXs descriptor(2);
- descriptor << _witdh, _length;
- setDescriptor(descriptor);
-
- corners_ << -_length / 2, _length / 2, _length / 2, -_length / 2,
- -_witdh / 2, -_witdh / 2, _witdh / 2, _witdh / 2,
- M_PI / 4, 3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4;
-}
-
-LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh, const Scalar& _length) :
- LandmarkBase("CONTAINER", _p_ptr, _o_ptr),
- corners_(3,4)
-{
- Eigen::VectorXs descriptor(2);
- descriptor << _witdh, _length;
- setDescriptor(descriptor);
-
- corners_ << -_length / 2, _length / 2, _length / 2, -_length / 2,
- -_witdh / 2, -_witdh / 2, _witdh / 2, _witdh / 2,
- M_PI / 4, 3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4;
-
- // Computing center
- WOLF_DEBUG( "Container constructor: Computing center pose... " );
- Eigen::Vector2s container_position(getP()->getState());
- Eigen::Vector1s container_orientation(getO()->getState());
-
- WOLF_DEBUG( "Container constructor: _corner_1_idx ", _corner_1_idx,
- "_corner_2_idx ", _corner_2_idx );
-
- // Large side detected (A & B)
- if ( (_corner_1_idx == 0 && _corner_2_idx == 1) || (_corner_1_idx == 1 && _corner_2_idx == 0) )
- {
- WOLF_DEBUG( "Large side detected" );
- Eigen::Vector2s AB = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- Eigen::Vector2s perpendicularAB;
- perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
- container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AB / 2 + perpendicularAB * _witdh / 2;
- container_orientation(0) = atan2(AB(1),AB(0));
- }
-
- // Short side detected (B & C)
- else if ( (_corner_1_idx == 1 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 1) )
- {
- WOLF_DEBUG( "Short side detected" );
- Eigen::Vector2s BC = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- Eigen::Vector2s perpendicularBC;
- perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
- container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BC / 2 + perpendicularBC * _length / 2;
- container_orientation(0) = atan2(BC(1),BC(0)) - M_PI / 2;
- }
-
- // Diagonal detected
- // A & C
- else if ( (_corner_1_idx == 0 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 0) )
- {
- WOLF_DEBUG( "diagonal AC detected" );
- Eigen::Vector2s AC = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AC / 2;
- container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
- }
- // B & D
- else if ( (_corner_1_idx == 1 && _corner_2_idx == 3) || (_corner_1_idx == 3 && _corner_2_idx == 1) )
- {
- WOLF_DEBUG( "diagonal BD detected" );
- Eigen::Vector2s BD = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BD / 2;
- container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
- }
- else
- assert(0 && "index corners combination not implemented!");
-
- WOLF_DEBUG( "_corner_1_pose... ", _corner_1_pose.transpose() );
- WOLF_DEBUG( "_corner_2_pose... ", _corner_2_pose.transpose() );
- WOLF_DEBUG( "Container center pose... ", container_position.transpose(), " ", container_orientation.transpose() );
-
- getP()->setState(container_position);
- getO()->setState(container_orientation);
-}
-
-//LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) :
-// LandmarkBase(_p_ptr, _o_ptr),
-// corners_(3,4)
-//{
-// assert((_corner_A_ptr != nullptr || _corner_B_ptr != nullptr || _corner_C_ptr != nullptr || _corner_D_ptr != nullptr) && "all corner pointer are null in landmark container constructor from corners");
-//
-// Eigen::VectorXs descriptor(2);
-// descriptor << _witdh, _length;
-// setDescriptor(descriptor);
-//
-// corners_ << -_length / 2, _length / 2, _length / 2, -_length / 2,
-// -_witdh / 2, -_witdh / 2, _witdh / 2, _witdh / 2,
-// 0, M_PI/2, M_PI, -M_PI/2;
-//
-// // Computing center
-// //std::cout << "Container constructor: Computing center position... " << std::endl;
-// Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->get());
-// Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->get(), _o_ptr->getSize());
-//
-// container_position = Eigen::Vector2s::Zero();
-//
-// // Large side detected
-// // A & B
-// if (_corner_A_ptr != nullptr && _corner_B_ptr != nullptr)
-// {
-// Eigen::Vector2s AB = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
-// Eigen::Vector2s perpendicularAB;
-// perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AB / 2 + perpendicularAB * _witdh / 2;
-// container_orientation(0) = atan2(AB(1),AB(0));
-// }
-// // C & D
-// else if (_corner_C_ptr != nullptr && _corner_D_ptr != nullptr)
-// {
-// Eigen::Vector2s CD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get());
-// Eigen::Vector2s perpendicularCD;
-// perpendicularCD << -CD(1)/CD.norm(), CD(0)/CD.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) + CD / 2 + perpendicularCD * _witdh / 2;
-// container_orientation(0) = atan2(-CD(1),-CD(0));
-// }
-// // Short side detected
-// // B & C
-// else if (_corner_B_ptr != nullptr && _corner_C_ptr != nullptr)
-// {
-// Eigen::Vector2s BC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
-// Eigen::Vector2s perpendicularBC;
-// perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BC / 2 + perpendicularBC * _length / 2;
-// container_orientation(0) = atan2(BC(1),BC(0));
-// }
-// // D & A
-// else if (_corner_D_ptr != nullptr && _corner_A_ptr != nullptr)
-// {
-// Eigen::Vector2s DA = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get());
-// Eigen::Vector2s perpendicularDA;
-// perpendicularDA << -DA(1)/DA.norm(), DA(0)/DA.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) + DA / 2 + perpendicularDA * _length / 2;
-// container_orientation(0) = atan2(-DA(1),-DA(0));
-// }
-// // Diagonal detected
-// // A & C
-// else if (_corner_A_ptr != nullptr && _corner_C_ptr != nullptr)
-// {
-// Eigen::Vector2s AC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AC / 2;
-// container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
-// }
-// // B & D
-// else if (_corner_B_ptr != nullptr && _corner_D_ptr != nullptr)
-// {
-// Eigen::Vector2s BD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BD / 2;
-// container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
-// }
-//}
-
-LandmarkContainer::~LandmarkContainer()
-{
- //
-}
-
-Scalar LandmarkContainer::getWidth() const
-{
- return descriptor_(0);
-}
-
-Scalar LandmarkContainer::getLength() const
-{
- return descriptor_(1);
-}
-
-Eigen::MatrixXs LandmarkContainer::getCorners() const
-{
- return corners_;
-}
-
-Eigen::VectorXs LandmarkContainer::getCorner(const unsigned int _id) const
-{
- assert(/*_id >= 0 &&*/ _id <= 4 && "wrong corner id parameter in getCorner(id)");
- return corners_.col(_id);
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_corner_2D.cpp b/src/landmark/landmark_corner_2D.cpp
deleted file mode 100644
index 558292f90e54923dcacabe157d5818e40a1199e2..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_corner_2D.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-
-#include "base/landmark/landmark_corner_2D.h"
-
-namespace wolf {
-
-LandmarkCorner2D::LandmarkCorner2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _aperture) :
- LandmarkBase("CORNER 2D", _p_ptr, _o_ptr)
-{
- setDescriptor(Eigen::VectorXs::Constant(1,_aperture));
-}
-
-LandmarkCorner2D::~LandmarkCorner2D()
-{
- //
-}
-
-Scalar LandmarkCorner2D::getAperture() const
-{
- return descriptor_(0);
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_line_2D.cpp b/src/landmark/landmark_line_2D.cpp
deleted file mode 100644
index aa2a14d9f8a59f6d45b4b58f37d1a6307ee5a7a7..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_line_2D.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-
-#include "base/landmark/landmark_line_2D.h"
-
-namespace wolf {
-
-LandmarkLine2D::LandmarkLine2D(StateBlockPtr _p_ptr, Eigen::Vector3s & _point1, Eigen::Vector3s & _point2) :
- LandmarkBase("LINE 2D", _p_ptr)
-{
- //set extreme points
- point1_ = _point1;
- point2_ = _point2;
-}
-
-LandmarkLine2D::~LandmarkLine2D()
-{
- //
-}
-
-void LandmarkLine2D::updateExtremePoints(Eigen::Vector3s & _q1, Eigen::Vector3s & _q2)
-{
- //Mainly this method performs two actions:
- // 1. First check if new points are "inside" or "outside" the segment
- // 2. In case extremes have been updated, fit them on the line
- //
- // p1 & p2 are this->point1_ and point2_
- // _q1 & _q2 are the new points to evaluate
- // vector p1p2 is the vector from p1 to p2
- // dot product s_p1q1 is the dot product between vector p1p2 and vector p1q1
- // dot product s_p2q1 is the dot product between vector p2p1 and vector p2q1
-
- //local variables
- Eigen::Vector2s p1p2, p1q1, p1q2, p2p1, p2q1, p2q2;
- //Scalar s_p1q1, s_p1q2, s_p2q1, s_p2q2;
-
- //compute all necessary vectors
- p1p2 = point2_.head(2) - point1_.head(1);
- p1q1 = _q1.head(2) - point1_.head(1);
- p1q2 = _q2.head(2) - point1_.head(1);
- p2p1 = point1_.head(2) - point2_.head(1);
- p2q1 = _q1.head(2) - point2_.head(1);
- p2q2 = _q2.head(2) - point2_.head(1);
-
- //compute all necessary scalar products.
- //s_p1q1 = p1p2.dot()
-
-}
-
-const Eigen::Vector3s & LandmarkLine2D::point1() const
-{
- return point1_;
-}
-
-const Eigen::Vector3s & LandmarkLine2D::point2() const
-{
- return point2_;
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
deleted file mode 100644
index f6028792b639b98aaf538e5a682a75c242d26171..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_match_polyline_2D.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
- * landmark_match_polyline_2D.cpp
- *
- * Created on: Apr 2, 2019
- * Author: jvallve
- */
-
-#include "base/landmark/landmark_match_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-
-namespace wolf {
-
-bool LandmarkMatchPolyline2D::check() const
-{
- LandmarkPolyline2DPtr pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_);
- if (pl_lmk == nullptr)
- {
- WOLF_ERROR("LandmarkMatchPolyline2D landmark is not polyline 2D");
- return false;
- }
- if (!pl_lmk->isClosed())
- {
- if (landmark_from_id_ > landmark_to_id_)
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk");
- return false;
- }
- if (landmark_from_id_ < pl_lmk->getFirstId())
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk");
- return false;
- }
- if (landmark_to_id_ > pl_lmk->getLastId())
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk");
- return false;
- }
- }
- int lmk_id = landmark_from_id_;
- int ftr_id = feature_from_id_;
- while (1)
- {
- if (lmk_id == landmark_to_id_)
- {
- if (ftr_id != feature_to_id_)
- {
- WOLF_ERROR("LandmarkMatchPolyline2D landmark_to_id_ does not correspond to feature_to_id_");
- return false;
- }
- break;
- }
- else if (lmk_id == pl_lmk->getLastId() && !pl_lmk->isClosed())
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_id == lmk_last_id but lmk_id != landmark_to_id_ in a not closed lmk");
- return false;
- }
- else
- {
- lmk_id = pl_lmk->getNextValidId(lmk_id);
- ftr_id++;
- }
- }
- return true;
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_point_3D.cpp b/src/landmark/landmark_point_3D.cpp
deleted file mode 100644
index dda3984e904ecb1221074df749cdcc671dfed3ff..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_point_3D.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-#include "base/landmark/landmark_point_3D.h"
-
-namespace wolf {
-
-LandmarkPoint3D::LandmarkPoint3D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, Eigen::Vector3s _position, cv::Mat _2D_descriptor) :
- LandmarkBase("POINT 3D", _p_ptr, _o_ptr),
- descriptor_(_2D_descriptor),
- position_(_position)
-{
- //LandmarkPoint3D* landmark_ptr = (LandmarkPoint3D*)_p_ptr;
-// position_ =
-// descriptor_ = _2D_descriptor;
-}
-
-LandmarkPoint3D::~LandmarkPoint3D()
-{
- //
-}
-
-}
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
deleted file mode 100644
index 7abfae36142847df7511e5fd19e94f5f25ab85c0..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_polyline_2D.cpp
+++ /dev/null
@@ -1,1050 +0,0 @@
-/**
- * \file landmark_polyline_2D.cpp
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/local_parametrization_polyline_extreme.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/factory.h"
-#include "base/yaml/yaml_conversion.h"
-
-namespace wolf
-{
-
-
-LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id) :
- LandmarkPolyline2D(_p_ptr, _o_ptr, _points, _first_defined, _last_defined, _first_id, PolylineRectangularClass())
-{
-}
-
-LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
- LandmarkBase("POLYLINE 2D", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
-{
- //std::cout << "LandmarkPolyline2D::LandmarkPolyline2D" << std::endl;
- assert(_points.cols() >= 2 && "2 points at least needed.");
-
- for (auto i = 0; i < _points.cols(); i++)
- point_state_ptr_map_[i+_first_id] = std::make_shared<StateBlock>(_points.col(i).head<2>());
-
- if (!first_defined_)
- firstStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.begin())->second));
- if (!last_defined_)
- lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.rbegin())->second));
-
- first_id_ = getFirstId();
- last_id_ = getLastId();
-
- //std::cout << "LandmarkPolyline2D " << id() << " created. First: " << first_id_ << " last: "<< last_id_ << std::endl;
-}
-
-LandmarkPolyline2D::~LandmarkPolyline2D()
-{
- removeStateBlocks();
-}
-
-void LandmarkPolyline2D::setFirst(const Eigen::VectorXs& _point, bool _defined)
-{
- //std::cout << "LandmarkPolyline2D::setFirst. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "LandmarkPolyline2D::setFirstExtreme: bad point size");
- assert(!(!_defined && first_defined_) && "setting a defined extreme with a not defined point");
- firstStateBlock()->setState(_point.head(2));
- if (!first_defined_ && _defined)
- defineFirst();
-}
-
-void LandmarkPolyline2D::setLast(const Eigen::VectorXs& _point, bool _defined)
-{
- //std::cout << "LandmarkPolyline2D::setLast. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "LandmarkPolyline2D::setLastExtreme: bad point size");
- assert(!(!_defined && last_defined_) && "setting a defined extreme with a not defined point");
- lastStateBlock()->setState(_point.head(2));
- if (!last_defined_ && _defined)
- defineLast();
-}
-
-const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
-{
- assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
- return point_state_ptr_map_.at(_i)->getState();
-}
-
-StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
-{
- assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
- return point_state_ptr_map_.at(_i);
-}
-
-Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal() const
-{
- Eigen::MatrixXs points_global = Eigen::MatrixXs::Ones(3,getNPoints());
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getO()->getState()(0)).matrix();
- Eigen::Vector2s t_world_points = getP()->getState();
-
- for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id))
- {
- points_global.block(0,i,2,1) = getPointVector(valid_id);
- if (valid_id == getLastId())
- break;
- }
-
- if (classification_.type != UNCLASSIFIED) // points are in landmark PO reference only when clasified
- points_global.topRows(2) = (R_world_points * points_global.topRows(2)).colwise() + t_world_points;
-
- return points_global;
-}
-
-
-Eigen::MatrixXs LandmarkPolyline2D::computePointsCovGlobal() const
-{
- Eigen::MatrixXs points_cov_global = Eigen::MatrixXs::Zero(2,2*getNPoints());
-
- for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id))
- {
- // TODO
- points_cov_global.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
-
- if (valid_id == getLastId())
- break;
- }
-
- return points_cov_global;
-}
-
-void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back)
-{
- assert(!closed_ && "adding point to a closed polyline!");
-
- //std::cout << "LandmarkPolyline2D::addPoint. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "bad point size");
-
- // define previous extreme if not defined
- if (_back ? !last_defined_ : !first_defined_)
- defineExtreme(_back);
-
- // add new extreme
- if (_back)
- {
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_point.head<2>(),
- false,
- (!_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
- nullptr));
- point_state_ptr_map_[++last_id_] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
-
- last_defined_ = _defined;
- }
- else
- {
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_point.head<2>(),
- false,
- (!_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
- nullptr));
- point_state_ptr_map_[--first_id_] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
-
- first_defined_ = _defined;
- }
-
- // new version
- version_++;
-
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
-}
-
-void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined,
- const bool& _back)
-{
- assert(!closed_ && "adding points to a closed polyline!");
-
- //std::cout << "LandmarkPolyline2D::addPoints from/to: " << _idx << " Defined " << _defined << std::endl;
- assert(_points.rows() >= 2 && "bad points size");
- assert(_idx < _points.cols() && "bad index!");
-
- // define previous extreme if not defined
- if (_back ? !last_defined_ : !first_defined_)
- defineExtreme(_back);
-
- // add new extreme points
- if (_back)
- {
- for (int i = _idx; i < _points.cols(); i++)
- {
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_points.block(0,i,2,1),
- false,
- (i == _points.cols()-1 && !_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
- nullptr));
- point_state_ptr_map_[++last_id_] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
- }
- last_defined_ = _defined;
- }
- else
- {
- for (int i = _idx; i >= 0; i--)
- {
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_points.block(0,i,2,1),
- false,
- (i == 0 && !_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
- nullptr));
- point_state_ptr_map_[--first_id_] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
- }
- first_defined_ = _defined;
- }
-
- // new version
- version_++;
-
- //std::cout << "final number of points: " << point_state_ptr_vector_.size() << std::endl;
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
-}
-
-void LandmarkPolyline2D::defineFirst()
-{
- defineExtreme(false);
-}
-
-void LandmarkPolyline2D::defineLast()
-{
- defineExtreme(true);
-}
-
-void LandmarkPolyline2D::defineExtreme(const bool _back)
-{
- StateBlockPtr extreme_state = (_back ? lastStateBlock() : firstStateBlock());
- assert((_back ? !last_defined_: !first_defined_) && "defining an already defined extreme");
- assert(extreme_state->hasLocalParametrization() && "not defined extreme without local parameterization");
-
- //std::cout << "Defining extreme --> Removing and adding state blocks and factors" << std::endl;
-
- // remove local parameterization
- extreme_state->removeLocalParametrization();
-
- // update boolean
- if (_back)
- last_defined_ = true;
- else
- first_defined_ = true;
-}
-
-bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
-{
- //std::cout << "tryClose landmark " << id() << " with tolerance = " << _dist_tol << std::endl;
-
- if (closed_)
- {
- WOLF_WARN("trying to close an already closed polyline landmark!");
- return true;
- }
- if (getNDefinedPoints() < 2)
- {
- WOLF_WARN("trying to close an polyline landmark with less than 2 defined points!");
- return false;
- }
- if (getNDefinedPoints() == 2 && getNPoints() < 4)
- {
- WOLF_WARN("trying to close an polyline landmark with 2 defined points and less than 4 points!");
- return false;
- }
-
- WOLF_TRACE("tryClose landmark ",id()," point ids: ");
- for (auto id : getValidIds())
- std::cout << id << " ";
- std::cout << std::endl;
-
- std::vector<std::pair<int, int> > overlapped_ids;
-
- // Check first defined point against the rest defined points
- int first_point_id = (first_defined_ ? first_id_ : getNextValidId(first_id_));
- int with_point_id = (last_defined_ ? last_id_ : getPrevValidId(last_id_));
- int last_with_id = first_point_id; // limit for avoiding more than one overlapping per defined point (full overlapping of defined points)
- bool found = false;
-
- // Trying to close overlapping defined points
- if (getNDefinedPoints() > 2)
- {
- // Find the point that matches with the first defined point - the search is done until full overlapping
- // last_with_id is the point overlapped with the last defined point, it CAN'T be overlapped with with_point_id since it's already overlapped with first defined point
- while (with_point_id > last_with_id)
- {
- WOLF_TRACE("first_point_id: ",first_point_id,"\nwith_point_id: ",with_point_id,"\nlast_with_id: ", last_with_id);
- if ((getPointVector(with_point_id)-getPointVector(first_point_id)).squaredNorm() < _dist_tol*_dist_tol)
- {
- found = true;
- break;
- }
-
- with_point_id=getPrevValidId(with_point_id);
- last_with_id=getNextValidId(last_with_id);
- }
-
- // IF MATCH FOUND CHECK THE WHOLE OVERLAPPING
- if (found)
- {
- WOLF_TRACE("good point match first: ", first_point_id, " with: ", with_point_id);
-
- // COMPUTE OVERLAPPING POINTS
- // if first not defined -> add previous overlapping point
- if (!first_defined_)
- {
- first_point_id = getPrevValidId(first_point_id);
- with_point_id = getPrevValidId(with_point_id);
- }
-
- // overlapped points from corresponding first point match to match with last point
- while (with_point_id <= last_id_)
- {
- overlapped_ids.push_back(std::pair<int,int>(first_point_id, with_point_id));
- if (with_point_id == last_id_)
- break;
- first_point_id = getNextValidId(first_point_id);
- with_point_id = getNextValidId(with_point_id);
- }
-
- // CHECK ALL OVERLAPPED POINTS if any overlapping candidate found (if just one overlapped point, it has already been checked)
- if (overlapped_ids.size() > 1)
- {
- // Check that the other overlapped points are close enough (0 already checked)
- for (auto i = 0; i < overlapped_ids.size(); i++)
- {
- Scalar sq_dist=1e6;
-
- // 2 defined points: point-point
- if ( (overlapped_ids[i].first != first_id_ || first_defined_) &&
- (overlapped_ids[i].second != last_id_ || last_defined_) )
- sq_dist = (getPointVector(overlapped_ids[i].first)-getPointVector(overlapped_ids[i].second)).squaredNorm();
- // defined with not defined: not defined point-segment
- else if (overlapped_ids[i].first != first_id_ || first_defined_)
- sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].second), getPointVector(overlapped_ids[i].first), getPointVector(getNextValidId(overlapped_ids[i].first)));
- // not defined with defined: not defined point-segment
- else if (overlapped_ids[i].second != last_id_ || last_defined_)
- sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].first),getPointVector(overlapped_ids[i].second), getPointVector(getPrevValidId(overlapped_ids[i].second)));
- else
- throw std::runtime_error("when trying to close, overlapping two not defined points in a landmark");
-
- if (sq_dist > _dist_tol*_dist_tol)
- {
- WOLF_TRACE("\nBad overlapping: ", sq_dist,
- "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""),
- "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : ""));
- return false;
- }
- WOLF_TRACE("\nGood overlapping: ", sq_dist,
- "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""),
- "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : ""));
- }
- }
- }
- }
-
- // Trying to close overlapping both not defined extremes
- if (overlapped_ids.empty() && !first_defined_ && !last_defined_)
- {
- WOLF_TRACE("No overlapping found with defined points, trying with not defined extremes");
-
- // Check for both not defined extremes that the distance to the segment of first and last defined points is below the threshold
- Eigen::Vector2s first_not_def = getPointVector(first_id_);
- Eigen::Vector2s last_not_def = getPointVector(last_id_);
- Eigen::Vector2s first_def = getPointVector(getNextValidId(first_id_));
- Eigen::Vector2s last_def = getPointVector(getPrevValidId(last_id_));
-
- if(sqDistPoint2Segment(first_not_def, last_def, first_def) < _dist_tol*_dist_tol &&
- sqDistPoint2Segment(last_not_def, last_def, first_def) < _dist_tol*_dist_tol)
- {
- WOLF_TRACE("The two not defined extremes close to the segment defined first_defined and last_defined:",
- "\n\tfirst point: ", first_not_def.transpose(),
- "\n\tlast point: ", last_not_def.transpose(),
- "\n\tfirst defined point: ", first_def.transpose(),
- "\n\tlast defined point: ", last_def.transpose());
-
- // Also that the projection of them in the segment should be overlapped
- // It means that last_not_def projected onto the segment (first_def-last_def) is closer to first_def than the projection of first_not_def
- Scalar last_proj_2_first_def = (last_def-first_def).dot(last_not_def -first_def) / (last_def-first_def).norm();
- Scalar first_proj_2_first_def = (last_def-first_def).dot(first_not_def-first_def) / (last_def-first_def).norm();
-
- if (last_proj_2_first_def < first_proj_2_first_def)
- {
-
- WOLF_TRACE("The not defined extremes are overlapped",
- "\n\tlast_proj_2_first_def: ", last_proj_2_first_def,
- "\n\tfirst_proj_2_first_def: ", first_proj_2_first_def);
- overlapped_ids.push_back(std::pair<int,int>(first_id_, last_id_));
- }
- }
- }
-
- // IF ANY OVERLAPPING -> CLOSE AND MERGE OVERLAPPED POINTS
- if (!overlapped_ids.empty())
- {
- WOLF_TRACE("All overlappings OK, merging ", overlapped_ids.size(), " points");
-
- // merge points
- while (!overlapped_ids.empty())
- {
- // not defined points are merged into defined ones
- if ((overlapped_ids.back().first == first_id_ && !first_defined_) || (overlapped_ids.back().first == last_id_ && !last_defined_))
- mergePoints(overlapped_ids.back().first,overlapped_ids.back().second);
- else
- mergePoints(overlapped_ids.back().second,overlapped_ids.back().first);
- overlapped_ids.pop_back();
- }
- // Close
- setClosed();
-
- WOLF_TRACE("Landmark ",id()," was closed.");
-
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
-
- return true;
- }
- return false;
-}
-
-void LandmarkPolyline2D::setClosed()
-{
- assert(getNDefinedPoints() >= 2 && "closing a polyline with less than 2 defined points");
-
- // set closed
- closed_ = true;
-
- // new version
- version_++;
-}
-
-void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
-{
- assert(point_state_ptr_map_.find(_remove_id) != point_state_ptr_map_.end() && "removing an inexistent point");
- assert(point_state_ptr_map_.find(_remain_id) != point_state_ptr_map_.end() && "merging an inexistent point");
- assert(!(_remain_id == getLastId() && !last_defined_) && "in merging points, the remaining point must be defined");
- assert(!(_remain_id == getFirstId() && !first_defined_) && "in merging points, the remaining point must be defined");
-
- //std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << first_id_ << " to " << getLastId() << ")" << std::endl;
-
- StateBlockPtr remove_state = getPointStateBlock(_remove_id);
- //std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
-
- // Change factors from remove_state to remain_state
- FactorBasePtrList old_factors_list = getConstrainedByList();
- //std::cout << "changing " << old_factors_list.size() << " factors." << std::endl;
- FactorBasePtr new_fac_ptr = nullptr;
- for (auto fac_ptr : old_factors_list)
- {
- FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
- FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
- if (fac_point_ptr)
- {
- // If landmark point constrained -> new factor
- if (fac_point_ptr->getLandmarkPointId() == _remove_id)
- {
- new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_ptr->getProcessor(),
- fac_point_ptr->getFeaturePointId(),
- _remain_id,
- fac_point_ptr->getApplyLossFunction(),
- fac_point_ptr->getStatus());
- }
- }
- else if (fac_point_line_ptr)
- {
- // If landmark point constrained -> new factor
- if (fac_point_line_ptr->getLandmarkPointId() == _remove_id)
- {
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- _remain_id,
- fac_point_line_ptr->getLandmarkPointAuxId(),
- fac_point_line_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
- }
- // If landmark point is aux point -> new factor
- else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
- {
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- fac_point_line_ptr->getLandmarkPointId(),
- _remain_id,
- fac_point_line_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
- }
- }
- else
- {
- WOLF_ERROR("polyline factor of unknown type");
- throw std::runtime_error ("polyline factor of unknown type");
- }
-
- // If new factor
- if (new_fac_ptr)
- {
- //std::cout << "created new factor: " << new_fac_ptr->id() << std::endl;
- //std::cout << "deleting factor: " << fac_ptr->id() << std::endl;
-
- // add new factor
- fac_ptr->getFeature()->addFactor(new_fac_ptr);
- addConstrainedBy(new_fac_ptr);
-
- // remove factor
- fac_ptr->remove();
-
- new_fac_ptr = nullptr;
- }
- }
-
- // If removed was a not defined extreme, set defined extreme
- if (_remove_id == first_id_ && !first_defined_)
- first_defined_ = true;
- if (_remove_id == last_id_ && !last_defined_)
- last_defined_ = true;
-
- // Remove remove_state
- if (getProblem() != nullptr)
- getProblem()->removeStateBlock(remove_state);
- //std::cout << "state removed " << std::endl;
-
- // remove element from deque
- point_state_ptr_map_.erase(_remove_id);
- //std::cout << "state removed from point vector " << std::endl;
-
- // reset first_id_ & last_id_
- first_id_ = getFirstId();
- last_id_ = getLastId();
-
- // new version
- version_++;
-
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
-}
-
-bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes)
-{
- //std::cout << "tryClassify landmark " << id() << " with " << getNDefinedPoints() << " defined points of " << getNPoints() << " - tol = " << _dist_tol << std::endl;
-
- assert(classification_.type == UNCLASSIFIED && "trying to classify a container already classified");
- assert((!closed_ || getNDefinedPoints() == 4) && "trying to classify a closed polyline of Npoints different than 4");
- assert(getNDefinedPoints() > 2 && getNDefinedPoints() < 5 && "wrong number of defined points for classification of rectangular forms");
-
- std::vector<int> points_ids = getValidIds();
- if (!first_defined_)
- points_ids.erase(points_ids.begin());
- if (!last_defined_)
- points_ids.pop_back();
-
- // consider 3 first defined points A-0, B-1, C-2 (and D-3)
- Scalar dAB = (getPointVector(points_ids[0]) - getPointVector(points_ids[1])).norm();
- Scalar dBC = (getPointVector(points_ids[1]) - getPointVector(points_ids[2])).norm();
- Scalar dAC = (getPointVector(points_ids[0]) - getPointVector(points_ids[2])).norm();
-
- //std::cout << "dAB = " << dAB << std::endl;
- //std::cout << "dBC = " << dBC << std::endl;
- //std::cout << "dAC = " << dAC << std::endl;
-
- PolylineRectangularClass pl_class;
- bool configuration = true;
-
- for (auto candidate_class : _classes)
- //for (unsigned int i = 0; i < _classes.size(); i++)
- {
- //std::cout << "Trying classification " << i << ":\n";
- //std::cout << "Configuration 1:\n\terror L: " << fabs(dAB-candidate_class.L) << " error W: " << fabs(dBC-candidate_class.W) << " error D: " << fabs(dAC-candidate_class.D) << std::endl;
- //std::cout << "Configuration 2:\n\terror L: " << fabs(dBC-candidate_class.L) << " error W: " << fabs(dAB-candidate_class.W) << " error D: " << fabs(dAC-candidate_class.D) << std::endl;
-
- // check configuration 1
- if(fabs(dAB-candidate_class.L) < _dist_tol &&
- fabs(dBC-candidate_class.W) < _dist_tol &&
- fabs(dAC-candidate_class.D) < _dist_tol)
- {
- configuration = true;
- pl_class = candidate_class;
- }
-
- // check configuration 2
- if(fabs(dAB-candidate_class.W) < _dist_tol &&
- fabs(dBC-candidate_class.L) < _dist_tol &&
- fabs(dAC-candidate_class.D) < _dist_tol)
- {
- configuration = false;
- pl_class = candidate_class;
- }
-
- // type
- if (pl_class.type != UNCLASSIFIED)
- {
- // Using 4th defined point to validate (if exists)
- if (getNDefinedPoints() == 4)
- {
- //std::cout << "checking with 4th point... " << std::endl;
-
- Scalar dAD = (getPointVector(points_ids[0]) - getPointVector(points_ids[3])).norm();
- Scalar dBD = (getPointVector(points_ids[1]) - getPointVector(points_ids[3])).norm();
- Scalar dCD = (getPointVector(points_ids[2]) - getPointVector(points_ids[3])).norm();
-
- // necessary conditions (rectangular shape) not fulfilled
- if (fabs(dAD-dBC) > _dist_tol ||
- fabs(dBD-dAC) > _dist_tol ||
- fabs(dCD-dAB) > _dist_tol)
- {
- //std::cout << "4th point doesn't match, discarding classification" << std::endl;
- pl_class = PolylineRectangularClass();
- continue;
- }
- }
- break;
- }
- }
-
- // any container position fitted
- if (pl_class.type != UNCLASSIFIED)
- {
- //std::cout << "landmark " << id() << " classified as " << pl_class.type << " in configuration " << (configuration ? "1" : "2") << std::endl;
- //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
-
- // ENSURE 4 DEFINITE POINTS
- // if not 4 defined add/define/merge points
- if (getNDefinedPoints() != 4 || getNPoints() != 4)
- {
- // Add a point if not enough points
- if (getNPoints() < 4)
- {
- addPoint(Eigen::Vector2s::Zero(), true, true);
- points_ids.push_back(last_id_);
- //std::cout << "ADDING POINT\n";
- //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
- }
- assert(getNPoints() >= 4 && "classified landmark must have 4 points");
-
- // First
- if (!first_defined_)
- {
- //std::cout << "FIRST NOT DEFINED POINT\n";
- if (getNDefinedPoints() == 4) // merge
- {
- //std::cout << "4 DEFINED POINTS\n";
- if (!last_defined_)
- mergePoints(first_id_, getPrevValidId(last_id_));
- else
- mergePoints(first_id_, last_id_);
- }
- else // define
- {
- defineFirst();
- points_ids.push_back(first_id_);
- }
- //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
- }
- assert(first_defined_ && "first point should be defined at this point");
-
- // Last
- if (!last_defined_)
- {
- //std::cout << "FIRST NOT DEFINED POINT\n";
- if (getNDefinedPoints() == 4) // merge
- {
- //std::cout << "4 DEFINED POINTS\n";
- mergePoints(last_id_, first_id_);
- }
- else // define
- {
- defineLast();
- points_ids.push_back(last_id_); // it is points_ids[3]
- }
- //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
- }
- assert(last_defined_ && first_defined_ && "classified landmark must have all extremes defined");
- assert(getNDefinedPoints() == 4 && "classified landmark must have 4 defined points");
- assert(getNPoints() == 4 && "classified landmark must have 4 points");
- assert(points_ids.size() == 4 && "classified landmark must have 4 points");
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
- }
-
- // Close
- if (!closed_)
- setClosed();
-
- // Classify
- classify(pl_class);
-
- // Move origin to to the center
- getP()->setState((getPointVector(points_ids[0]) + getPointVector(points_ids[2])) / 2.0);
- Eigen::Vector2s frame_x = (configuration ? getPointVector(points_ids[0])-getPointVector(points_ids[1]) : getPointVector(points_ids[2])-getPointVector(points_ids[1]));
- getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
-
- // Unfix origin
- getP()->unfix();
- getO()->unfix();
-
- //std::cout << "A: " << getPointVector(points_ids[0]).transpose() << std::endl;
- //std::cout << "B: " << getPointVector(points_ids[1]).transpose() << std::endl;
- //std::cout << "C: " << getPointVector(points_ids[2]).transpose() << std::endl;
- //std::cout << "frame_x: " << frame_x.transpose() << std::endl;
- //std::cout << "frame position: " << getP()->getState().transpose() << std::endl;
- //std::cout << "frame orientation: " << getO()->getState() << std::endl;
-
- // Set polyline points to its respective relative positions
- if (configuration)
- {
- getPointStateBlock(points_ids[0])->setState(Eigen::Vector2s( classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[1])->setState(Eigen::Vector2s(-classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[2])->setState(Eigen::Vector2s(-classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[3])->setState(Eigen::Vector2s( classification_.L / 2, classification_.W / 2));
- }
- else
- {
- getPointStateBlock(points_ids[0])->setState(Eigen::Vector2s(-classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[1])->setState(Eigen::Vector2s(-classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[2])->setState(Eigen::Vector2s( classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[3])->setState(Eigen::Vector2s( classification_.L / 2,-classification_.W / 2));
- }
-
- for (auto st_pair : point_state_ptr_map_)
- st_pair.second->fix();
-
- return true;
- }
- return false;
-}
-
-void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, const int& _from, const int& _to, const int& _merged_from, const int& _merged_to)
-{
- // merge landmark into this landmark. Assumptions & ideas:
- // - if merged landmark is closed, this is also closed.
- // - if this is closed and the merged landmark has not defined extremes,
- // then: if all merged defined points match with all points of this -> more than one point can be merged into the same point of this landmark.
- // - each merged landmark point is only merged with one point of this, not necessarily the opposite (previous statement).
-
-
- //std::cout << "merging landmark " << _merged_lmk->id() << " to landmark " << id() << std::endl;
- //std::cout << "\tmerged landmark from " << _merged_from << " to " << _merged_to << std::endl;
- //std::cout << "\tremaining landmark from " << _from << " to " << _to << std::endl;
- assert(!isRemoving() && !_merged_lmk->isRemoving() && "merging any landmark already merged or removed");
- assert(!(_merged_lmk->isClosed() && !this->isClosed()) && "merging a closed landmark into a not closed one");
-
- std::map<int,int> merged_id_to_id;
-
- // ------------------------ ADD POINTS (if not closed)
- if (!closed_)
- {
- // ADD FRONT POINTS
- if (_merged_from > _merged_lmk->getFirstId())
- {
- assert(_from == first_id_);
- for (int merged_id = _merged_lmk->getPrevValidId(_merged_from); ; merged_id = _merged_lmk->getPrevValidId(merged_id))
- {
- //std::cout << "adding front point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl;
- //std::cout << "Until current first point: " << first_id_ << "\n";
-
- // Add point
- addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined() ,false);
- //std::cout << "added\n";
- //std::cout << "adding correspondence" << merged_id << " with " << first_id_ << "\n";
- merged_id_to_id[merged_id] = first_id_;
- //std::cout << "added correspondence\n";
-
- // exit loop
- if (merged_id == _merged_lmk->getFirstId())
- break;
- }
- }
- // ADD BACK POINTS
- if (_merged_to < _merged_lmk->getLastId())
- {
- assert(_to == last_id_);
- for (int merged_id = _merged_lmk->getNextValidId(_merged_to); ; merged_id = _merged_lmk->getNextValidId(merged_id))
- {
- //std::cout << "adding back point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl;
- //std::cout << "Until current last point: " << last_id_ << "\n";
-
- // Add point
- addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined() ,true);
- //std::cout << "added\n";
- //std::cout << "adding correspondence" << merged_id << " with " << last_id_ << "\n";
- merged_id_to_id[merged_id] = last_id_;
- //std::cout << "added correspondence\n";
-
- // exit loop
- if (merged_id == _merged_lmk->getLastId())
- break;
- }
- }
- }
- // store id correspondences of overlapped points (also handles closed lmk)
- for (int merged_id = _merged_from, id = _from; ; merged_id = _merged_lmk->getNextValidId(merged_id), id = getNextValidId(id))
- {
- //std::cout << "correspondence points: " << id << " with " << merged_id << std::endl;
-
- WOLF_ERROR_COND(!(merged_id_to_id.find(merged_id) == merged_id_to_id.end()), "This point id was already indexed as merged:", merged_id);
- assert(merged_id_to_id.find(merged_id) == merged_id_to_id.end());
- merged_id_to_id[merged_id] = id;
-
- // Set Defined extremes if it's the case
- if (id == first_id_ && !first_defined_ && (merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined()))
- defineFirst();
- if (id == last_id_ && !last_defined_ && (merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined()))
- defineLast();
-
- // exit loop
- if (merged_id == _merged_to)
- {
- WOLF_ERROR_COND(!(id == _to && merged_id == _merged_to), "Amount of merged points should be the same for remaining and merged landmarks.");
- assert(id == _to && merged_id == _merged_to);
- break;
- }
- }
-
- // ------------------------- COPY FACTORS
- FactorBasePtrList old_factors_list = _merged_lmk->getConstrainedByList();
- //std::cout << "\tchanging " << old_factors_list.size() << " factors." << std::endl;
- FactorBasePtr new_fac_ptr = nullptr;
- for (auto fac_ptr : old_factors_list)
- {
- //std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOther()->id() << std::endl;
- assert(fac_ptr->getLandmarkOther() == _merged_lmk);
-
- FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
- FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
-
- // point 2 point
- if (fac_point_ptr)
- {
- //std::cout << "\t\tpoint-point factor to id: " << fac_point_ptr->getLandmarkPointId() << std::endl;
- assert(merged_id_to_id.find(fac_point_ptr->getLandmarkPointId()) != merged_id_to_id.end());
-
- new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_ptr->getProcessor(),
- fac_point_ptr->getFeaturePointId(),
- merged_id_to_id[fac_point_ptr->getLandmarkPointId()],
- fac_point_ptr->getApplyLossFunction(),
- fac_point_ptr->getStatus());
-
- }
- // point 2 line
- else if (fac_point_line_ptr)
- {
- //std::cout << "\t\tpoint-line factor to ids: " << fac_point_line_ptr->getLandmarkPointId() << ", " << fac_point_line_ptr->getLandmarkPointAuxId() << std::endl;
- assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointId()) != merged_id_to_id.end());
- assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointAuxId()) != merged_id_to_id.end());
-
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- merged_id_to_id[fac_point_line_ptr->getLandmarkPointId()],
- merged_id_to_id[fac_point_line_ptr->getLandmarkPointAuxId()],
- fac_point_line_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
-
- }
- else
- {
- WOLF_ERROR("polyline factor of unknown type");
- throw std::runtime_error ("polyline factor of unknown type");
- }
- // Establish new factor
- assert(new_fac_ptr != nullptr);
- //std::cout << "\t\tcreated new factor: " << new_fac_ptr->id() << std::endl;
- //std::cout << "\t\tdeleting factor: " << fac_ptr->id() << std::endl;
-
- // add new factor
- fac_ptr->getFeature()->addFactor(new_fac_ptr);
- addConstrainedBy(new_fac_ptr);
-
- // remove factor
- fac_ptr->remove();
- //std::cout << "\t\tdeleted\n";
- }
-
- // ------------------------- REMOVE _merged_lmk
- _merged_lmk->setMergedInLandmark(std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()));
- _merged_lmk->remove();
- //std::cout << "\tLandmark deleted\n";
-}
-
-void LandmarkPolyline2D::registerNewStateBlocks()
-{
- LandmarkBase::registerNewStateBlocks();
- if (getProblem())
- for (auto state_it : point_state_ptr_map_)
- getProblem()->addStateBlock(state_it.second);
-}
-
-void LandmarkPolyline2D::removeStateBlocks()
-{
- auto sbp_pair = point_state_ptr_map_.begin();
- while (sbp_pair != point_state_ptr_map_.end())
- {
- if (getProblem())
- getProblem()->removeStateBlock(sbp_pair->second);
-
- sbp_pair = point_state_ptr_map_.erase(sbp_pair);
- }
- LandmarkBase::removeStateBlocks();
-}
-
-// static
-LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
-{
- // Parse YAML node with lmk info and data
- unsigned int id = _lmk_node["id"].as<unsigned int>();
- Eigen::VectorXs pos = _lmk_node["position"].as<Eigen::VectorXs>();
- bool pos_fixed = true;//_lmk_node["position fixed"].as<bool>();
- Eigen::VectorXs ori = _lmk_node["orientation"].as<Eigen::VectorXs>();
- bool ori_fixed = true;//_lmk_node["orientation fixed"].as<bool>();
- int first_id = _lmk_node["first_id"].as<int>();
- bool first_defined = _lmk_node["first_defined"].as<bool>();
- bool last_defined = _lmk_node["last_defined"].as<bool>();
- unsigned int npoints = _lmk_node["points"].size();
- int classif_type = _lmk_node["classification_type"].as<int>();
- Scalar classif_L = _lmk_node["classification_L"].as<Scalar>();
- Scalar classif_W = _lmk_node["classification_W"].as<Scalar>();
-
- // load points
- Eigen::MatrixXs points(2,npoints);
- for (unsigned int i = 0; i < npoints; i++)
- points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2s>();
-
- // Create a new landmark
- LandmarkPolyline2DPtr lmk_ptr = std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(pos, pos_fixed),
- std::make_shared<StateBlock>(ori, ori_fixed),
- points,
- first_defined,
- last_defined,
- first_id,
- PolylineRectangularClass(static_cast<PolylineClassType>(classif_type),
- classif_L,
- classif_W));
- lmk_ptr->setId(id);
-
- // fix all points
- for (auto st_pair : lmk_ptr->getPointStatePtrMap())
- st_pair.second->fix();
-
- return lmk_ptr;
-}
-
-YAML::Node LandmarkPolyline2D::saveToYaml() const
-{
- // First base things
- YAML::Node node = LandmarkBase::saveToYaml();
-
- // Then add specific things
- node["first_id"] = first_id_;
- node["first_defined"] = first_defined_;
- node["last_defined"] = last_defined_;
- node["classification_type"] = static_cast<int>(classification_.type);
- node["classification_L"] = classification_.L;
- node["classification_W"] = classification_.W;
-
- for (auto st : getPointsStateBlockVector())
- node["points"].push_back(st->getState());
-
- return node;
-}
-
-void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol)
-{
- std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n";
- assert(_lmk_list.size() >= 2);
-
- // 2 by 2
- LandmarkPolyline2DPtr remaining_lmk = _lmk_list.front();
- _lmk_list.pop_front();
- LandmarkPolyline2DPtr lmk_1, lmk_2, merged_lmk;
- int remaining_from, remaining_to, merged_from, merged_to;
-
- while (!_lmk_list.empty())
- {
- lmk_1 = remaining_lmk;
- lmk_2 = _lmk_list.front();
- _lmk_list.pop_front();
-
- if (lmk_2->isRemoving())
- continue;
-
- std::cout << "Candidates to be merged: " << lmk_1->id() << " " << lmk_2->id() << "\n";
-
- // check best correspondence
- Eigen::MatrixXs points_1 = lmk_1->computePointsGlobal();
- Eigen::MatrixXs points_2 = lmk_2->computePointsGlobal();
- MatchPolyline2DPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(),
- points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(), _sq_dist_tol);
-
- // match
- if ( match )
- {
- std::vector<int> valid_id_1 = lmk_1->getValidIds();
- std::vector<int> valid_id_2 = lmk_2->getValidIds();
-
- // decide which is the remaining landmark:
- // If only one closed, this one remains.
- // Otherwise (both closed or both not closed) the one with most factors remains
- if ((lmk_1->isClosed() && !lmk_2->isClosed()) ||
- (lmk_1->isClosed() == lmk_2->isClosed() && lmk_1->getHits() > lmk_2->getHits()))
- {
- remaining_lmk = lmk_1;
- merged_lmk = lmk_2;
- remaining_from = valid_id_1.at(match->from_A_id_);
- remaining_to = valid_id_1.at(match->to_A_id_);
- merged_from = valid_id_2.at(match->from_B_id_);
- merged_to = valid_id_2.at(match->to_B_id_);
- }
- else
- {
- remaining_lmk = lmk_2;
- merged_lmk = lmk_1;
- remaining_from = valid_id_2.at(match->from_B_id_);
- remaining_to = valid_id_2.at(match->to_B_id_);
- merged_from = valid_id_1.at(match->from_A_id_);
- merged_to = valid_id_1.at(match->to_A_id_);
- }
-
- // merge 2 landmarks
- remaining_lmk->mergeLandmark(merged_lmk, remaining_from, remaining_to, merged_from, merged_to);
-
- // reset pointers
- lmk_1.reset();
- lmk_2.reset();
- assert(merged_lmk->getHits() == 0);
- std::cout << "merged\n";
- }
- // no match (remaining: most factors)
- else
- remaining_lmk = (lmk_1->getHits() > lmk_2->getHits() ? lmk_1 : lmk_2);
- }
- std::cout << "tryMergeLandmarks::done\n";
-}
-
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_polyline_2D = LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
-}
-
-} /* namespace wolf */
diff --git a/src/local_parametrization_base.cpp b/src/local_parametrization_base.cpp
deleted file mode 100644
index e2bc1cf4e36aafd25c8209826d3c8afe43828d62..0000000000000000000000000000000000000000
--- a/src/local_parametrization_base.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "base/local_parametrization_base.h"
-
-namespace wolf {
-
-LocalParametrizationBase::LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size) :
- global_size_(_global_size), local_size_(_local_size)
-{
-}
-
-LocalParametrizationBase::~LocalParametrizationBase()
-{
-}
-
-unsigned int LocalParametrizationBase::getLocalSize() const
-{
- return local_size_;
-}
-
-unsigned int LocalParametrizationBase::getGlobalSize() const
-{
- return global_size_;
-}
-
-} // namespace wolf
diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
deleted file mode 100644
index 14abaecd1a9fd93cfd1da4b9a6e1e1d0f9982dbd..0000000000000000000000000000000000000000
--- a/src/local_parametrization_homogeneous.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * \file local_parametrization_homogeneous.cpp
- *
- * Created on: 24/02/2016
- * Author: jsola
- */
-
-#include "base/local_parametrization_homogeneous.h"
-#include "iostream"
-#include "base/rotations.h" // we use quaternion algebra here
-
-namespace wolf {
-
-LocalParametrizationHomogeneous::LocalParametrizationHomogeneous() :
- LocalParametrizationBase(4, 3)
-{
- //
-}
-
-LocalParametrizationHomogeneous::~LocalParametrizationHomogeneous()
-{
- //
-}
-
-bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const
-{
- assert(_h.size() == global_size_ && "Wrong size of input homogeneous point.");
- assert(_delta.size() == local_size_ && "Wrong size of input delta.");
- assert(_h_plus_delta.size() == global_size_ && "Wrong size of output homogeneous point.");
-
- using namespace Eigen;
-
- _h_plus_delta = ( exp_q(_delta) * Quaternions(_h.data()) ).coeffs();
-
- return true;
-}
-
-bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
-{
- assert(_h.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
-
- Eigen::Vector4s hh = _h/2;
- _jacobian << hh(3), hh(2), -hh(1),
- -hh(2), hh(3), hh(0),
- hh(1), -hh(0), hh(3),
- -hh(0), -hh(1), -hh(2) ;
- return true;
-}
-
-bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXs>& _h1,
- Eigen::Map<const Eigen::VectorXs>& _h2,
- Eigen::Map<Eigen::VectorXs>& _h2_minus_h1)
-{
- using Eigen::Quaternions;
- _h2_minus_h1 = log_q(Quaternions(_h2.data()) * Quaternions(_h1.data()).conjugate());
- return true;
-}
-
-} // namespace wolf
-
diff --git a/src/local_parametrization_polyline_extreme.cpp b/src/local_parametrization_polyline_extreme.cpp
deleted file mode 100644
index da6851691bfc97d6178e5e55073691b4730064b8..0000000000000000000000000000000000000000
--- a/src/local_parametrization_polyline_extreme.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-#include "base/local_parametrization_polyline_extreme.h"
-#include "base/state_block.h"
-#include "base/rotations.h"
-
-namespace wolf {
-
-LocalParametrizationPolylineExtreme::LocalParametrizationPolylineExtreme(StateBlockPtr _reference_point) :
- LocalParametrizationBase(2, 1),
- reference_point_(_reference_point)
-{
-}
-
-LocalParametrizationPolylineExtreme::~LocalParametrizationPolylineExtreme()
-{
-}
-
-bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const
-{
-
- assert(_point.size() == global_size_ && "Wrong size of input point.");
- assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
- assert(_point_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
-
- _point_plus_delta_theta = reference_point_->getState().head(2) + Eigen::Rotation2Ds(_delta_theta(0)) * (_point - reference_point_->getState().head(2));
-
- assert(std::fabs( (_point_plus_delta_theta - reference_point_->getState().head(2)).squaredNorm()
- -(_point - reference_point_->getState().head(2)).squaredNorm() ) < Constants::EPS);
-
- return true;
-}
-
-bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
-{
- assert(_point.size() == global_size_ && "Wrong size of input point.");
- assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
-
- _jacobian(0,0) = reference_point_->getState()(1) - _point(1);
- _jacobian(1,0) = _point(0) - reference_point_->getState()(0);
-
- return true;
-}
-
-bool LocalParametrizationPolylineExtreme::minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta)
-{
- Eigen::Vector2s v1 = _point1 - reference_point_->getState().head(2);
- Eigen::Vector2s v2 = _point2 - reference_point_->getState().head(2);
-
- _delta_theta(0) = pi2pi(atan2(v2(1),v2(0)) - atan2(v1(1),v1(0)));
-
- return true;
-}
-
-} // namespace wolf
-
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
deleted file mode 100644
index d2af8f544f21f8f83528c7db6371f0a8407f12d0..0000000000000000000000000000000000000000
--- a/src/local_parametrization_quaternion.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-
-#include "base/local_parametrization_quaternion.h"
-#include "base/rotations.h"
-
-#include <iostream>
-namespace wolf {
-
-////////// LOCAL PERTURBATION //////////////
-
-template <>
-bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
-{
-
- assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
- assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
-
- assert(fabs(1.0 - _q.norm()) < Constants::EPS && "Quaternion not normalized.");
-
- using namespace Eigen;
-
- _q_plus_delta_theta = ( Quaternions(_q.data()) * exp_q(_delta_theta) ).coeffs();
-
- return true;
-}
-
-template <>
-bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
-{
- assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
-
- Eigen::Vector4s qq = _q/2;
- _jacobian << qq(3), -qq(2), qq(1),
- qq(2), qq(3), -qq(0),
- -qq(1), qq(0), qq(3),
- -qq(0), -qq(1), -qq(2) ;
-
- return true;
-}
-
-template <>
-bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
- Eigen::Map<const Eigen::VectorXs>& _q2,
- Eigen::Map<Eigen::VectorXs>& _q2_minus_q1)
-{
- assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
-
- using Eigen::Quaternions;
- _q2_minus_q1 = log_q(Quaternions(_q1.data()).conjugate() * Quaternions(_q2.data()));
-
- return true;
-}
-
-////////// GLOBAL PERTURBATION //////////////
-
-template <>
-bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
-{
-
- assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
- assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
-
- assert(fabs(1.0 - _q.norm()) < Constants::EPS && "Quaternion not normalized.");
-
- using namespace Eigen;
-
- _q_plus_delta_theta = ( exp_q(_delta_theta) * Quaternions(_q.data()) ).coeffs();
-
- return true;
-}
-
-template <>
-bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
-{
- assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
-
- Eigen::Vector4s qq = _q/2;
- _jacobian << qq(3), qq(2), -qq(1),
- -qq(2), qq(3), qq(0),
- qq(1), -qq(0), qq(3),
- -qq(0), -qq(1), -qq(2);
-
- return true;
-}
-
-template <>
-bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
- Eigen::Map<const Eigen::VectorXs>& _q2,
- Eigen::Map<Eigen::VectorXs>& _q2_minus_q1)
-{
- assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
-
- using Eigen::Quaternions;
- _q2_minus_q1 = log_q(Quaternions(_q2.data()) * Quaternions(_q1.data()).conjugate());
-
- return true;
-}
-
-} // namespace wolf
diff --git a/src/map_base.cpp b/src/map_base.cpp
deleted file mode 100644
index d1ad03121691b64052bfab01372010f68de863f5..0000000000000000000000000000000000000000
--- a/src/map_base.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-
-// wolf
-#include "base/map_base.h"
-#include "base/landmark/landmark_base.h"
-#include "base/factory.h"
-
-// YAML
-#include <yaml-cpp/yaml.h>
-
-// stl
-#include <fstream>
-#include <ctime>
-#include <iomanip>
-#include <iostream>
-#include <sstream>
-
-namespace wolf {
-
-MapBase::MapBase() :
- NodeBase("MAP", "Base")
-{
-// std::cout << "constructed M"<< std::endl;
-}
-
-MapBase::~MapBase()
-{
-// std::cout << "destructed -M" << std::endl;
-}
-
-LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
-{
- landmark_list_.push_back(_landmark_ptr);
- _landmark_ptr->setMap(shared_from_this());
- _landmark_ptr->setProblem(getProblem());
- _landmark_ptr->registerNewStateBlocks();
- return _landmark_ptr;
-}
-
-void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
-{
- LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
- for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
- {
- landmark_ptr->setMap(shared_from_this());
- landmark_ptr->setProblem(getProblem());
- landmark_ptr->registerNewStateBlocks();
- }
- landmark_list_.splice(landmark_list_.end(), _landmark_list);
-}
-
-void MapBase::load(const std::string& _map_file_dot_yaml)
-{
- YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
-
- unsigned int nlandmarks = map["nlandmarks"].as<unsigned int>();
-
- assert(nlandmarks == map["landmarks"].size() && "Number of landmarks in map file does not match!");
-
- for (unsigned int i = 0; i < nlandmarks; i++)
- {
- YAML::Node lmk_node = map["landmarks"][i];
- LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create(lmk_node["type"].as<std::string>(), lmk_node);
- addLandmark(lmk_ptr);
- }
-
-}
-
-void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name)
-{
- 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 << "landmarks" << YAML::BeginSeq;
-
- for (LandmarkBasePtr lmk_ptr : getLandmarkList())
- {
- emitter << YAML::Flow << lmk_ptr->saveToYaml();
- }
- emitter << YAML::EndSeq << YAML::EndMap;
-
- std::ofstream fout(_map_file_yaml);
- fout << emitter.c_str();
- fout.close();
-}
-
-std::string MapBase::dateTimeNow()
-{
- // Get date and time for archiving purposes
- std::time_t rawtime;
- std::time(&rawtime);
- const std::tm* timeinfo = std::localtime(&rawtime);
- char time_char[30];
- std::strftime(time_char, sizeof(time_char), "%d/%m/%Y at %H:%M:%S", timeinfo);
- std::string date_time(time_char);
- return date_time;
-}
-
-} // namespace wolf
diff --git a/src/motion_buffer.cpp b/src/motion_buffer.cpp
deleted file mode 100644
index 44b7d5856c7c5b46b380d3b1364f24cff09617f6..0000000000000000000000000000000000000000
--- a/src/motion_buffer.cpp
+++ /dev/null
@@ -1,225 +0,0 @@
-#include "base/motion_buffer.h"
-namespace wolf
-{
-
-Motion::Motion(const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _delta,
- const MatrixXs& _delta_cov,
- const VectorXs& _delta_integr,
- const MatrixXs& _delta_integr_cov,
- const MatrixXs& _jac_delta,
- const MatrixXs& _jac_delta_int,
- const MatrixXs& _jac_calib) :
- data_size_(_data.size()),
- delta_size_(_delta.size()),
- cov_size_(_delta_cov.cols()),
- calib_size_(_jac_calib.cols()),
- ts_(_ts),
- data_(_data),
- data_cov_(_data_cov),
- delta_(_delta),
- delta_cov_(_delta_cov),
- delta_integr_(_delta_integr),
- delta_integr_cov_(_delta_integr_cov),
- jacobian_delta_(_jac_delta),
- jacobian_delta_integr_(_jac_delta_int),
- jacobian_calib_(_jac_calib)
-{
-}
-
-Motion::Motion(const TimeStamp& _ts,
- SizeEigen _data_size,
- SizeEigen _delta_size,
- SizeEigen _cov_size,
- SizeEigen _calib_size) :
- data_size_(_data_size),
- delta_size_(_delta_size),
- cov_size_(_cov_size),
- calib_size_(_calib_size),
- ts_(_ts)
-{
- resize(_data_size, _delta_size, _cov_size, _calib_size);
-}
-
-Motion::~Motion()
-{
-}
-
-void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s)
-{
- WOLF_DEBUG("Motion::resize\n_data_s: ", _data_s, "\n_delta_s: ", _delta_s, "\n_delta_cov_s: ", _delta_cov_s, "\n_calib_s: ", _calib_s);
- data_.resize(_data_s);
- data_cov_.resize(_data_s, _data_s);
- delta_.resize(_delta_s);
- delta_cov_.resize(_delta_cov_s, _delta_cov_s);
- delta_integr_.resize(_delta_s);
- delta_integr_cov_.resize(_delta_cov_s, _delta_cov_s);
- jacobian_delta_.resize(_delta_cov_s, _delta_cov_s);
- jacobian_delta_integr_.resize(_delta_cov_s, _delta_cov_s);
- jacobian_calib_.resize(_delta_cov_s, _calib_s);
-}
-
-////////////////////////////////////////////////////////////////////////////////////////
-
-MotionBuffer::MotionBuffer(SizeEigen _data_size,
- SizeEigen _delta_size,
- SizeEigen _cov_size,
- SizeEigen _calib_size) :
- data_size_(_data_size),
- delta_size_(_delta_size),
- cov_size_(_cov_size),
- calib_size_(_calib_size),
- calib_preint_(_calib_size)
-{
- container_.clear();
-}
-
-const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
-{
- //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
- {
- return m.ts_ <= _ts;
- });
- if (previous == container_.rend())
- // The time stamp is older than the buffer's oldest data.
- // We could do something here, and throw an error or something, but by now we'll return the first valid data
- previous--;
-
- return *previous;
-}
-
-void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
-{
- //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
- {
- return m.ts_ <= _ts;
- });
- if (previous == container_.rend())
- // The time stamp is older than the buffer's oldest data.
- // We could do something here, but by now we'll return the last valid data
- previous--;
-
- _motion = *previous;
-}
-
-void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
-{
- assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
-
- _buffer_part_before_ts.setCalibrationPreint(calib_preint_);
-
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
- {
- return m.ts_ <= _ts;
- });
- if (previous == container_.rend())
- {
- // The time stamp is more recent than the buffer's most recent data:
- // return an empty buffer as the _oldest_buffer
- _buffer_part_before_ts.get().clear();
- }
- else
- {
- // Transfer part of the buffer
- _buffer_part_before_ts.container_.splice(_buffer_part_before_ts.container_.begin(),
- container_,
- container_.begin(),
- (previous--).base());
- }
-}
-
-//MatrixXs MotionBuffer::integrateCovariance() const
-//{
-// Eigen::MatrixXs delta_integr_cov(cov_size_, cov_size_);
-// delta_integr_cov.setZero();
-// for (Motion mot : container_)
-// {
-// delta_integr_cov = mot.jacobian_delta_integr_ * delta_integr_cov * mot.jacobian_delta_integr_.transpose()
-// + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
-// }
-// return delta_integr_cov;
-//}
-//
-//MatrixXs MotionBuffer::integrateCovariance(const TimeStamp& _ts) const
-//{
-// Eigen::MatrixXs delta_integr_cov(cov_size_, cov_size_);
-// delta_integr_cov.setZero();
-// for (Motion mot : container_)
-// {
-// if (mot.ts_ > _ts)
-// break;
-//
-// delta_integr_cov = mot.jacobian_delta_integr_ * delta_integr_cov * mot.jacobian_delta_integr_.transpose()
-// + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
-// }
-// return delta_integr_cov;
-//}
-//
-//MatrixXs MotionBuffer::integrateCovariance(const TimeStamp& _ts_1, const TimeStamp _ts_2) const
-//{
-// Eigen::MatrixXs cov(cov_size_, cov_size_);
-// cov.setZero();
-// for (Motion mot : container_)
-// {
-// if (mot.ts_ > _ts_2)
-// break;
-//
-// if (mot.ts_ >= _ts_1)
-// cov = mot.jacobian_delta_integr_ * cov * mot.jacobian_delta_integr_.transpose()
-// + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
-// }
-// return cov;
-//}
-
-void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs)
-{
- using std::cout;
- using std::endl;
-
- if (!show_data && !show_delta && !show_delta_int && !show_jacs)
- {
- cout << "Buffer state [" << container_.size() << "] : <";
- for (Motion mot : container_)
- cout << " " << mot.ts_;
- cout << " >" << endl;
- }
- else
- {
- print(0,0,0,0);
- for (Motion mot : container_)
- {
- cout << "-- Motion (" << mot.ts_ << ")" << endl;
-// if (show_ts)
-// cout << " ts: " << mot.ts_ << endl;
- if (show_data)
- {
- cout << " data: " << mot.data_.transpose() << endl;
- 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_delta_int)
- {
- cout << " delta integrated: " << mot.delta_integr_.transpose() << endl;
- cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
- }
- if (show_jacs)
- {
- cout << " Jac delta: \n" << mot.jacobian_delta_ << endl;
- cout << " Jac delta integr: \n" << mot.jacobian_delta_integr_ << endl;
- cout << " Jac calib: \n" << mot.jacobian_calib_ << endl;
-
- }
- }
- }
-}
-
-}
-
diff --git a/src/node_base.cpp b/src/node_base.cpp
deleted file mode 100644
index 24cb56c200843b2c816cb243de2096b9ebcd7f98..0000000000000000000000000000000000000000
--- a/src/node_base.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-#include "base/node_base.h"
-
-namespace wolf {
-
-//init static node counter
-unsigned int NodeBase::node_id_count_ = 0;
-
-} // namespace wolf
diff --git a/src/problem.cpp b/src/problem.cpp
deleted file mode 100644
index 729e1b81a26c14f6a33be2337c533081347a437a..0000000000000000000000000000000000000000
--- a/src/problem.cpp
+++ /dev/null
@@ -1,1383 +0,0 @@
-// wolf includes
-#include "base/problem.h"
-#include "base/hardware_base.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
-#include "base/sensor/sensor_base.h"
-#include "base/processor/processor_motion.h"
-#include "base/processor/processor_tracker.h"
-#include "base/capture/capture_pose.h"
-#include "base/factor/factor_base.h"
-#include "base/sensor/sensor_factory.h"
-#include "base/processor/processor_factory.h"
-#include "base/state_block.h"
-
-
-// IRI libs includes
-
-// C++ includes
-#include <algorithm>
-
-namespace wolf
-{
-
-// unnamed namespace used for helper functions local to this file.
-namespace
-{
-std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;}
-}
-
-Problem::Problem(const std::string& _frame_structure) :
- hardware_ptr_(std::make_shared<HardwareBase>()),
- trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
- map_ptr_(std::make_shared<MapBase>()),
- processor_motion_ptr_(),
- prior_is_set_(false)
-{
- if (_frame_structure == "PO 2D")
- {
- state_size_ = 3;
- state_cov_size_ = 3;
- dim_ = 2;
- }
-
- else if (_frame_structure == "PO 3D")
- {
- state_size_ = 7;
- state_cov_size_ = 6;
- dim_ = 3;
- }
- else if (_frame_structure == "POV 3D")
- {
- state_size_ = 10;
- state_cov_size_ = 9;
- dim_ = 3;
- }
- else std::runtime_error(
- "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
-
-}
-
-void Problem::setup()
-{
- hardware_ptr_ -> setProblem(shared_from_this());
- trajectory_ptr_-> setProblem(shared_from_this());
- map_ptr_ -> setProblem(shared_from_this());
-}
-
-ProblemPtr Problem::create(const std::string& _frame_structure)
-{
- ProblemPtr p(new Problem(_frame_structure)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
- p->setup();
- return p->shared_from_this();
-}
-
-Problem::~Problem()
-{
- // WOLF_DEBUG("destructed -P");
-}
-
-void Problem::addSensor(SensorBasePtr _sen_ptr)
-{
- getHardware()->addSensor(_sen_ptr);
-}
-
-SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
- IntrinsicsBasePtr _intrinsics)
-{
- SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
- addSensor(sen_ptr);
- return sen_ptr;
-}
-
-SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
- const std::string& _intrinsics_filename)
-{
-
- if (_intrinsics_filename != "")
- {
- assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
- IntrinsicsBasePtr intr_ptr = IntrinsicsFactory::get().create(_sen_type, _intrinsics_filename);
- return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
- }
- else
- return installSensor(_sen_type, _unique_sensor_name, _extrinsics, IntrinsicsBasePtr());
-
-}
-
-ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- SensorBasePtr _corresponding_sensor_ptr, //
- ProcessorParamsBasePtr _prc_params)
-{
- if (_corresponding_sensor_ptr == nullptr)
- {
- WOLF_ERROR("Cannot install processor '", _unique_processor_name,
- "' since the associated sensor does not exist !");
- return ProcessorBasePtr();
- }
-
- ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params, _corresponding_sensor_ptr);
- prc_ptr->configure(_corresponding_sensor_ptr);
- _corresponding_sensor_ptr->addProcessor(prc_ptr);
-
- // setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
-
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
-
- return prc_ptr;
-}
-
-ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- const std::string& _corresponding_sensor_name, //
- const std::string& _params_filename)
-{
- SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
- if (sen_ptr == nullptr)
- throw std::runtime_error("Sensor not found. Cannot bind Processor.");
- if (_params_filename == "")
- return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
- else
- {
- assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
- ProcessorParamsBasePtr prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename);
- return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
- }
-}
-
-SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
-{
- auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
- getHardware()->getSensorList().end(),
- [&](SensorBasePtr sb)
- {
- return sb->getName() == _sensor_name;
- }); // lambda function for the find_if
-
- if (sen_it == getHardware()->getSensorList().end())
- return nullptr;
-
- return (*sen_it);
-}
-
-ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
-{
- for (auto sen : getHardware()->getSensorList()) // loop all sensors
- {
- auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
- sen->getProcessorList().end(),
- [&](ProcessorBasePtr prc)
- {
- return prc->getName() == _processor_name;
- }); // lambda function for the find_if
-
- if (prc_it != sen->getProcessorList().end()) // found something!
- {
- if ((*prc_it)->isMotion()) // found, and it's motion!
- {
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(*prc_it);
- return processor_motion_ptr_;
- }
- else // found, but it's not motion!
- {
- return nullptr;
- }
- }
- }
- // nothing found!
- WOLF_ERROR("Processor '%s' not found!",_processor_name);
- return nullptr;
-}
-
-void Problem::setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr)
-{
- processor_motion_ptr_ = _processor_motion_ptr;
-}
-
-void Problem::clearProcessorMotion()
-{
- processor_motion_ptr_.reset();
-}
-
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
- FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
- const TimeStamp& _time_stamp)
-{
- FrameBasePtr frm = FrameFactory::get().create(_frame_structure, _frame_key_type, _time_stamp, _frame_state);
- trajectory_ptr_->addFrame(frm);
- return frm;
-}
-
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
- FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
-{
- return emplaceFrame(_frame_structure, _frame_key_type, getState(_time_stamp), _time_stamp);
-}
-
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
- const TimeStamp& _time_stamp)
-{
- return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _frame_state, _time_stamp);
-}
-
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
-{
- return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _time_stamp);
-}
-
-Eigen::VectorXs Problem::getCurrentState()
-{
- Eigen::VectorXs state(getFrameStructureSize());
- getCurrentState(state);
- return state;
-}
-
-void Problem::getCurrentState(Eigen::VectorXs& state)
-{
- if (processor_motion_ptr_ != nullptr)
- processor_motion_ptr_->getCurrentState(state);
- else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
- trajectory_ptr_->getLastKeyFrame()->getState(state);
- else
- state = zeroState();
-}
-
-void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
-{
- if (processor_motion_ptr_ != nullptr)
- {
- processor_motion_ptr_->getCurrentState(state);
- processor_motion_ptr_->getCurrentTimeStamp(ts);
- }
- else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
- {
- trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts);
- trajectory_ptr_->getLastKeyFrame()->getState(state);
- }
- else
- state = zeroState();
-}
-
-void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
-{
- // try to get the state from processor_motion if any, otherwise...
- if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
- {
- FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
- if (closest_frame != nullptr)
- closest_frame->getState(state);
- else
- state = zeroState();
- }
-}
-
-Eigen::VectorXs Problem::getState(const TimeStamp& _ts)
-{
- Eigen::VectorXs state(getFrameStructureSize());
- getState(_ts, state);
- return state;
-}
-
-SizeEigen Problem::getFrameStructureSize() const
-{
- return state_size_;
-}
-
-void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const
-{
- _x_size = state_size_;
- _cov_size = state_cov_size_;
-}
-
-SizeEigen Problem::getDim() const
-{
- return dim_;
-}
-
-Eigen::VectorXs Problem::zeroState()
-{
- Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());
-
- // Set the quaternion identity for 3D states. Set only the real part to 1:
- if (trajectory_ptr_->getFrameStructure() == "PO 3D" ||
- trajectory_ptr_->getFrameStructure() == "POV 3D")
- state(6) = 1.0;
-
- return state;
-}
-
-bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
-{
- // This should implement a black list of processors that have forbidden keyframe creation
- // This decision is made at problem level, not at processor configuration level.
- // If you want to configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
-
- // Currently allowing all processors to vote:
- return true;
-}
-
-void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
-{
- if (_processor_ptr)
- {
- WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
- }
- else
- {
- WOLF_DEBUG("External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
- }
-
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if (processor && (processor != _processor_ptr) )
- processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
-}
-
-LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
-{
- getMap()->addLandmark(_lmk_ptr);
- return _lmk_ptr;
-}
-
-void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
-{
- getMap()->addLandmarkList(_lmk_list);
-}
-
-StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
-{
- //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end())
- {
- WOLF_WARN("Adding a state block that has already been added");
- return _state_ptr;
- }
-
- // add the state unit to the list
- state_block_list_.push_back(_state_ptr);
-
- // Add add notification
- // Check if there is already a notification for this state block
- auto notification_it = state_block_notification_map_.find(_state_ptr);
- if (notification_it != state_block_notification_map_.end() && notification_it->second == ADD)
- {
- WOLF_WARN("There is already an ADD notification of this state block");
- }
- else
- state_block_notification_map_[_state_ptr] = ADD;
-
- return _state_ptr;
-}
-
-void Problem::removeStateBlock(StateBlockPtr _state_ptr)
-{
- //std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
- //assert(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end() && "Removing a state_block that hasn't been added or already removed");
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) == state_block_list_.end())
- {
- WOLF_WARN("Removing a state_block that hasn't been added or already removed");
- return;
- }
-
- // add the state unit to the list
- state_block_list_.remove(_state_ptr);
-
- // Check if there is already a notification for this state block
- auto notification_it = state_block_notification_map_.find(_state_ptr);
- if (notification_it != state_block_notification_map_.end())
- {
- if (notification_it->second == REMOVE)
- {
- WOLF_WARN("There is already an REMOVE notification of this state block");
- }
- // Remove ADD notification
- else
- {
- state_block_notification_map_.erase(notification_it);
- }
- }
- // Add REMOVE notification
- else
- state_block_notification_map_[_state_ptr] = REMOVE;
-}
-
-FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
-{
- //std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
-
- // Add ADD notification
- // Check if there is already a notification for this state block
- auto notification_it = factor_notification_map_.find(_factor_ptr);
- if (notification_it != factor_notification_map_.end() && notification_it->second == ADD)
- {
- WOLF_WARN("There is already an ADD notification of this factor");
- }
- // Add ADD notification (override in case of REMOVE)
- else
- factor_notification_map_[_factor_ptr] = ADD;
-
- return _factor_ptr;
-}
-
-void Problem::removeFactor(FactorBasePtr _factor_ptr)
-{
- //std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
-
- // Check if there is already a notification for this state block
- auto notification_it = factor_notification_map_.find(_factor_ptr);
- if (notification_it != factor_notification_map_.end())
- {
- if (notification_it->second == REMOVE)
- {
- WOLF_WARN("There is already an REMOVE notification of this state block");
- }
- // Remove ADD notification
- else
- factor_notification_map_.erase(notification_it);
- }
- // Add REMOVE notification
- else
- factor_notification_map_[_factor_ptr] = REMOVE;
-}
-
-void Problem::clearCovariance()
-{
- covariances_.clear();
-}
-
-void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov)
-{
- assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
- assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
-
- covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
-}
-
-void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov)
-{
- assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
- assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
-
- covariances_[std::make_pair(_state1, _state1)] = _cov;
-}
-
-bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row,
- const int _col)
-{
- //std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
- //std::cout << "_row " << _row << std::endl;
- //std::cout << "_col " << _col << std::endl;
- //std::cout << "_state1 size: " << _state1->getSize() << std::endl;
- //std::cout << "_state2 size: " << _state2->getSize() << std::endl;
- //std::cout << "part of cov to be filled:" << std::endl << _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) << std::endl;
- //if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
- // std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] << std::endl;
- //else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
- // std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose() << std::endl;
-
- assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-
- if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
- _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
- covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
- else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
- _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
- covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose();
- else
- {
- WOLF_DEBUG("Could not find the requested covariance block.");
- return false;
- }
-
- return true;
-}
-
-bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
-{
- // fill covariance
- for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
- for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
- {
- StateBlockPtr sb1 = it1->first;
- StateBlockPtr sb2 = it2->first;
- std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2);
- std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1);
-
- // search st1 & st2
- if (covariances_.find(pair_12) != covariances_.end())
- {
- assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
- _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
- _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_12];
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_12].transpose();
- }
- else if (covariances_.find(pair_21) != covariances_.end())
- {
- assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
- _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
- _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_21].transpose();
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_21];
- }
- else
- return false;
- }
-
- return true;
-}
-
-bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col)
-{
- return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
-}
-
-bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance)
-{
- bool success(true);
- int i = 0, j = 0;
-
- const auto& state_bloc_vec = _frame_ptr->getStateBlockVec();
-
- // computing size
- SizeEigen sz = 0;
- for (const auto& sb : state_bloc_vec)
- if (sb)
- sz += sb->getSize();
-
- // resizing
- _covariance = Eigen::MatrixXs(sz, sz);
-
- // filling covariance
- for (const auto& sb_i : state_bloc_vec)
- {
- if (sb_i)
- {
- j = 0;
- for (const auto& sb_j : state_bloc_vec)
- {
- if (sb_j)
- {
- success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
- j += sb_j->getSize();
- }
- }
- i += sb_i->getSize();
- }
- }
- return success;
-}
-
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
-{
- FrameBasePtr frm = getLastKeyFrame();
- return getFrameCovariance(frm, cov);
-}
-
-bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
-{
- bool success(true);
- int i = 0, j = 0;
-
- const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
-
- // computing size
- SizeEigen sz = 0;
- for (const auto& sb : state_bloc_vec)
- if (sb)
- sz += sb->getSize();
-
- // resizing
- _covariance = Eigen::MatrixXs(sz, sz);
-
- // filling covariance
-
- for (const auto& sb_i : state_bloc_vec)
- {
- if (sb_i)
- {
- j = 0;
- for (const auto& sb_j : state_bloc_vec)
- {
- if (sb_j)
- {
- success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
- j += sb_j->getSize();
- }
- }
- i += sb_i->getSize();
- }
- }
- return success;
-}
-
-MapBasePtr Problem::getMap()
-{
- return map_ptr_;
-}
-
-TrajectoryBasePtr Problem::getTrajectory()
-{
- return trajectory_ptr_;
-}
-
-HardwareBasePtr Problem::getHardware()
-{
- return hardware_ptr_;
-}
-
-FrameBasePtr Problem::getLastFrame()
-{
- return trajectory_ptr_->getLastFrame();
-}
-
-FrameBasePtr Problem::getLastKeyFrame()
-{
- return trajectory_ptr_->getLastKeyFrame();
-}
-
-StateBlockPtrList& Problem::getStateBlockPtrList()
-{
- return state_block_list_;
-}
-
-FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
-{
- if ( ! prior_is_set_ )
- {
- // Create origin frame
- FrameBasePtr origin_keyframe = emplaceFrame(KEY_FRAME, _prior_state, _ts);
-
- // create origin capture with the given state as data
- // Capture fix only takes 3D position and Quaternion orientation
- CapturePosePtr init_capture;
- if (trajectory_ptr_->getFrameStructure() == "POV 3D")
- init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- else
- init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
-
- origin_keyframe->addCapture(init_capture);
-
- // emplace feature and factor
- init_capture->emplaceFeatureAndFactor();
-
- // Notify all processors motion about the prior KF
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if (processor->isMotion())
- // Motion processors will set its origin at the KF
- (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe);
-
- prior_is_set_ = true;
-
- // Notify all other processors about the origin KF --> they will join it or not depending on their received data
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if ( !processor->isMotion() )
- processor->keyFrameCallback(origin_keyframe, _time_tolerance);
-
- return origin_keyframe;
- }
- else
- throw std::runtime_error("Origin already set!");
-}
-
-void Problem::loadMap(const std::string& _filename_dot_yaml)
-{
- getMap()->load(_filename_dot_yaml);
-}
-
-void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
-{
- getMap()->save(_filename_dot_yaml, _map_name);
-}
-
-void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
-{
- using std::cout;
- using std::endl;
-
- cout << endl;
- cout << "P: wolf tree status ---------------------" << endl;
- cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
- if (depth >= 1)
- {
- // Sensors =======================================================================================
- for (auto S : getHardware()->getSensorList())
- {
- cout << " S" << S->id() << " " << S->getType();
- if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
- if (depth < 2)
- cout << " -- " << S->getProcessorList().size() << "p";
- cout << endl;
- if (metric && state_blocks)
- {
- for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++)
- {
- if (i==0) cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- if (i==2) cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlock(i);
- if (sb)
- {
- cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
- }
- if (i==1) cout << " ]" << endl;
- }
- if (S->getStateBlockVec().size() > 2) cout << " ]" << endl;
- }
- else if (metric)
- {
- cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
- if (S->getP())
- cout << S->getP()->getState().transpose();
- if (S->getO())
- cout << " " << S->getO()->getState().transpose();
- cout << " )";
- if (S->getIntrinsic())
- cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
- cout << endl;
- }
- else if (state_blocks)
- {
- cout << " sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
- for (auto sb : S->getStateBlockVec())
- if (sb != nullptr)
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
- cout << endl;
- }
- if (depth >= 2)
- {
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (p->isMotion())
- {
- std::cout << " pm" << p->id() << " " << p->getType() << endl;
- ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOrigin())
- cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKey() ? " KF" : " F")
- << pm->getOrigin()->getFrame()->id() << endl;
- if (pm->getLast())
- cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKey() ? " KF" : " F")
- << pm->getLast()->getFrame()->id() << endl;
- if (pm->getIncoming())
- cout << " i: C" << pm->getIncoming()->id() << endl;
- }
- else
- {
- cout << " pt" << p->id() << " " << p->getType() << endl;
- ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
- if (pt)
- {
-// ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt);
-// if (ptt)
-// {
-// if (ptt->getPrevOrigin())
-// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " F")
-// << ptt->getPrevOrigin()->getFrame()->id() << endl;
-// }
- if (pt->getOrigin())
- cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKey() ? " KF" : " F")
- << pt->getOrigin()->getFrame()->id() << endl;
- if (pt->getLast())
- cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKey() ? " KF" : " F")
- << pt->getLast()->getFrame()->id() << endl;
- if (pt->getIncoming())
- cout << " i: C" << pt->getIncoming()->id() << endl;
- }
- }
- } // for p
- }
- } // for S
- }
- cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
- if (depth >= 1)
- {
- // Frames =======================================================================================
- for (auto F : getTrajectory()->getFrameList())
- {
- cout << (F->isKey() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (constr_by)
- {
- cout << " <-- ";
- for (auto cby : F->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
- }
- cout << endl;
- if (metric)
- {
- cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp().get();
- cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- cout << endl;
- }
- if (state_blocks)
- {
- cout << " sb:";
- for (auto sb : F->getStateBlockVec())
- if (sb != nullptr)
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
- cout << endl;
- }
- if (depth >= 2)
- {
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
-
- if(C->getSensor() != nullptr)
- {
- cout << " -> S" << C->getSensor()->id();
- cout << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
- cout << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
- }
- else
- cout << " -> S-";
- if (C->isMotion())
- {
- auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (CM->getOriginFrame())
- cout << " -> OF" << CM->getOriginFrame()->id();
- }
-
- cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (constr_by)
- {
- cout << " <-- ";
- for (auto cby : C->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
- }
- cout << endl;
-
- if (state_blocks)
- for(auto sb : C->getStateBlockVec())
- if(sb != nullptr)
- {
- cout << " sb: ";
- cout << (sb->isFixed() ? "Fix" : "Est");
- if (metric)
- cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- cout << endl;
- }
-
- if (C->isMotion() )
- {
- CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- cout << " buffer size : " << CM->getBuffer().get().size() << endl;
- if ( metric && ! CM->getBuffer().get().empty())
- {
- cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
- if (CM->hasCalibration())
- {
- cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
- cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
- cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
- cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
- }
- }
- }
-
- if (depth >= 3)
- {
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (constr_by)
- {
- cout << " <--\t";
- for (auto cby : f->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
- }
- cout << endl;
- if (metric)
- cout << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << endl;
- if (depth >= 4)
- {
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- cout << " c" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- cout << " A";
- if (c->getFrameOther() != nullptr)
- cout << " F" << c->getFrameOther()->id();
- if (c->getCaptureOther() != nullptr)
- cout << " C" << c->getCaptureOther()->id();
- if (c->getFeatureOther() != nullptr)
- cout << " f" << c->getFeatureOther()->id();
- if (c->getLandmarkOther() != nullptr)
- cout << " L" << c->getLandmarkOther()->id();
- cout << endl;
- } // for c
- }
- } // for f
- }
- } // for C
- }
- } // for F
- }
- cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
- if (depth >= 1)
- {
- // Landmarks =======================================================================================
- for (auto L : getMap()->getLandmarkList())
- {
- cout << " L" << L->id() << " " << L->getType();
- if (constr_by)
- {
- cout << "\t<-- ";
- for (auto cby : L->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
- }
- cout << endl;
- if (metric)
- {
- cout << (L->isFixed() ? " Fix" : " Est");
- cout << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- cout << endl;
- }
- if (state_blocks)
- {
- cout << " sb:";
- for (auto sb : L->getStateBlockVec())
- if (sb != nullptr)
- cout << (sb->isFixed() ? " Fix" : " Est");
- cout << endl;
- }
- } // for L
- }
- cout << "-----------------------------------------" << endl;
- cout << endl;
-}
-
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
-{
- return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-}
-
-bool Problem::check(int verbose_level)
-{
- using std::cout;
- using std::endl;
-
- bool is_consistent = true; // true if all checks passed; false if any check fails.
-
- if (verbose_level) cout << endl;
- if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl;
- auto P_raw = this;
- if (verbose_level > 0)
- {
- cout << "P @ " << P_raw << endl;
- }
- // ------------------------
- // HARDWARE branch
- // ------------------------
- auto H = hardware_ptr_;
- if (verbose_level > 0)
- {
- cout << "H @ " << H.get() << endl;
- }
- // check pointer to Problem
- is_consistent = is_consistent && (H->getProblem().get() == P_raw);
-
- // Sensors =======================================================================================
- for (auto S : H->getSensorList())
- {
- if (verbose_level > 0)
- {
- cout << " S" << S->id() << " @ " << S.get() << endl;
- cout << " -> P @ " << S->getProblem().get() << endl;
- cout << " -> H @ " << S->getHardware().get() << endl;
- for (auto sb : S->getStateBlockVec())
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
- }
- // check problem and hardware pointers
- is_consistent = is_consistent && (S->getProblem().get() == P_raw);
- is_consistent = is_consistent && (S->getHardware() == H);
-
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (verbose_level > 0)
- {
- cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl;
- cout << " -> P @ " << p->getProblem().get() << endl;
- cout << " -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << endl;
- }
- // check problem and sensor pointers
- is_consistent = is_consistent && (p->getProblem().get() == P_raw);
- is_consistent = is_consistent && (p->getSensor() == S);
- }
- }
- // ------------------------
- // TRAJECTORY branch
- // ------------------------
- auto T = trajectory_ptr_;
- if (verbose_level > 0)
- {
- cout << "T @ " << T.get() << endl;
- }
- // check pointer to Problem
- is_consistent = is_consistent && (T->getProblem().get() == P_raw);
-
- // Frames =======================================================================================
- for (auto F : T->getFrameList())
- {
- if (verbose_level > 0)
- {
- cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << endl;
- cout << " -> P @ " << F->getProblem().get() << endl;
- cout << " -> T @ " << F->getTrajectory().get() << endl;
- for (auto sb : F->getStateBlockVec())
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
- }
- // check problem and trajectory pointers
- is_consistent = is_consistent && (F->getProblem().get() == P_raw);
- is_consistent = is_consistent && (F->getTrajectory() == T);
- for (auto cby : F->getConstrainedByList())
- {
- if (verbose_level > 0)
- {
- cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
- }
- // check constrained_by pointer to this frame
- is_consistent = is_consistent && (cby->getFrameOther() == F);
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
- }
- }
-
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- if (verbose_level > 0)
- {
- cout << " C" << C->id() << " @ " << C.get() << " -> S";
- if (C->getSensor()) cout << C->getSensor()->id();
- else cout << "-";
- cout << endl;
- cout << " -> P @ " << C->getProblem().get() << endl;
- cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
- for (auto sb : C->getStateBlockVec())
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
- }
- // check problem and frame pointers
- is_consistent = is_consistent && (C->getProblem().get() == P_raw);
- is_consistent = is_consistent && (C->getFrame() == F);
-
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- if (verbose_level > 0)
- {
- cout << " f" << f->id() << " @ " << f.get() << endl;
- cout << " -> P @ " << f->getProblem().get() << endl;
- cout << " -> C" << f->getCapture()->id() << " @ " << f->getCapture().get()
- << endl;
- }
- // check problem and capture pointers
- is_consistent = is_consistent && (f->getProblem().get() == P_raw);
- is_consistent = is_consistent && (f->getCapture() == C);
-
- for (auto cby : f->getConstrainedByList())
- {
- if (verbose_level > 0)
- {
- cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
- }
- // check constrained_by pointer to this feature
- is_consistent = is_consistent && (cby->getFeatureOther() == f);
- }
-
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- if (verbose_level > 0)
- cout << " c" << c->id() << " @ " << c.get();
-
- auto Fo = c->getFrameOther();
- auto Co = c->getCaptureOther();
- auto fo = c->getFeatureOther();
- auto Lo = c->getLandmarkOther();
-
- if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
- {
- if (verbose_level > 0)
- cout << " --> Abs." << endl;
- }
-
- // find constrained_by pointer in constrained frame
- if ( Fo ) // case FRAME:
- {
- if (verbose_level > 0)
- cout << " --> F" << Fo->id() << " <- ";
- bool found = false;
- for (auto cby : Fo->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
- }
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained frame
- is_consistent = is_consistent && found;
- }
-
- // find constrained_by pointer in constrained capture
- if ( Co ) // case CAPTURE:
- {
- if (verbose_level > 0)
- cout << " --> C" << Co->id() << " <- ";
- bool found = false;
- for (auto cby : Co->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
- }
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained frame
- is_consistent = is_consistent && found;
- }
-
- // find constrained_by pointer in constrained feature
- if ( fo ) // case FEATURE:
- {
- if (verbose_level > 0)
- cout << " --> f" << fo->id() << " <- ";
- bool found = false;
- for (auto cby : fo->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
- }
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained feature
- is_consistent = is_consistent && found;
- }
-
- // find constrained_by pointer in constrained landmark
- if ( Lo ) // case LANDMARK:
- {
- if (verbose_level > 0)
- cout << " --> L" << Lo->id() << " <- ";
- bool found = false;
- for (auto cby : Lo->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
- }
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained landmark
- is_consistent = is_consistent && found;
- }
- if (verbose_level > 0)
- {
- cout << " -> P @ " << c->getProblem().get() << endl;
- cout << " -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl;
- }
- // check problem and feature pointers
- is_consistent = is_consistent && (c->getProblem().get() == P_raw);
- is_consistent = is_consistent && (c->getFeature() == f);
-
- // find state block pointers in all constrained nodes
- SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
- for (auto sb : c->getStateBlockPtrVector())
- {
- bool found = false;
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- }
- // find in own Frame
- found = found || (std::find(F->getStateBlockVec().begin(), F->getStateBlockVec().end(), sb) != F->getStateBlockVec().end());
- // find in own Capture
- found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
- // find in own Sensor
- if (S)
- found = found || (std::find(S->getStateBlockVec().begin(), S->getStateBlockVec().end(), sb) != S->getStateBlockVec().end());
- // find in constrained Frame
- if (Fo)
- found = found || (std::find(Fo->getStateBlockVec().begin(), Fo->getStateBlockVec().end(), sb) != Fo->getStateBlockVec().end());
- // find in constrained Capture
- if (Co)
- found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
- // find in constrained Feature
- if (fo)
- {
- // find in constrained feature's Frame
- FrameBasePtr foF = fo->getFrame();
- found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end());
- // find in constrained feature's Capture
- CaptureBasePtr foC = fo->getCapture();
- found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
- // find in constrained feature's Sensor
- SensorBasePtr foS = fo->getCapture()->getSensor();
- found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
- }
- // find in constrained landmark
- if (Lo)
- found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end());
- if (verbose_level > 0)
- {
- if (found)
- cout << " found";
- else
- cout << " NOT FOUND !";
- cout << endl;
- }
- // check that all state block pointers were found
- is_consistent = is_consistent && found;
- }
- }
- }
- }
- }
- // ------------------------
- // MAP branch
- // ------------------------
- auto M = map_ptr_;
- if (verbose_level > 0)
- cout << "M @ " << M.get() << endl;
- // check pointer to Problem
- is_consistent = is_consistent && (M->getProblem().get() == P_raw);
-
- // Landmarks =======================================================================================
- for (auto L : M->getLandmarkList())
- {
- if (verbose_level > 0)
- {
- cout << " L" << L->id() << " @ " << L.get() << endl;
- cout << " -> P @ " << L->getProblem().get() << endl;
- cout << " -> M @ " << L->getMap().get() << endl;
- for (auto sb : L->getStateBlockVec())
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
- }
- // check problem and map pointers
- is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- is_consistent = is_consistent && (L->getMap() == M);
- for (auto cby : L->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl;
- // check constrained_by pointer to this landmark
- is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id());
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
- }
- }
- }
-
- if (verbose_level) cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "Not OK !!") << endl;
- if (verbose_level) cout << endl;
-
- return is_consistent;
-}
-
-void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks)
-{
- if (depth.compare("T") == 0)
- print(0, constr_by, metric, state_blocks);
- else if (depth.compare("F") == 0)
- print(1, constr_by, metric, state_blocks);
- else if (depth.compare("C") == 0)
- print(2, constr_by, metric, state_blocks);
- else if (depth.compare("f") == 0)
- print(3, constr_by, metric, state_blocks);
- else if (depth.compare("c") == 0)
- print(4, constr_by, metric, state_blocks);
- else
- print(0, constr_by, metric, state_blocks);
-}
-
-} // namespace wolf
diff --git a/src/processor/CMakeLists.txt b/src/processor/CMakeLists.txt
deleted file mode 100644
index 5c223e4f28ff54f4c62267f980124d62f3d6ccda..0000000000000000000000000000000000000000
--- a/src/processor/CMakeLists.txt
+++ /dev/null
@@ -1,33 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-IF (vision_utils_FOUND)
-SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_feature_trifocal.h
- )
-
-SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_feature_trifocal.cpp
- )
-ENDIF (vision_utils_FOUND)
-
-#=========================================
-#=========================================
-
-
-SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.h
- )
-
-SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} PARENT_SCOPE)
-SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} PARENT_SCOPE)
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
deleted file mode 100644
index fc0e62dda2a417407976edcee977e0118a44a142..0000000000000000000000000000000000000000
--- a/src/processor/polyline_2D_utils.cpp
+++ /dev/null
@@ -1,644 +0,0 @@
-#include "base/processor/polyline_2D_utils.h"
-
-namespace wolf {
-
-Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta)
-{
- Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
-
- T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(theta).matrix();
- T.topRightCorner(2,1) = t;
-
- return T;
-}
-
-Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose)
-{
- Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
-
- T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(pose(2)).matrix();
- T.topRightCorner(2,1) = pose.head(2);
-
- return T;
-}
-
-Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T)
-{
- Eigen::Vector3s pose;
- pose.head(2) = T.topRightCorner(2,1);
- pose(2) = atan2(T(1,0),T(0,0));
-
- return pose;
-}
-
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B, const bool& first_defined, const bool& last_defined)
-{
- assert(_points_A.cols() == _points_B.cols());
- assert(_points_A.rows() >= 2);
- assert(_points_B.rows() >= 2);
-
-
- int N_defined_points = _points_A.cols() - (first_defined ? 0 : 1) - (last_defined ? 0 : 1);
- bool all_defined = first_defined && last_defined;
- Eigen::Vector3s t_A_B;
- assert(N_defined_points > 0 && _points_A.cols() > 1);
-
- // Rotation
- Eigen::VectorXs angles_A_B(Eigen::VectorXs::Zero(_points_A.cols()));
- Eigen::Vector2s points_A_mean((all_defined ? _points_A.row(0).mean() : _points_A.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
- (all_defined ? _points_A.row(1).mean() : _points_A.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
- Eigen::Vector2s points_B_mean((all_defined ? _points_B.row(0).mean() : _points_B.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
- (all_defined ? _points_B.row(1).mean() : _points_B.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
- Eigen::MatrixXs points_A_centered = _points_A.topRows<2>().colwise()-points_A_mean;
- Eigen::MatrixXs points_B_centered = _points_B.topRows<2>().colwise()-points_B_mean;
-
- for (auto i = 0; i < _points_A.cols(); i++ )
- angles_A_B(i) = pi2pi(atan2(points_A_centered(1,i),points_A_centered(0,i)) - atan2(points_B_centered(1,i),points_B_centered(0,i)));
-
- // fix 2nd&3rd quadrant angles bad resulting mean
- if (angles_A_B.maxCoeff() > M_PI / 2 && angles_A_B.minCoeff() < -M_PI / 2)
- angles_A_B = (angles_A_B.array() >= 0).select(angles_A_B, angles_A_B.array()+2*M_PI);
-
- // if only one defined point, mean of not defined points angles (mean is pivot)
- if (N_defined_points == 1)
- t_A_B(2) = ((first_defined ? 0 : angles_A_B(0)) + (last_defined ? 0 : angles_A_B(_points_A.cols()-1))) / ((first_defined ? 0 : 1) + (last_defined ? 0 : 1));
- // use all points if all defined
- else if (all_defined)
- t_A_B(2) = angles_A_B.mean();
- // otherwise compute using defined points
- else
- t_A_B(2) = angles_A_B.segment(first_defined ? 0 : 1, N_defined_points).mean();
-
- // Translation
- t_A_B.head<2>() = points_A_mean - Eigen::Rotation2D<Scalar>(t_A_B(2))*points_B_mean;
-
- return t_A_B;
-}
-
-/** \brief Computes the squared distance from a point to a segment defined by two points
- **/
-Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b)
-{
- Scalar ap_sq = (p-a).squaredNorm();
- Scalar bp_sq = (p-b).squaredNorm();
- Scalar ab_sq = (b-a).squaredNorm();
- Scalar sqDist;
-
- // If the projection of p onto ab is inside the segment (acute triangle: all angles < 90º), return the sq distance point-line.
- if (ap_sq <= bp_sq + ab_sq && bp_sq <= ap_sq + ab_sq) // acute triangle: applying pitagoras twice, angles ABP and PAB <= 90º
- sqDist = ap_sq - std::pow((b-a).dot(p-a),2) / ab_sq;
-
- // Otherwise, return the sq distance to the closest segment point.
- else
- sqDist = std::min(bp_sq,ap_sq);
-
- return sqDist;
-}
-
-Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict)
-{
- /* Squared distance from B to the line A_aux-A (match evaluated is A-B)
- *
- * No matter if A_aux is defined or not.
- *
- * Case 1: B not defined & A not defined
- *
- * If projection of B over AAaux is in [A_aux, inf), return sq.dist. B-Line(AAux)
- * Otherwise, return sqDist(Aaux-B) or 1e12 if strict=true.
- *
- * Check: Angle BAauxA is <= 90º: (AB)² <= (AauxB)² + (AAaux)²
- *
- * Case 2: B not defined & A defined
- *
- * If projection of B over AAaux is in [A_aux, A], return sq.dist. B-Line(AAux)
- * Otherwise, return min(sqDist(Aaux-B),sqDist(A-B)) or 1e12 if strict=true.
- *
- * Check: Angles BAauxA & BAAaux are <= 90º: (AB)² <= (AauxB)² + (AAaux)² & (AauxB)² <= (AB)² + (AAaux)²
- *
- * Case 3: B defined & A not defined
- *
- * If projection of B over AAaux is in [A, inf), return sq.dist. B-Line(AAux)
- * Otherwise, return sqDist(A-B)) or 1e12 if strict=true.
- *
- * Check: Angle BAAaux is >= 90º: (AauxB)² >= (AB)² + (AAaux)²
- *
- * (both points defined is not point-to-line, is point-to-point)
- *
- */
-
- assert((!_A_defined || !_B_defined) && "ProcessorPolyline::sqDistPointToLine: at least one point must not be defined.");
-
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
-
- // squared distance to line
- // Case 1
- if (!_B_defined && !_A_defined)
- {
- if (AB_sq <= AauxB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq;
- else
- return (strict ? 1e12 : AauxB_sq);
- }
-
- // Case 2
- if (!_B_defined && _A_defined)
- {
- if (AB_sq <= AauxB_sq + AAaux_sq && AauxB_sq <= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq;
- else
- return (strict ? 1e12 : std::min(AauxB_sq,AB_sq));
- }
-
- // Case 3
- if (_B_defined && !_A_defined)
- {
- if (AauxB_sq >= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq;
- else
- return (strict ? 1e12 : AB_sq);
- }
-
- // Default return unused
- return 1e12;
-}
-
-MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error )
-{
- //std::cout << "computeBestSqDist...\n";
- //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "OPEN") << " has "<< _points_A.cols() << " points\n";
- //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "OPEN") << " has "<< _points_B.cols() << " points\n";
- //std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
- //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
-
- // ASSERTIONS if closed -> all points defined
- assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
- assert((_closed_B &&_first_defined_B && _last_defined_B) || !_closed_B);
-
- /* ASSUMPTIONS:
- *
- * IF BOTH NOT CLOSED:
- * (a) - A has equal or more points than B --> Otherwise: Call the function switching A<->B
- *
- * IF ONE CLOSED:
- * (b) - Should be A closed (not B) --> Otherwise: Call the function switching A<->B
- * (c) - B.defined_points <= A.(defined_)points --> Otherwise: No matching
- *
- * IF BOTH CLOSED:
- * (d) - B.(defined_)points == A.(defined_)points --> Otherwise: No matching
- */
-
- MatchPolyline2DPtr best_match = nullptr;
-
- // ----------------- Call switching A<->B
- if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
- (!_closed_A && _closed_B) ) // (b)
- {
- //std::cout << "Auto-calling switching A<->B...\n";
-
- MatchPolyline2DPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
- _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
- // undo switching
- if (best_match != nullptr)
- {
- std::swap(best_match->from_A_id_, best_match->from_B_id_);
- std::swap(best_match->to_A_id_, best_match->to_B_id_);
- }
- return best_match;
- }
-
- // ------------------ No matching (c) & (d)
- int N_defined_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
-
- if ((_closed_A && !_closed_B && _points_A.cols() < N_defined_B) || // (c)
- (_closed_A && _closed_B && _points_A.cols() != _points_B.cols())) // (d)
- return nullptr;
-
- /* ------------------ Normal function call:
- * Search along all possible overlapping configurations between A&B points
- * We define offset as the correspondence (if exists) of the first point_A to B's.
- * Offset is within [min_offset, max_offset], we distinguish between 3 cases:
- * 1. None closed: [-last_A, last_B]
- * 2. A closed: [-last_A, 0]
- */
-
- int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlapped;
- last_A = _points_A.cols() - 1;
- last_B = _points_B.cols() - 1;
-
- if (!_closed_A)
- {
- min_offset = -last_A;
- max_offset = last_B;
- }
- else
- {
- min_offset = -last_A;
- max_offset = 0;
- }
-
- // Check all overlapping configurations
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- from_A = std::max(0,-offset);
- from_B = std::max(0, offset);
-
- if (!_closed_A && !_closed_B)
- N_overlapped = std::min(last_B - from_B, last_A - from_A)+1;
- else if(_closed_A)
- N_overlapped = _points_B.cols();
- else
- std::runtime_error("Impossible state. Function should auto-called reversing A&B");
-
- to_A = from_A+N_overlapped-1;
- to_B = from_B+N_overlapped-1;
-
- while (to_A > last_A && _closed_A)
- to_A -= _points_A.cols();
-
- //std::cout << "\toffset " << offset << std::endl;
- //std::cout << "\t\tfrom_A " << from_A << std::endl;
- //std::cout << "\t\tto_A " << to_A << std::endl;
- //std::cout << "\t\tfrom_B " << from_B << std::endl;
- //std::cout << "\t\tto_B " << to_B << std::endl;
- //std::cout << "\t\tlast_A " << last_A << std::endl;
- //std::cout << "\t\tlast_B " << last_B << std::endl;
- //std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
-
- bool from_A_defined = ((from_A != 0 && from_A != last_A) || _first_defined_A);
- bool from_B_defined = ((from_B != 0 && from_B != last_B) || _first_defined_B);
- bool to_A_defined = ((to_A != 0 && to_A != last_A) || _last_defined_A);
- bool to_B_defined = ((to_B != 0 && to_B != last_B) || _last_defined_B);
-
- //std::cout << "\t\tfrom_A_defined " << from_A_defined << (from_A == 0) << (from_A == last_A) << _first_defined_A << std::endl;
- //std::cout << "\t\tfrom_B_defined " << from_B_defined << (from_B == 0) << (from_B == last_B) << _first_defined_B << std::endl;
- //std::cout << "\t\tto_A_defined " << to_A_defined << (to_A == 0) << (to_A == last_A) << _last_defined_A << std::endl;
- //std::cout << "\t\tto_B_defined " << to_B_defined << (to_B == 0) << (to_B == last_B) << _last_defined_B << std::endl;
-
- // If only one overlapped point, both should be defined
- if ( N_overlapped== 1 &&
- (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
- continue;
-
- assert(N_overlapped <= _points_B.cols());
- assert(N_overlapped <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlapped can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
- assert(from_A >= 0);
- assert(from_B >= 0);
- assert(from_B == 0 || !_closed_A);
- assert(to_B == _points_B.cols()-1 || !_closed_A);
- assert(to_A <= last_A);
- assert(to_B <= last_B);
- assert(to_A < _points_A.cols());
- assert(to_B < _points_B.cols());
-
- // POINT-POINT DISTANCES for all overlapped points (in case of closed A in two parts)
- // Fill with B points
- Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlapped).array());
-
- // Substract A points
- if (to_A >= from_A && N_overlapped <= _points_A.cols())
- d -= _points_A.block(0,from_A, 2, N_overlapped).array();
- else
- {
- d.leftCols(_points_A.cols()-from_A) -= _points_A.block(0,from_A, 2, _points_A.cols()-from_A).array();
- d.rightCols(to_A+1) -= _points_A.block(0, 0, 2, to_A+1).array();
- }
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
-
- // CORRECT POINT-LINE DISTANCES for not defined extremes
- // First
- if (!from_B_defined || !from_A_defined)
- {
- // take care of closed A: next of from_A
- int next_from_A = (from_A+1 > last_A ? 0 : from_A+1);
- //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
-
- // std::cout << "\t\t\tA " << _points_A.col(from_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(next_from_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(from_B).transpose() << std::endl;
- dist2(0) = sqDistPointToLine(_points_A.col(from_A),
- _points_A.col(next_from_A),
- _points_B.col(from_B),
- from_A_defined,
- from_B_defined);
- assert(N_overlapped > 1);
- }
- // Last
- if (!to_B_defined || !to_A_defined)
- {
- // take care of closed A: next of to_A
- int prev_to_A = (to_A == 0 ? last_A : to_A-1);
- //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
-
- // std::cout << "\t\t\tA " << _points_A.col(to_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(to_B).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(_points_A.col(to_A),
- _points_A.col(prev_to_A),
- _points_B.col(to_B),
- to_A_defined,
- to_B_defined);
- assert(N_overlapped > 1);
- }
- //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be within a threshold
- if ((dist2 < max_sq_error).all())
- {
- Scalar normalized_score = (max_sq_error - dist2.mean()) / max_sq_error;
-
- // Choose the most overlapped match, if equally overlapped: the best score
- if (best_match == nullptr ||
- N_overlapped > best_match->to_A_id_-best_match->from_A_id_+1 ||
- (N_overlapped == best_match->to_A_id_-best_match->from_A_id_+1 && normalized_score > best_match->normalized_score_ ))
- {
- //std::cout << "\tBEST MATCH" << std::endl;
- if (best_match == nullptr)
- best_match = std::make_shared<MatchPolyline2D>();
-
- best_match->from_A_id_= from_A;
- best_match->to_A_id_= to_A;
- best_match->from_B_id_= from_B;
- best_match->to_B_id_= to_B;
- best_match->normalized_score_ = normalized_score;
- }
- }
- }
-
- return best_match;
-}
-
-MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error )
-{
- std::cout << "computeBestRigidTrans...\n";
-
- // ASSERTIONS if closed -> all points defined
- assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
- assert((_closed_B &&_first_defined_B && _last_defined_B) || !_closed_B);
-
- /* ASSUMPTIONS:
- *
- * IF BOTH NOT CLOSED:
- * (a) - A has equal or more points than B --> Otherwise: Call the function switching A<->B
- *
- * IF ONE CLOSED:
- * (b) - Should be A closed (not B) --> Otherwise: Call the function switching A<->B
- * (c) - B.defined_points <= A.(defined_)points --> Otherwise: No matching
- *
- * IF BOTH CLOSED:
- * (d) - B.(defined_)points == A.(defined_)points --> Otherwise: No matching
- */
-
- MatchPolyline2DList matches;
- auto N_defined_points_A = _points_A.cols() - (_first_defined_A ? 0 : 1) - (_last_defined_A ? 0 : 1);
- auto N_defined_points_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
-
- // Not enough defined points
- if (N_defined_points_A <= 1 || N_defined_points_B <= 1 )
- return matches;
-
- std::cout << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points (" << N_defined_points_A << " defined)\n";
- std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " has "<< _points_B.cols() << " points (" << N_defined_points_B << " defined)\n";
- std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
- std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
-
- // ----------------- Call switching A<->B
- if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
- (!_closed_A && _closed_B) ) // (b)
- {
- //std::cout << "Auto-calling switching A<->B...\n";
-
- matches = computeBestRigidTrans(_points_B, _first_defined_B, _last_defined_B, _closed_B,
- _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
- // undo switching
- for (auto match : matches)
- {
- std::swap(match->from_A_id_, match->from_B_id_);
- std::swap(match->to_A_id_, match->to_B_id_);
- match->T_A_B_ = match->T_A_B_.inverse().eval();
- }
- return matches;
- }
-
- // ------------------ No matching (c) & (d) return empty list
- int N_defined_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
-
- if ((_closed_A && !_closed_B && _points_A.cols() < N_defined_B) || // (c)
- (_closed_A && _closed_B && _points_A.cols() != _points_B.cols())) // (d)
- return matches;
-
- /* ------------------ Normal function call:
- * Search along all possible overlapping configurations between A&B points
- * We define offset as the correspondence (if exists) of the first point_A to B's.
- * Offset is within [min_offset, max_offset], we distinguish between 3 cases:
- * 1. None closed: [-last_A, last_B]
- * 2. A closed: [-last_A, 0]
- */
-
- int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlapped;
- last_A = _points_A.cols() - 1;
- last_B = _points_B.cols() - 1;
-
- if (!_closed_A)
- {
- min_offset = -last_A;
- max_offset = last_B;
- }
- else
- {
- min_offset = -last_A;
- max_offset = 0;
- }
-
- // Check all overlapping configurations
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- from_A = std::max(0,-offset);
- from_B = std::max(0, offset);
-
- if (!_closed_A && !_closed_B)
- N_overlapped = std::min(last_B - from_B, last_A - from_A)+1;
- else if(_closed_A)
- N_overlapped = _points_B.cols();
- else
- std::runtime_error("Impossible state. Function should auto-called reversing A&B");
-
- to_A = from_A+N_overlapped-1;
- to_B = from_B+N_overlapped-1;
-
- while (to_A > last_A && _closed_A)
- to_A -= _points_A.cols();
-
- //std::cout << "\toffset " << offset << std::endl;
- //std::cout << "\t\tfrom_A " << from_A << std::endl;
- //std::cout << "\t\tto_A " << to_A << std::endl;
- //std::cout << "\t\tfrom_B " << from_B << std::endl;
- //std::cout << "\t\tto_B " << to_B << std::endl;
- //std::cout << "\t\tlast_A " << last_A << std::endl;
- //std::cout << "\t\tlast_B " << last_B << std::endl;
- //std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
-
- bool from_A_defined = ((from_A != 0 && from_A != last_A) || _first_defined_A);
- bool from_B_defined = ((from_B != 0 && from_B != last_B) || _first_defined_B);
- bool to_A_defined = ((to_A != 0 && to_A != last_A) || _last_defined_A);
- bool to_B_defined = ((to_B != 0 && to_B != last_B) || _last_defined_B);
-
- //std::cout << "\t\tfrom_A_defined " << from_A_defined << (from_A == 0) << (from_A == last_A) << _first_defined_A << std::endl;
- //std::cout << "\t\tfrom_B_defined " << from_B_defined << (from_B == 0) << (from_B == last_B) << _first_defined_B << std::endl;
- //std::cout << "\t\tto_A_defined " << to_A_defined << (to_A == 0) << (to_A == last_A) << _last_defined_A << std::endl;
- //std::cout << "\t\tto_B_defined " << to_B_defined << (to_B == 0) << (to_B == last_B) << _last_defined_B << std::endl;
-
- // If only one overlapped point, no rigid transformation possible
- if ( N_overlapped == 1 )
- continue;
- // If only two overlapped points, all should be defined
- if ( N_overlapped == 2 &&
- (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
- continue;
-
- assert(N_overlapped <= _points_B.cols());
- assert(N_overlapped <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlapped can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
- assert(from_A >= 0);
- assert(from_B >= 0);
- assert(from_B == 0 || !_closed_A);
- assert(to_B == _points_B.cols()-1 || !_closed_A);
- assert(to_A <= last_A);
- assert(to_B <= last_B);
- assert(to_A < _points_A.cols());
- assert(to_B < _points_B.cols());
-
- // RIGID TRANSFORMATION FOR DEFINED POINTS
- //std::cout << "\tcomputing rigid transformation...\n";
- int from_B_def = from_B_defined ? from_B : from_B+1;
- int from_A_def = from_A_defined ? from_A : from_A+1;
- int to_A_def = to_A_defined ? to_A : to_A+1;
- int N_overlapped_def = N_overlapped - (from_B_defined && from_A_defined ? 0 : 1) - (to_B_defined && to_A_defined ? 0 : 1);
- Eigen::MatrixXs points_B_def = _points_B.middleCols(from_B_def, N_overlapped_def);
- Eigen::MatrixXs points_A_def(_points_A.rows(), N_overlapped_def);
- if (to_A >= from_A)
- points_A_def = _points_A.middleCols(from_A_def, N_overlapped_def);
- else
- {
- points_A_def.leftCols(_points_A.cols()-from_A_def) = _points_A.middleCols(from_A_def, _points_A.cols()-from_A_def);
- points_A_def.rightCols(to_A_def+1) = _points_A.middleCols(0, to_A_def+1);
- }
- //std::cout << "\tpoints filled...\n";
-
- //std::cout << "\tcomputeRigidTrans\n";
- Eigen::Vector3s pose_A_B = computeRigidTrans(points_A_def, points_B_def);
- Eigen::Matrix3s T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2));
-
- // POINT-POINT DISTANCES of the matching of defined points
- Eigen::MatrixXs d = (points_A_def - T_A_B * points_B_def).topRows(2);
- Eigen::ArrayXd dist2(N_overlapped);
- dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlapped_def) = d.colwise().squaredNorm().transpose().array();
-
- // POINT-LINE DISTANCES for not defined extremes
- //std::cout << "\tPOINT-LINE DISTANCES\n";
- // First
- if (!from_B_defined || !from_A_defined)
- {
- // take care of closed A: next of from_A
- int next_from_A = (from_A+1 > last_A ? 0 : from_A+1);
- //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
-
- // std::cout << "\t\t\tA " << _points_A.col(from_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(next_from_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(from_B).transpose() << std::endl;
- dist2(0) = sqDistPointToLine(_points_A.col(from_A),
- _points_A.col(next_from_A),
- T_A_B * _points_B.col(from_B),
- from_A_defined,
- from_B_defined);
- assert(N_overlapped > 1);
- }
- // Last
- if (!to_B_defined || !to_A_defined)
- {
- // take care of closed A: next of to_A
- int prev_to_A = (to_A == 0 ? last_A : to_A-1);
- //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
-
- // std::cout << "\t\t\tA " << _points_A.col(to_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(to_B).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(_points_A.col(to_A),
- _points_A.col(prev_to_A),
- T_A_B * _points_B.col(to_B),
- to_A_defined,
- to_B_defined);
- assert(N_overlapped > 1);
- }
- //std::cout << "\t\tsquared distances = " << dist2.transpose() << "( should be < " << max_sq_error << ")" << std::endl;
-
- // All squared distances should be within a threshold
- if ((dist2 < max_sq_error).all())
- {
- auto match = std::make_shared<MatchPolyline2D>();
-
- match->from_A_id_= from_A;
- match->to_A_id_= to_A;
- match->from_B_id_= from_B;
- match->to_B_id_= to_B;
- match->normalized_score_ = (max_sq_error - dist2.mean()) / max_sq_error;
- match->T_A_B_ = T_A_B;
-
- // Store matches ordered by the squared norm of the transformation (I know, mixing mts and rads..)
- matches.push_back(match);
- std::cout << "match added to list of matches!\n";
- }
- }
-
- std::cout << matches.size() << " rigid transformations found\n";
- return matches;
-}
-
-bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B)
-{
- /* The orthogonal projection of B over the line A-Aaux is in the segment [A,Aaux].
- *
- * Check: the angles BAAaux and BAauxA are <= 90º:
- * (BAaux)² <= (BA)² + (AAaux)² & (BA)² <= (BAaux)² + (AAaux)²
- *
- */
-
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
-
- return (AauxB_sq <= AB_sq + AAaux_sq) && (AB_sq <= AauxB_sq + AAaux_sq);
-}
-
-Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
-{
- // ------------------------ d
- Eigen::Vector2s d = _feature - _expected_feature;
-
- // ------------------------ Sigma_d
- Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
- for (unsigned int i = 0; i < _mu.cols(); i++)
- {
- squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
- //if ((d - _mu.col(i)).norm() < 1)
- //{
- // std::cout << "distance: " << (d - _mu.col(i)).norm() << std::endl;
- // std::cout << "iSigma_d: " << std::endl << iSigma_d << std::endl;
- // std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl;
- //}
- }
-
- return squared_mahalanobis_distances;
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_GPS.cpp b/src/processor/processor_GPS.cpp
deleted file mode 100644
index 8409f78c2572489083d36a6c9deb31c45ed53ee1..0000000000000000000000000000000000000000
--- a/src/processor/processor_GPS.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-//
-// Created by ptirindelli on 16/12/15.
-//
-
-#include "base/factor/factor_GPS_pseudorange_2D.h"
-#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/processor/processor_GPS.h"
-
-#include "base/capture/capture_GPS.h"
-
-namespace wolf
-{
-
-ProcessorGPS::ProcessorGPS(ProcessorParamsBasePtr _params) :
- ProcessorBase("GPS", _params),
- capture_gps_ptr_(nullptr)
-{
- gps_covariance_ = 10;
-}
-
-ProcessorGPS::~ProcessorGPS()
-{
-}
-
-void ProcessorGPS::init(CaptureBasePtr _capture_ptr)
-{
-}
-
-void ProcessorGPS::process(CaptureBasePtr _capture_ptr)
-{
- std::cout << "ProcessorGPS::process(GPScapture)" << std::endl;
- capture_gps_ptr_ = std::static_pointer_cast<CaptureGPS>(_capture_ptr);
-
- //std::cout << "Extracting gps features..." << std::endl;
- rawgpsutils::SatellitesObs obs = capture_gps_ptr_->getData();
- for (unsigned int i = 0; i < obs.measurements_.size(); ++i)
- {
- Eigen::Vector3s sat_pos = obs.measurements_[i].sat_position_;
- Scalar pr = obs.measurements_[i].pseudorange_;
- capture_gps_ptr_->addFeature(std::make_shared<FeatureGPSPseudorange>(sat_pos, pr, gps_covariance_));
- }
- //std::cout << "gps features extracted" << std::endl;
- //std::cout << "Establishing factors to gps features..." << std::endl;
- for (auto i_it = capture_gps_ptr_->getFeatureList().begin();
- i_it != capture_gps_ptr_->getFeatureList().end(); i_it++)
- {
- capture_gps_ptr_->getFeatureList().front()->addFactor(std::make_shared<FactorGPSPseudorange2D>((*i_it), shared_from_this()));
- }
- //std::cout << "Factors established" << std::endl;
-}
-
-bool ProcessorGPS::voteForKeyFrame()
-{
- return false;
-}
-
-void ProcessorGPS::keyFrameCallback(FrameBasePtr, const Scalar& _time_tol)
-{
- //
-}
-
-ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
-{
- ProcessorGPSPtr prc_ptr = std::make_shared<ProcessorGPS>(_params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("GPS",ProcessorGPS)
-} // namespace wolf
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
deleted file mode 100644
index 6e602469c611b02ed1df6d88691e5779ea6987bf..0000000000000000000000000000000000000000
--- a/src/processor/processor_IMU.cpp
+++ /dev/null
@@ -1,342 +0,0 @@
-// wolf
-#include "base/processor/processor_IMU.h"
-#include "base/factor/factor_IMU.h"
-#include "base/IMU_tools.h"
-
-namespace wolf {
-
-ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) :
- ProcessorMotion("IMU", 10, 10, 9, 6, 6, _params_motion_IMU),
- params_motion_IMU_(std::make_shared<ProcessorParamsIMU>(*_params_motion_IMU))
-{
- // Set constant parts of Jacobians
- jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros
- jacobian_delta_.setIdentity(9,9); //
- jacobian_calib_.setZero(9,6);
-}
-
-ProcessorIMU::~ProcessorIMU()
-{
- //
-}
-
-ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
-{
- std::shared_ptr<ProcessorParamsIMU> prc_imu_params;
- if (_params)
- prc_imu_params = std::static_pointer_cast<ProcessorParamsIMU>(_params);
- else
- prc_imu_params = std::make_shared<ProcessorParamsIMU>();
-
- ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>(prc_imu_params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-bool ProcessorIMU::voteForKeyFrame()
-{
- if(!isVotingActive())
- return false;
- // time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span)
- {
- WOLF_DEBUG( "PM: vote: time span" );
- return true;
- }
- // buffer length
- if (getBuffer().get().size() > params_motion_IMU_->max_buff_length)
- {
- WOLF_DEBUG( "PM: vote: buffer length" );
- return true;
- }
- // angle turned
- Scalar angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
- if (angle > params_motion_IMU_->angle_turned)
- {
- WOLF_DEBUG( "PM: vote: angle turned" );
- return true;
- }
- //WOLF_DEBUG( "PM: do not vote" );
- return false;
-}
-
-Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_second, TimeStamp& _ts)
-{
- /* Note: See extensive documentation in ProcessorMotion::interpolate().
- *
- * Interpolate between motion_ref and motion, as in:
- *
- * motion_ref ------ ts_ ------ motion_sec
- * return
- *
- * and return the motion at the given time_stamp ts_.
- *
- * DATA:
- * Data receives no change
- *
- * DELTA:
- * The delta's position and velocity receive linear interpolation:
- * p_ret = (ts - t_ref) / dt * (p - p_ref)
- * v_ret = (ts - t_ref) / dt * (v - v_ref)
- *
- * The delta's quaternion receives a slerp interpolation
- * q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
- *
- * DELTA_INTEGR:
- * The interpolated delta integral is just the reference integral plus the interpolated delta
- *
- * The second integral does not change
- *
- * Covariances receive linear interpolation
- * Q_ret = (ts - t_ref) / dt * Q_sec
- */
- /*
- // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
- if (_ts <= _motion_ref.ts_) // behave as if _ts == _motion_ref.ts_
- {
- // return null motion. Second stays the same.
- Motion motion_int ( _motion_ref );
- motion_int.data_ = _motion_second.data_;
- motion_int.data_cov_ = _motion_second.data_cov_;
- motion_int.delta_ = deltaZero();
- motion_int.delta_cov_ . setZero();
- return motion_int;
- }
- if (_ts >= _motion_second.ts_) // behave as if _ts == _motion_second.ts_
- {
- // return original second motion. Second motion becomes null motion
- Motion motion_int ( _motion_second );
- _motion_second.delta_ = deltaZero();
- _motion_second.delta_cov_ . setZero();
- return motion_int;
- }
- assert(_motion_ref.ts_ <= _ts && "Interpolation time stamp out of bounds");
- assert(_ts <= _motion_second.ts_ && "Interpolation time stamp out of bounds");
-
- assert(_motion_ref.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- assert(_motion_second.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- // reference
- TimeStamp t_ref = _motion_ref.ts_;
-
- // second
- TimeStamp t_sec = _motion_second.ts_;
- Map<VectorXs> motion_sec_dp (_motion_second.delta_.data() + 0, 3);
- Map<Quaternions> motion_sec_dq (_motion_second.delta_.data() + 3 );
- Map<VectorXs> motion_sec_dv (_motion_second.delta_.data() + 7, 3);
-
- // interpolated
- Motion motion_int = motionZero(_ts);
-
- // Jacobians for covariance propagation
- MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
- MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
- // interpolation factor
- Scalar dt1 = _ts - t_ref;
- Scalar dt2 = t_sec - _ts;
- Scalar tau = dt1 / (t_sec - t_ref); // interpolation factor (0 to 1)
- Scalar tau_sq = tau * tau;
-
- // copy data
- motion_int.data_ = _motion_second.data_;
- motion_int.data_cov_ = _motion_second.data_cov_;
-
- // interpolate delta
- motion_int.ts_ = _ts;
- Map<VectorXs> motion_int_dp (motion_int.delta_.data() + 0, 3);
- Map<Quaternions> motion_int_dq (motion_int.delta_.data() + 3 );
- Map<VectorXs> motion_int_dv (motion_int.delta_.data() + 7, 3);
- motion_int_dp = tau_sq * motion_sec_dp; // FIXME: delta_p not correctly interpolated
- motion_int_dv = tau * motion_sec_dv;
- motion_int_dq = Quaternions::Identity().slerp(tau, motion_sec_dq);
- motion_int.delta_cov_ = tau * _motion_second.delta_cov_;
-
- // integrate
- deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, dt1, motion_int.delta_integr_, J_ref, J_int);
- motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose()
- + J_int * _motion_second.delta_cov_ * J_int.transpose();
-
- // update second delta ( in place update )
- motion_sec_dp = motion_int_dq.conjugate() * (motion_sec_dp - motion_int_dp - motion_int_dv * dt2);
- motion_sec_dv = motion_int_dq.conjugate() * (motion_sec_dv - motion_int_dv);
- motion_sec_dq = motion_int_dq.conjugate() * motion_sec_dq;
- _motion_second.delta_cov_ *= (1 - tau); // easy interpolation // TODO check for correctness
- //Dp = Dp; // trivial, just leave the code commented
- //Dq = Dq; // trivial, just leave the code commented
- //_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
-
- return motion_int;
- */
-
- // return _motion_ref;
-
- return ProcessorMotion::interpolate(_motion_ref, _motion_second, _ts);
-
-}
-
-CaptureMotionPtr ProcessorIMU::createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
-{
- CaptureIMUPtr capture_imu = std::make_shared<CaptureIMU>(_ts,
- _sensor,
- _data,
- _data_cov,
- _frame_origin);
- return capture_imu;
-}
-
-FeatureBasePtr ProcessorIMU::createFeature(CaptureMotionPtr _capture_motion)
-{
- MatrixXs cov = _capture_motion->getBuffer().get().back().delta_integr_cov_;
- if (!isPositiveSemiDefinite(cov))
- cov += MatrixXs::Identity(9,9) * Constants::EPS; // TODO use a helper function, yet to be made (issue #157)
-
- FeatureIMUPtr key_feature_ptr = std::make_shared<FeatureIMU>(
- _capture_motion->getBuffer().get().back().delta_integr_,
- cov,
- _capture_motion->getBuffer().getCalibrationPreint(),
- _capture_motion->getBuffer().get().back().jacobian_calib_);
- return key_feature_ptr;
-}
-
-FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
-{
- CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
- FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
- FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
-
- // link ot wolf tree
- _feature_motion->addFactor(fac_imu);
- _capture_origin->addConstrainedBy(fac_imu);
- _capture_origin->getFrame()->addConstrainedBy(fac_imu);
-
- return fac_imu;
-}
-
-void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jac_delta_calib)
-{
- assert(_data.size() == data_size_ && "Wrong data size!");
-
- using namespace Eigen;
- Matrix<Scalar, 9, 6> jac_delta_data;
-
- /*
- * We have the following computing pipeline:
- * Input: data, calib, dt
- * Output: delta, delta_cov, jac_calib
- *
- * We do:
- * body = data - calib
- * delta = body2delta(body, dt) --> jac_body
- * jac_data = jac_body
- * jac_calib = - jac_body
- * delta_cov <-- propagate data_cov using jac_data
- *
- * where body2delta(.) produces a delta from body=(a,w) as follows:
- * dp = 1/2 * a * dt^2
- * dq = exp(w * dt)
- * dv = a * dt
- */
-
- // create delta
- imu::body2delta(_data - _calib, _dt, _delta, jac_delta_data); // Jacobians tested in imu_tools
-
- // compute delta_cov
- _delta_cov = jac_delta_data * _data_cov * jac_delta_data.transpose();
-
- // compute jacobian_calib
- _jac_delta_calib = - jac_delta_data;
-
-}
-
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta)
-{
- /* MATHS according to Sola-16
- * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = 1/2*(a-a_b)*dt^2
- * Dv' = Dv + Dq*(a-a_b)*dt = Dv + Dq*dv if dv = (a-a_b)*dt
- * Dq' = Dq * exp((w-w_b)*dt) = Dq * dq if dq = exp((w-w_b)*dt)
- *
- * where (dp, dq, dv) need to be computed in data2delta(), and Dq*dx =is_equivalent_to= Dq*dx*Dq'.
- */
- _delta_preint_plus_delta = imu::compose(_delta_preint, _delta, _dt);
-}
-
-void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta)
-{
- assert(_x.size() == 10 && "Wrong _x vector size");
- assert(_delta.size() == 10 && "Wrong _delta vector size");
- assert(_x_plus_delta.size() == 10 && "Wrong _x_plus_delta vector size");
- assert(_dt >= 0 && "Time interval _dt is negative!");
-
- _x_plus_delta = imu::composeOverState(_x, _delta, _dt);
-}
-
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta,
- Eigen::MatrixXs& _jacobian_delta_preint,
- Eigen::MatrixXs& _jacobian_delta)
-{
- /*
- * Expression of the delta integration step, D' = D (+) d:
- *
- * Dp' = Dp + Dv*dt + Dq*dp
- * Dv' = Dv + Dq*dv
- * Dq' = Dq * dq
- *
- * Jacobians for covariance propagation.
- *
- * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as:
- *
- * D'_D = [ I -DR*skew(dp) I*dt
- * 0 dR.tr 0
- * 0 -DR*skew(dv) I ] // See Sola-16
- *
- * b. With respect to delta, gives _jacobian_delta = D'_d as:
- *
- * D'_d = [ DR 0 0
- * 0 I 0
- * 0 0 DR ] // See Sola-16
- *
- * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
- */
- imu::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu_tools
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR("IMU", ProcessorIMU)
-}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
deleted file mode 100644
index 82fa41c137c0211bb78a699125d287118edc1775..0000000000000000000000000000000000000000
--- a/src/processor/processor_base.cpp
+++ /dev/null
@@ -1,187 +0,0 @@
-#include "base/processor/processor_base.h"
-#include "base/processor/processor_motion.h"
-#include "base/capture/capture_base.h"
-#include "base/frame_base.h"
-
-namespace wolf {
-
-unsigned int ProcessorBase::processor_id_count_ = 0;
-
-ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params) :
- NodeBase("PROCESSOR", _type),
- processor_id_(++processor_id_count_),
- params_(_params),
- sensor_ptr_()
-{
-// WOLF_DEBUG("constructed +p" , id());
-}
-
-ProcessorBase::~ProcessorBase()
-{
-// WOLF_DEBUG("destructed -p" , id());
-}
-
-bool ProcessorBase::permittedKeyFrame()
-{
- return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
-}
-
-FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr)
-{
- std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl;
-
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp());
- new_frame_ptr->addCapture(_capture_ptr);
-
- return new_frame_ptr;
-}
-
-FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state)
-{
- FrameBasePtr new_frame_ptr = emplaceFrame(_type, _capture_ptr);
- new_frame_ptr->setState(_state);
-
- return new_frame_ptr;
-}
-
-void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
-{
- WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
- if (_keyframe_ptr != nullptr)
- kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other);
-}
-
-void ProcessorBase::remove()
-{
- if (!is_removing_)
- {
- is_removing_ = true;
- ProcessorBasePtr this_p = shared_from_this();
-
- // clear Problem::processor_motion_ptr_
- if (isMotion())
- {
- ProblemPtr P = getProblem();
- if(P && P->getProcessorMotion()->id() == this->id())
- P->clearProcessorMotion();
- }
-
- // remove from upstream
- SensorBasePtr sen = sensor_ptr_.lock();
- if(sen)
- sen->getProcessorList().remove(this_p);
- }
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////
-
-void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp)
-{
- PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
- container_.erase(container_.begin(), post); // erasing by range
-}
-
-void PackKeyFrameBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
-{
- TimeStamp time_stamp = _key_frame->getTimeStamp();
- PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
- container_.emplace(time_stamp, kfpack);
-}
-
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
-{
- if (container_.empty())
- return nullptr;
-
- PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
-
- // remove packs corresponding to removed KFs (keeping the next iterator in post)
- while (post != container_.end() && post->second->key_frame->isRemoving())
- post = container_.erase(post);
- while (post != container_.begin() && std::prev(post)->second->key_frame->isRemoving())
- container_.erase(std::prev(post));
-
- bool prev_exists = (post != container_.begin());
- bool post_exists = (post != container_.end());
-
- bool post_ok = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
-
- if (prev_exists)
- {
- PackKeyFrameBuffer::Iterator prev = std::prev(post);
-
- bool prev_ok = checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
-
- if (prev_ok && !post_ok)
- return prev->second;
-
- else if (!prev_ok && post_ok)
- return post->second;
-
- else if (prev_ok && post_ok)
- {
- if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
- return post->second;
- else
- return prev->second;
- }
- }
- else if (post_ok)
- return post->second;
-
- return nullptr;
-}
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
-{
- return selectPack(_capture->getTimeStamp(), _time_tolerance);
-}
-
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
-{
- // remove packs corresponding to removed KFs
- while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
- container_.erase(container_.begin());
-
- // There is no pack
- if (container_.empty())
- return nullptr;
-
- // Checking on begin() since packs are ordered in time
- // Return first pack if is older than time stamp
- if (container_.begin()->first < _time_stamp)
- return container_.begin()->second;
-
- // Return first pack if despite being newer, it is within the time tolerance
- if (checkTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance))
- return container_.begin()->second;
-
- // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
- return nullptr;
-
-}
-
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
-{
- return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
-}
-
-void PackKeyFrameBuffer::print(void)
-{
- std::cout << "[ ";
- for (auto iter : container_)
- {
- std::cout << "( tstamp: " << iter.first << ", id: " << iter.second->key_frame->id() << ") ";
- }
- std::cout << "]" << std::endl;
-}
-
-bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
- const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
-{
- Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
- Scalar time_tol = std::min(_time_tolerance1, _time_tolerance2);
- bool pass = time_diff <= time_tol;
- return pass;
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
deleted file mode 100644
index 2958739d7e6d0c45e8db4d2df31c5c1db2ae5ae0..0000000000000000000000000000000000000000
--- a/src/processor/processor_capture_holder.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-/**
- * \file processor_capture_holder.h
- *
- * Created on: Jul 12, 2017
- * \author: Jeremie Deray
- */
-
-//Wolf includes
-#include "base/processor/processor_capture_holder.h"
-
-namespace wolf {
-
-ProcessorCaptureHolder::ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder) :
- ProcessorBase("CAPTURE HOLDER", _params_capture_holder),
- params_capture_holder_(_params_capture_holder),
- buffer_(_params_capture_holder->buffer_size)
-{
- //
-}
-
-void ProcessorCaptureHolder::process(CaptureBasePtr _capture_ptr)
-{
- buffer_.push_back(_capture_ptr);
-}
-
-void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tolerance)
-{
- assert(_keyframe_ptr->getTrajectory() != nullptr
- && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
-
- // get keyframe's time stamp
- const TimeStamp new_ts = _keyframe_ptr->getTimeStamp();
-
- // find capture whose buffer is affected by the new keyframe
- CaptureBasePtr existing_capture = findCaptureContainingTimeStamp(new_ts);
-
- if (existing_capture == nullptr)
- {
- WOLF_WARN("Could not find a capture at ts : ", new_ts.get());
- return;
- }
-
- WOLF_DEBUG("ProcessorCaptureHolder::keyFrameCallback", _time_tolerance);
- WOLF_DEBUG("Capture of type : ", existing_capture->getType());
- WOLF_DEBUG("CaptureBuffer size : ", buffer_.container_.size());
-
- // add motion capture to keyframe
- if (std::abs(new_ts - existing_capture->getTimeStamp()) < _time_tolerance)
- {
- auto frame_ptr = existing_capture->getFrame();
-
- if (frame_ptr != _keyframe_ptr)
- {
- _keyframe_ptr->addCapture(existing_capture);
-
- //WOLF_INFO("Adding capture laser !");
-
- //frame_ptr->remove();
- }
- //else
- //WOLF_INFO("Capture laser already exists !");
-
- // Remove as we don't want duplicates
- buffer_.remove(existing_capture);
-
- return;
- }
- else
- {
- WOLF_DEBUG("Capture doesn't fit dt : ",
- std::abs(new_ts - existing_capture->getTimeStamp()),
- " vs ", _time_tolerance);
-
- return;
- }
-}
-
-CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
-{
- WOLF_DEBUG("ProcessorCaptureHolder::findCaptureContainingTimeStamp: ts = ",
- _ts.getSeconds(), ".", _ts.getNanoSeconds());
-
-// auto capture_ptr = last_ptr_;
-// while (capture_ptr != nullptr)
-// {
-// // capture containing motion previous than the ts found:
-// if (buffer_.get().front()->getTimeStamp() < _ts)
-// return capture_ptr;
-// else
-// {
-// // go to the previous motion capture
-// if (capture_ptr == last_ptr_)
-// capture_ptr = origin_ptr_;
-// else if (capture_ptr->getOriginFrame() == nullptr)
-// return nullptr;
-// else
-// {
-// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFrame()->getCaptureOf(getSensor());
-// if (capture_base_ptr == nullptr)
-// return nullptr;
-// else
-// capture_ptr = std::static_pointer_cast<CaptureMotion>(capture_base_ptr);
-// }
-// }
-// }
-
-// return capture_ptr;.
-
-// auto capt = buffer_.getCapture(_ts);
-
- /// @todo
-// WOLF_WARN("ProcessorCaptureHolder::findCaptureContainingTimeStamp "
-// "looking for stamp ",
-// _ts.get() - ((long)_ts.get()),
-// " in (",
-// buffer_.earliest().get() - ((long)buffer_.earliest().get()), ",",
-// buffer_.latest().get() - ((long)buffer_.latest().get()), ").\n",
-// "Found capture with stamp ",
-// capt->getTimeStamp().get() - ((long)capt->getTimeStamp().get()));
-
-// return capt;
-
- return buffer_.getCapture(_ts);
-}
-
-ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr)
-{
- ProcessorParamsCaptureHolderPtr params;
-
- params = std::static_pointer_cast<ProcessorParamsCaptureHolder>(_params);
-
- // if cast failed use default value
- if (params == nullptr)
- params = std::make_shared<ProcessorParamsCaptureHolder>();
-
- ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params);
- prc_ptr->setName(_unique_name);
-
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the ProcessorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("CAPTURE HOLDER", ProcessorCaptureHolder)
-} // namespace wolf
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
deleted file mode 100644
index b8251c3aa78637fdaa55b2a670a774bbadf9fa5d..0000000000000000000000000000000000000000
--- a/src/processor/processor_diff_drive.cpp
+++ /dev/null
@@ -1,289 +0,0 @@
-#include "base/processor/processor_diff_drive.h"
-
-#include "base/sensor/sensor_diff_drive.h"
-
-#include "base/capture/capture_wheel_joint_position.h"
-#include "base/capture/capture_velocity.h"
-
-#include "base/rotations.h"
-#include "base/factor/factor_odom_2D.h"
-#include "base/factor/factor_diff_drive.h"
-#include "base/feature/feature_diff_drive.h"
-
-namespace wolf
-{
-
-ProcessorDiffDrive::ProcessorDiffDrive(SensorDiffDrivePtr _sensor_diff_drive_ptr, ProcessorParamsDiffDrivePtr _params) :
- ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, (_sensor_diff_drive_ptr->getIntrinsic()->isFixed() ? 0 : 3), _params),
- sensor_diff_drive_ptr_(_sensor_diff_drive_ptr),
- params_motion_diff_drive_(_params)
-{
- unmeasured_perturbation_cov_ = Matrix3s::Identity() * params_motion_diff_drive_->unmeasured_perturbation_std * params_motion_diff_drive_->unmeasured_perturbation_std;
-}
-
-void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_delta_calib)
-{
- assert(_data.size() == data_size_ && "Wrong _data vector size");
- assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
- assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
- assert(_calib.size() == calib_size_ && "Wrong _calib vector size");
-
- /// Retrieve sensor intrinsics
- const IntrinsicsDiffDrive& intrinsics = *sensor_diff_drive_ptr_->getIntrinsics();
-
- VelocityComand<Scalar> vel;
- Eigen::MatrixXs J_vel_data;
- Eigen::MatrixXs J_vel_calib;
-
- // JV{
- Eigen::VectorXs factors = (hasCalibration() ? _calib : Eigen::VectorXs::Ones(3));
- // }
-
- switch (intrinsics.model_) {
- case DiffDriveModel::Two_Factor_Model:
- std::tie(vel, J_vel_data, J_vel_calib) =
- wheelPositionIncrementToVelocity<DiffDriveModel::Two_Factor_Model>(
- _data,
- _data_cov,
- intrinsics.left_radius_,
- intrinsics.right_radius_,
- intrinsics.separation_,
- factors,
- _dt);
- break;
- case DiffDriveModel::Three_Factor_Model:
- std::tie(vel, J_vel_data, J_vel_calib) =
- wheelPositionIncrementToVelocity<DiffDriveModel::Three_Factor_Model>(
- _data,
- _data_cov,
- intrinsics.left_radius_,
- intrinsics.right_radius_,
- intrinsics.separation_,
- factors,
- _dt);
- break;
- case DiffDriveModel::Five_Factor_Model:
- // std::tie(vel, J_vel_data, J_vel_calib) =
- // wheelPositionIncrementToVelocity<DiffDriveModel::Two_Factor_Model>(
- // data, data_cov,
- // intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
- // factors, _dt);
- throw std::runtime_error("DiffDriveModel::Five_Factor_Model not implemented !");
- break;
- default:
- throw std::runtime_error("Unknown DiffDrive model.");
- break;
- }
-
- /// Integrate delta vel to zero vel thus
- /// Convert delta vel to delta 2d pose
- Eigen::MatrixXs J_delta_vel;
- integrate(vel.comand, vel.comand_cov, _delta, _delta_cov, J_delta_vel);
-
- _delta_cov += unmeasured_perturbation_cov_;
-
- // JV{
- if (hasCalibration())
- //}
- _jacobian_delta_calib = J_delta_vel * J_vel_calib;
-}
-
-bool ProcessorDiffDrive::voteForKeyFrame()
-{
- // Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_motion_diff_drive_->dist_traveled)
- {
- //WOLF_PROCESSOR_DEBUG("vote for key-frame on distance criterion.");
- return true;
- }
-
- if (std::abs(getBuffer().get().back().delta_integr_.tail<1>()(0)) > params_motion_diff_drive_->angle_turned)
- {
- //WOLF_PROCESSOR_DEBUG("vote for key-frame on rotation criterion.");
- return true;
- }
-
- return false;
-}
-
-void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar /*_Dt2*/,
- Eigen::VectorXs& _delta1_plus_delta2)
-{
- assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
- assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
- assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
-
- /// Simple 2d frame composition
-
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
- _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
-}
-
-void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar /*_Dt2*/,
- Eigen::VectorXs& _delta1_plus_delta2,
- MatrixXs& _jacobian1, MatrixXs& _jacobian2)
-{
- using std::sin;
- using std::cos;
-
- assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
- assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
- assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- assert(_jacobian1.rows() == delta_cov_size_ && "Wrong _jacobian1 size");
- assert(_jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
- assert(_jacobian2.rows() == delta_cov_size_ && "Wrong _jacobian2 size");
- assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
-
- /// Simple 2d frame composition
-
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() +
- Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
-
- _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
-
- // Jac wrt delta_integrated
- _jacobian1 = Eigen::MatrixXs::Identity(delta_cov_size_,delta_cov_size_);
- _jacobian1(0,2) = -sin(_delta1(2))*_delta2(0) - cos(_delta1(2))*_delta2(1);
- _jacobian1(1,2) = cos(_delta1(2))*_delta2(0) - sin(_delta1(2))*_delta2(1);
-
- // jac wrt delta
- _jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
- _jacobian2.topLeftCorner<2,2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
-}
-
-void ProcessorDiffDrive::statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar /*_Dt*/,
- Eigen::VectorXs& _x_plus_delta)
-{
- assert(_x.size() == x_size_ && "Wrong _x vector size");
- assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
-
- // This is just a frame composition in 2D
- _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Ds(_x(2)).matrix() * _delta.head<2>();
- _x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
-}
-
-Eigen::VectorXs ProcessorDiffDrive::deltaZero() const
-{
- return Eigen::VectorXs::Zero(delta_size_);
-}
-
-Motion ProcessorDiffDrive::interpolate(const Motion& _ref,
- Motion& _second,
- TimeStamp& _ts)
-
-{
- // TODO: Implement actual interpolation
- // Implementation: motion ref keeps the same
- //
-// Motion _interpolated(_ref);
-// _interpolated.ts_ = _ts;
-// _interpolated.data_ = Vector3s::Zero();
-// _interpolated.data_cov_ = Matrix3s::Zero();
-// _interpolated.delta_ = deltaZero();
-// _interpolated.delta_cov_ = Eigen::MatrixXs::Zero(delta_size_, delta_size_);
-// _interpolated.delta_integr_ = _ref.delta_integr_;
-// _interpolated.delta_integr_cov_ = _ref.delta_integr_cov_;
-// _interpolated.jacobian_delta_integr_. setIdentity();
-// _interpolated.jacobian_delta_ . setZero();
-// _interpolated.jacobian_calib_ . setZero();
-// return _interpolated;
-
- return ProcessorMotion::interpolate(_ref, _second, _ts);
-
-}
-
-CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
-{
-
- StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic() ? std::make_shared<StateBlock>(3, false) : nullptr;
-
- return std::make_shared<CaptureWheelJointPosition>(_ts, _sensor, _data, _data_cov,
- _frame_origin, nullptr, nullptr, i_ptr);
-}
-
-FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
-{
- FactorBasePtr fac_odom;
-
- if (hasCalibration())
- fac_odom = std::make_shared<FactorDiffDrive>(_feature, _capture_origin->getFrame(), shared_from_this());
- else
- fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(), shared_from_this());
-
- _feature->addFactor(fac_odom);
- _capture_origin->getFrame()->addConstrainedBy(fac_odom);
-
- return fac_odom;
-}
-
-FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motion)
-{
- //WOLF_DEBUG( "PR ", getName()," - creating the feature...");
- if (!isCovariance(_capture_motion->getBuffer().get().back().delta_integr_cov_, Constants::EPS_SMALL))
- {
- WOLF_DEBUG("FIXING delta_integr_cov_ not Covariance matrix...");
- _capture_motion->getBuffer().get().back().delta_integr_cov_ = unmeasured_perturbation_cov_;
- }
-
- FeatureBasePtr key_feature_ptr = std::make_shared<FeatureDiffDrive>(
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_,
- _capture_motion->getBuffer().getCalibrationPreint(),
- _capture_motion->getBuffer().get().back().jacobian_calib_);
-
- //WOLF_DEBUG("ProcessorDiffDrive::createFeature : getCalibrationPreint ",_capture_motion->getBuffer().getCalibrationPreint());
- //WOLF_DEBUG("ProcessorDiffDrive::createFeature : jacobian_calib_ ", _capture_motion->getBuffer().get().back().jacobian_calib_);
-
- return key_feature_ptr;
-}
-
-ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr)
-{
- const auto params = std::static_pointer_cast<ProcessorParamsDiffDrive>(_params);
-
- if (params == nullptr)
- {
- WOLF_ERROR("ProcessorDiffDrive::create : input arg"
- " _params is NOT of type 'ProcessorParamsDiffDrive' !");
- return nullptr;
- }
-
- const auto sensor_ptr = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor_ptr);
-
- if (sensor_ptr == nullptr)
- {
- WOLF_ERROR("ProcessorDiffDrive::create : input arg"
- " '_sensor_ptr' is NOT of type 'SensorDiffDrive' !");
- return nullptr;
- }
-
- ProcessorBasePtr prc_ptr = std::make_shared<ProcessorDiffDrive>(sensor_ptr, params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the ProcessorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive)
-} // namespace wolf
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
deleted file mode 100644
index 7c4e63a75031de399473c9fa1c6e764fa28331a9..0000000000000000000000000000000000000000
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ /dev/null
@@ -1,507 +0,0 @@
-#include "base/processor/processor_frame_nearest_neighbor_filter.h"
-
-namespace wolf
-{
-
-ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF):
- ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params_NNF),
- params_NNF(_params_NNF)
-{
- // area of ellipse based on the Chi-Square Probabilities
- // https://people.richland.edu/james/lecture/m170/tbl-chi.html
- // cover both 2D and 3D cases
-
- if(params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSE ||
- params_NNF->distance_type_ == DistanceType::LC_ELLIPSE_ELLIPSE)
- {
- switch ((int)(params_NNF->probability_*100))
- {
- case 900: area_ = 4.605; // 90% probability
- break;
- case 950: area_ = 5.991; // 95% probability
- break;
- case 990: area_ = 9.210; // 99% probability
- break;
- default : area_ = 5.991; // 95% probability
- break;
- }
- }
- if (params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSOID ||
- params_NNF->distance_type_ == DistanceType::LC_ELLIPSOID_ELLIPSOID)
- {
- switch ((int)(params_NNF->probability_*100))
- {
- case 900: area_ = 6.251; // 90% probability
- break;
- case 950: area_ = 7.815; // 95% probability
- break;
- case 990: area_ = 11.345; // 99% probability
- break;
- default : area_ = 7.815; // 95% probability
- break;
- }
- }
-
- /*
- * The area is based on the Chi-Square Probabilities
- * Becasue they are very big for high likelihood, it is here manually set
- * on ONE. Therefore the ellipses/ ellipsoids are based on the unit ellipse/
- * ellipsoids and the axis are scaled by a / b / c
- */
-
- area_ = 1;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /*_incoming_ptr*/)
-{
- const ProblemPtr problem_ptr = getProblem();
-
- const std::string frame_structure =
- problem_ptr->getTrajectory()->getFrameStructure();
-
- // get the list of all frames
- const FrameBasePtrList& frame_list = problem_ptr->
- getTrajectory()->
- getFrameList();
-
- bool found_possible_candidate = false;
-
- for (const FrameBasePtr& key_it : frame_list)
- {
- // check for LC just if frame is key frame
- // Assert that the evaluated KF has a capture of the
- // same sensor as this processor
- if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr)
- {
- // Check if the two frames currently evaluated are already
- // constrained one-another.
- const FactorBasePtrList& fac_list = key_it->getConstrainedByList();
-
- bool are_constrained = false;
- for (const FactorBasePtr& crt : fac_list)
- {
- // Are the two frames constrained one-another ?
- if (crt->getFrameOther() == problem_ptr->getLastKeyFrame())
- {
- // By this very processor ?
- if (crt->getProcessor() == shared_from_this())
- {
- WOLF_DEBUG("Frames are already constrained together.");
- are_constrained = true;
- break;
- }
- }
- }
- if (are_constrained) continue;
-
- // check if feame is potential candidate for loop closure with
- // chosen distance type
- switch (params_NNF->distance_type_)
- {
- case LoopclosureDistanceType::LC_POINT_ELLIPSE:
- {
- if (frame_structure == "PO 3D" || frame_structure == "POV 3D") // @todo add frame structure "PQVBB 3D"
- {
- WOLF_ERROR("Distance Type LC_POINT_ELLIPSE does not fit 3D frame structure");
- return false;
- }
-
- Eigen::Vector5s frame_covariance;
- if (!computeEllipse2D(key_it, frame_covariance)) continue;
-
- const Eigen::VectorXs current_position = getProblem()->getCurrentState();
- found_possible_candidate = point2DIntersect(current_position,
- frame_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE:
- {
- if (frame_structure == "PO 3D" || frame_structure == "POV 3D")
- {
- WOLF_ERROR("Distance Type LC_ELLIPSE_ELLIPSE does not fit 3D frame structure");
- return false;
- }
-
- Eigen::Vector5s frame_covariance, current_covariance;
- if (!computeEllipse2D(key_it,
- frame_covariance)) continue;
- if (!computeEllipse2D(getProblem()->getLastKeyFrame(),
- current_covariance)) continue;
- found_possible_candidate = ellipse2DIntersect(frame_covariance,
- current_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_POINT_ELLIPSOID:
- {
- if (frame_structure == "PO 2D")
- {
- WOLF_ERROR("Distance Type POINT_ELLIPSOID does not fit 2D frame structure");
- return false;
- }
- Eigen::Vector10s frame_covariance;
- if (!computeEllipsoid3D(key_it,
- frame_covariance)) continue;
- const Eigen::VectorXs current_position = getProblem()->getCurrentState();
- found_possible_candidate = point3DIntersect(current_position,
- frame_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID:
- {
- if (frame_structure == "PO 2D")
- {
- WOLF_ERROR("Distance Type ELLIPSOID_ELLIPSOID does not fit 2D frame structure");
- return false;
- }
- Eigen::Vector10s frame_covariance, current_covariance;
- if (!computeEllipsoid3D(key_it,
- frame_covariance)) continue;
- if (!computeEllipsoid3D(getProblem()->getLastKeyFrame(),
- frame_covariance)) continue;
- found_possible_candidate = ellipsoid3DIntersect(frame_covariance,
- current_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE:
- {
- found_possible_candidate = insideMahalanisDistance(key_it,
- problem_ptr->getLastKeyFrame());
- break;
- }
-
- default:
- WOLF_WARN("NO IMPLEMENTATION FOR OTHER DISTANCE CALCULATION TYPES YET.");
- found_possible_candidate = false;
- return false;
- }
-
- // if a candidate was detected, push it to the appropiate list
- if (found_possible_candidate)
- {
- if (!frameInsideBuffer(key_it))
- loop_closure_candidates.push_back(key_it);
- else
- close_candidates.push_back(key_it);
- }
-
- } // end if key_it is keyframe
- } // end loop through every frame in list
-
- return found_possible_candidate;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& frame_ptr,
- Eigen::Vector5s& ellipse)
-{
- // get 3x3 covariance matrix AKA variance
- Eigen::MatrixXs covariance;
- if (!getProblem()->getFrameCovariance(frame_ptr, covariance))
- {
- WOLF_WARN("Frame covariance not found!");
- return false;
- }
-
- if (!isCovariance(covariance))
- {
- WOLF_WARN("Covariance is not proper !");
- return false;
- }
-
- // get position of frame AKA mean [x, y]
- const Eigen::VectorXs frame_position = frame_ptr->getP()->getState();
- ellipse(0) = frame_position(0);
- ellipse(1) = frame_position(1);
-
- // compute covariance error ellipse around state
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix2s> solver(covariance.block(0,0,2,2));
- Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
- Scalar eigenvalue2 = solver.eigenvalues()[1]; // bigger value
- //Eigen::VectorXs eigenvector1 = solver.eigenvectors().col(0);
- Eigen::VectorXs eigenvector2 = solver.eigenvectors().col(1);
-
- const Scalar scale = std::sqrt(area_);
- ellipse(2) = scale * std::sqrt(eigenvalue2) / 2.; // majorAxis
- ellipse(3) = scale * std::sqrt(eigenvalue1) / 2.; // minorAxis
- ellipse(4) = std::atan2(eigenvector2[1], eigenvector2[0]); // tilt
-
- if (ellipse(4) < Scalar(0)) ellipse(4) += Scalar(2) * M_PI;
-
- // [ pos_x, pos_y, a, b, tilt]
- return true;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr& frame_pointer,
- Eigen::Vector10s& ellipsoid)
-{
- // Ellipse description [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
-
- // get position of frame AKA mean [x, y, z]
- const Eigen::VectorXs frame_position = frame_pointer->getP()->getState();
- ellipsoid(0) = frame_position(0);
- ellipsoid(1) = frame_position(1);
- ellipsoid(2) = frame_position(2);
-
- // get 9x9 covariance matrix AKA variance
- Eigen::MatrixXs covariance;
- if (!getProblem()->getFrameCovariance(frame_pointer, covariance))
- {
- WOLF_WARN("Frame covariance not found!");
- return false;
- }
-
- if (!isCovariance(covariance))
- {
- WOLF_WARN("Covariance is not proper !");
- return false;
- }
-
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix3s> solver(covariance.block(0,0,3,3));
- const Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
- const Scalar eigenvalue2 = solver.eigenvalues()[1]; // mediate value
- const Scalar eigenvalue3 = solver.eigenvalues()[2]; // bigger value
-
- const Scalar scale = std::sqrt(area_);
-
- ellipsoid(3) = scale * std::sqrt(std::abs(eigenvalue3)) / Scalar(2); // majorAxis
- ellipsoid(4) = scale * std::sqrt(std::abs(eigenvalue2)) / Scalar(2); // mediumAxis
- ellipsoid(5) = scale * std::sqrt(std::abs(eigenvalue1)) / Scalar(2); // minorAxis
-
- // ROTATION COMPUTATION get rotation of the three axis from eigenvector
- // eigenvector belonging to biggest eigenvalue gives direction / z_axis
- // of ellipsoid
-
- // get eigenvectors that correspond to eigenvalues
- //const Eigen::Vector3s eigenvector1 = solver.eigenvectors().col(0); // minorAxis
- const Eigen::Vector3s eigenvector2 = solver.eigenvectors().col(1); // mediumAxis ->y
- const Eigen::Vector3s eigenvector3 = solver.eigenvectors().col(2); // majorAxis -> x
-
- // computed axis of coordinate system in ellipsoid center
- const Eigen::Vector3s z_axis = eigenvector3.cross(eigenvector2);
- const Eigen::Vector3s y_axis = z_axis.cross(eigenvector3);
-
- // use them to fill rotation matrix
- Eigen::Matrix3s rotationMatrix;
- rotationMatrix.col(0) = eigenvector3.normalized();
- rotationMatrix.col(1) = y_axis.normalized();
- rotationMatrix.col(2) = z_axis.normalized();
-
- // get quaternions for transformation
- Eigen::Quaternions rotation(rotationMatrix);
- rotation.normalize();
-
- ellipsoid(6) = rotation.w();
- ellipsoid(7) = rotation.x();
- ellipsoid(8) = rotation.y();
- ellipsoid(9) = rotation.z();
-
- WOLF_DEBUG("made ellipsoid: \n[", ellipsoid.transpose(), "]");
-
- // [ pos_x, pos_y, pos_z, a, b, c, alpha, beta, gamma]
- return true;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::ellipse2DIntersect(const Eigen::VectorXs& ellipse1,
- const Eigen::VectorXs& ellipse2)
-{
- for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
- {
- // parameterized form of first ellipse gives point on first ellipse
- // https://www.uwgb.edu/dutchs/Geometry/HTMLCanvas/ObliqueEllipses5.HTM
- Eigen::VectorXs pointOnEllipse(2);
- const Scalar theta = Scalar(i) * M_PI / 180.0;
-
- pointOnEllipse(0) = ellipse1(0) +
- ellipse1(2) * std::cos(ellipse1(4)) * std::cos(theta) -
- ellipse1(3) * std::sin(ellipse1(4)) * std::sin(theta);
-
- pointOnEllipse(1) = ellipse1(1) +
- ellipse1(2) * std::sin(ellipse1(4)) * std::cos(theta) +
- ellipse1(3) * std::cos(ellipse1(4)) * std::sin(theta);
-
- //WOLF_DEBUG(" for ", i, " deg / ", theta " rad ---> [" ,
- // pointOnEllipse.transpose(), "]");
-
- // check if point lies inside the other ellipse
- if (point2DIntersect(pointOnEllipse, ellipse2)) return true;
- }
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::point2DIntersect(const Eigen::VectorXs& point,
- const Eigen::VectorXs& ellipse)
-{
- const Scalar area51 = std::pow( ((point(0) - ellipse(0)) * cos(ellipse(4))
- + (point(1) - ellipse(1)) * sin(ellipse(4))), 2 )
- / std::pow( ellipse(2), 2 )
- + std::pow( ((point(0) - ellipse(0)) * sin(ellipse(4))
- - (point(1) - ellipse(1)) * cos(ellipse(4))), 2 )
- / std::pow( ellipse(3), 2 );
-
- //WOLF_DEBUG("computed area = ", area51);
-
- if ( area51 - area_ <= 0) return true;
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
- const Eigen::VectorXs &ellipsoid2)
-{
- // get transformation from elliposiod1 center frame to world frame
- Eigen::Matrix4s transformation_matrix;
- Eigen::Quaternions rotation(ellipsoid1(6),ellipsoid1(7),
- ellipsoid1(8),ellipsoid1(9));
-// Eigen::Vector4s translation;
-// translation << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
- transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
- transformation_matrix.col(3) << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
-
- Eigen::Vector4s point_hom = Eigen::Vector4s::Constant(1);
-
- Scalar omega = 0;
- for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
- {
- const Scalar theta = Scalar(i) * M_PI / 180.0;
- for( int j = 0 ; j < 180 ; j += params_NNF->sample_step_degree_)
- {
- omega = Scalar(j) * M_PI / 180.0;
-
- // compute point on surface of first ellipsoid
- point_hom(0) = ellipsoid1(3) * std::cos(theta) * std::sin(omega);
- point_hom(1) = ellipsoid1(4) * std::sin(theta) * std::sin(omega);
- point_hom(2) = ellipsoid1(5) * std::cos(omega);
-
- // transform point into world frame
- const Eigen::Vector4s point_on_ellipsoid = transformation_matrix * point_hom;
-
- // check if 3D point lies inside the second ellipsoid
- if (point3DIntersect(point_on_ellipsoid, ellipsoid2))
- return true;
- }
- }
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::point3DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipsoid)
-{
- // get transformation from ellipsoid center frame to world frame
- Eigen::Matrix4s transformation_matrix = Eigen::Matrix4s::Identity();
-
- Eigen::Quaternions rotation(ellipsoid(6),ellipsoid(7),
- ellipsoid(8),ellipsoid(9));
-// Eigen::Vector4s translation;
-// translation << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
- transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
- transformation_matrix.col(3) << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
- // inverse to get transformation from world frame to ellipsoid center frame
- transformation_matrix = transformation_matrix.inverse().eval();
-
- // homogenize 3D point
- // ???
- Eigen::Vector4s point_hom;
- point_hom << point(0), point(1), point(2), 1;
-
- // transform point from world frame to elliposiod center frame
- Eigen::Vector4s transformed_point = transformation_matrix * point_hom;
-
- // check if point is inside ellipsoid with general equation
- Scalar area51 = std::pow(transformed_point(0), 2) / std::pow( ellipsoid(3), 2) +
- std::pow(transformed_point(1), 2) / std::pow(ellipsoid(4), 2) +
- std::pow(transformed_point(2), 2) / std::pow(ellipsoid(5), 2);
-
- WOLF_DEBUG("computed area = ", area51);
-
- if (area51 - area_ <= 0) return true;
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame)
-{
- const Scalar distance = MahalanobisDistance(trajectory_frame, query_frame);
-
- WOLF_DEBUG("Mahalanobis Distance = ", distance);
-
- if ( distance < 0 )
- {
- WOLF_DEBUG("NO COVARIANCE AVAILABLE");
- }
-
- /*
- * TODO:
- * check if distance is smaller than a threshold,
- * if yes -> return true
- * else -> return false
- *
- */
- return false;
-}
-
-//##############################################################################
-Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBasePtr& trajectory,
- const FrameBasePtr& query)
-{
- Scalar distance = -1;
- Eigen::VectorXs traj_pose, query_pose;
-
- // get state and covariance matrix for both frames
- if (trajectory->getP()->getState().size() == 2)
- {
- traj_pose.resize(3);
- query_pose.resize(3);
- }
- else
- {
- traj_pose.resize(7);
- query_pose.resize(7);
- }
-
- traj_pose << trajectory->getP()->getState(),
- trajectory->getO()->getState();
-
- query_pose << query->getP()->getState(),
- query->getO()->getState();
-
- Eigen::MatrixXs traj_covariance, query_covariance;
- if ( !getProblem()->getFrameCovariance(trajectory, traj_covariance) ||
- !getProblem()->getFrameCovariance(query, query_covariance) ||
- !isCovariance(traj_covariance) ||
- !isCovariance(query_covariance))
- return distance;
-
- const Eigen::MatrixXs covariance = traj_covariance * query_covariance.transpose();
-
- const Eigen::VectorXs pose_difference = traj_pose - query_pose;
- distance = pose_difference.transpose() * covariance * pose_difference;
- distance = std::sqrt(distance);
-
- return distance;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
-{
- FrameBasePtr keyframe = getProblem()->getLastKeyFrame();
- if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ ))
- return false;
- else
- return true;
-}
-
-} // namespace wolf
-
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 1e8cc96d941d5fe3f47ea91ff8e11a3925ecf25d..d041d8e01fe1303d2318366b2539bc1e4981943b 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -1,8 +1,8 @@
-#include "base/factor/factor_gnss_fix_2D.h"
-#include "base/factor/factor_gnss_fix_3D.h"
-#include "base/feature/feature_gnss_fix.h"
-#include "base/processor/processor_gnss_fix.h"
-#include "base/capture/capture_gnss_fix.h"
+#include "gnss/factor/factor_gnss_fix_2D.h"
+#include "gnss/factor/factor_gnss_fix_3D.h"
+#include "gnss/feature/feature_gnss_fix.h"
+#include "gnss/processor/processor_gnss_fix.h"
+#include "gnss/capture/capture_gnss_fix.h"
namespace wolf
{
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index 179933a0789e5bc268351593f3fba3eaaf6349ef..3263b361e4958086a3e702bf73bb6b64a97b60ba 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -1,7 +1,7 @@
-#include "base/factor/factor_gnss_single_diff_2D.h"
-#include "base/feature/feature_gnss_single_diff.h"
-#include "base/processor/processor_gnss_single_diff.h"
-#include "base/capture/capture_gnss_single_diff.h"
+#include "gnss/factor/factor_gnss_single_diff_2D.h"
+#include "gnss/feature/feature_gnss_single_diff.h"
+#include "gnss/processor/processor_gnss_single_diff.h"
+#include "gnss/capture/capture_gnss_single_diff.h"
namespace wolf
{
diff --git a/src/processor/processor_loopclosure_base.cpp b/src/processor/processor_loopclosure_base.cpp
deleted file mode 100644
index 6953cd103d87b2fdad85df70114bdf1875ef847b..0000000000000000000000000000000000000000
--- a/src/processor/processor_loopclosure_base.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-/**
- * \file processor_loop_closure.h
- *
- * Created on: Aug 10, 2017
- * \author: Tessa Johanna
- */
-
-#include "base/processor/processor_loopclosure_base.h"
-
-namespace wolf
-{
-
-ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure):
- ProcessorBase(_type, _params_loop_closure),
- params_loop_closure_(_params_loop_closure)
-{
- //
-}
-
-//##############################################################################
-void ProcessorLoopClosureBase::process(CaptureBasePtr _incoming_ptr)
-{
- // clear all previous data from vector
- loop_closure_candidates.clear();
- close_candidates.clear();
-
- // the pre-process, if necessary, is implemented in the derived classes
- preProcess();
-
- findCandidates(_incoming_ptr);
-
- confirmLoopClosure();
-
- WOLF_DEBUG("ProcessorLoopClosureBase::process found ",
- loop_closure_candidates.size(), " candidates found.");
-
- // the post-process, if necessary, is implemented in the derived classes
- postProcess();
-}
-
-//##############################################################################
-void ProcessorLoopClosureBase::keyFrameCallback(FrameBasePtr /*_keyframe_ptr*/,
- const Scalar& /*_time_tol_other*/)
-{
- //
-}
-
-}// namespace wolf
-
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
deleted file mode 100644
index a556c3e8328daacd044b6cf22f90e48d456f97a8..0000000000000000000000000000000000000000
--- a/src/processor/processor_motion.cpp
+++ /dev/null
@@ -1,634 +0,0 @@
-#include "base/processor/processor_motion.h"
-namespace wolf
-{
-
-ProcessorMotion::ProcessorMotion(const std::string& _type,
- SizeEigen _state_size,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- SizeEigen _data_size,
- SizeEigen _calib_size,
- ProcessorParamsMotionPtr _params_motion) :
- ProcessorBase(_type, _params_motion),
- params_motion_(_params_motion),
- processing_step_(RUNNING_WITHOUT_PACK),
- x_size_(_state_size),
- data_size_(_data_size),
- delta_size_(_delta_size),
- delta_cov_size_(_delta_cov_size),
- calib_size_(_calib_size),
- origin_ptr_(),
- last_ptr_(),
- incoming_ptr_(),
- dt_(0.0), x_(_state_size),
- data_(_data_size),
- delta_(_delta_size),
- delta_cov_(_delta_cov_size, _delta_cov_size),
- delta_integrated_(_delta_size),
- delta_integrated_cov_(_delta_cov_size, _delta_cov_size),
- calib_preint_(_calib_size),
- jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
- jacobian_delta_(delta_cov_size_, delta_cov_size_),
- jacobian_calib_(delta_size_, calib_size_)
-{
- //
-}
-
-ProcessorMotion::~ProcessorMotion()
-{
-// std::cout << "destructed -p-Mot" << id() << std::endl;
-}
-
-void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
- TimeStamp _ts_split,
- const FrameBasePtr& _keyframe_target,
- const wolf::CaptureMotionPtr& _capture_target)
-{
- // split the buffer
- // and give the part of the buffer before the new keyframe to the capture for the KF callback
- _capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
-
- // interpolate individual delta which has been cut by the split timestamp
- if (!_capture_source->getBuffer().get().empty()
- && _capture_target->getBuffer().get().back().ts_ != _ts_split)
- {
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(_capture_target->getBuffer().get().back(), // last Motion of old buffer
- _capture_source->getBuffer().get().front(), // first motion of new buffer
- _ts_split,
- _capture_source->getBuffer().get().front());
-
- // add to old buffer
- _capture_target->getBuffer().get().push_back(motion_interpolated);
- }
-
- // Update the existing capture
- _capture_source->setOriginFrame(_keyframe_target);
-
- // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
- reintegrateBuffer(_capture_source);
-}
-
-void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
-{
- if (_incoming_ptr == nullptr)
- {
- WOLF_ERROR("Received capture is nullptr.");
- return;
- }
-
- incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
-
- preProcess(); // Derived class operations
-
- PackKeyFramePtr pack = computeProcessingStep();
- if (pack)
- kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
-
-
- switch(processing_step_)
- {
-
- case RUNNING_WITHOUT_PACK :
- {
- WOLF_DEBUG("PM: RUNNING_WITHOUT_PACK");
- }
- case RUNNING_WITH_PACK_ON_ORIGIN :
- {
- WOLF_DEBUG("PM: RUNNING_WITHOUT_PACK / RUNNING_WITH_PACK_ON_ORIGIN");
- break;
- }
-
- case RUNNING_WITH_PACK_BEFORE_ORIGIN :
- {
- WOLF_DEBUG("PM: RUNNING_WITH_PACK_BEFORE_ORIGIN");
- // extract pack elements
- FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
-
- // find the capture whose buffer is affected by the new keyframe
- auto existing_capture = findCaptureContainingTimeStamp(ts_from_callback);
-
- // Find the frame acting as the capture's origin
- auto keyframe_origin = existing_capture->getOriginFrame();
-
- // emplace a new motion capture to the new keyframe
- auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
- getSensor(),
- ts_from_callback,
- Eigen::VectorXs::Zero(data_size_),
- existing_capture->getDataCovariance(),
- existing_capture->getCalibration(),
- existing_capture->getCalibration(),
- keyframe_origin);
-
- // split the buffer
- // and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
-
- // create motion feature and add it to the capture
- auto new_feature = emplaceFeature(capture_for_keyframe_callback);
-
- // create motion factor and add it to the feature, and constrain to the other capture (origin)
- emplaceFactor(new_feature, keyframe_origin->getCaptureOf(getSensor()) );
-
- // modify existing feature and factor (if they exist in the existing capture)
- if (!existing_capture->getFeatureList().empty())
- {
- WOLF_DEBUG("PM: if (!existing_capture->getFeatureList().empty())");
- auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
-
- // Modify existing feature --------
- existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_);
- existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
-
- // Modify existing factor --------
- // Instead of modifying, we remove one ctr, and create a new one.
- auto fac_to_remove = existing_feature->getFactorList().back(); // there is only one factor!
- auto new_ctr = emplaceFactor(existing_feature, capture_for_keyframe_callback);
- fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
- }
- break;
- }
-
- case RUNNING_WITH_PACK_AFTER_ORIGIN :
- {
- WOLF_DEBUG("PM: RUNNING_WITH_PACK_AFTER_ORIGIN");
- // extract pack elements
- FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
-
- // Find the frame acting as the capture's origin
- auto keyframe_origin = last_ptr_->getOriginFrame();
-
- // emplace a new motion capture to the new keyframe
- VectorXs calib = last_ptr_->getCalibration();
- auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
- getSensor(),
- ts_from_callback,
- Eigen::VectorXs::Zero(data_size_),
- last_ptr_->getDataCovariance(),
- calib,
- calib,
- keyframe_origin);
-
- // split the buffer
- // and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(last_ptr_, ts_from_callback, keyframe_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, keyframe_origin->getCaptureOf(getSensor()) );
-
- // reset processor origin
- origin_ptr_ = capture_for_keyframe_callback;
-
- break;
- }
-
- default :
- break;
- }
-
- ////////////////////////////////////////////////////
- // NOW on with the received data
-
- // integrate data
- integrateOneStep();
-
- // Update state and time stamps
- last_ptr_->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setState(getCurrentState());
-
- if (voteForKeyFrame() && permittedKeyFrame())
- {
- // Set the frame of last_ptr as key
- auto key_frame_ptr = last_ptr_->getFrame();
- key_frame_ptr->setKey();
-
- // create motion feature and add it to the key_capture
- auto key_feature_ptr = emplaceFeature(last_ptr_);
-
- // create motion factor and link it to parent feature and other frame (which is origin's frame)
- auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
-
- // create a new frame
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
- getCurrentState(),
- getCurrentTimeStamp());
- // create a new capture
- auto new_capture_ptr = emplaceCapture(new_frame_ptr,
- getSensor(),
- key_frame_ptr->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
- last_ptr_->getCalibration(),
- last_ptr_->getCalibration(),
- key_frame_ptr);
- // reset the new buffer
- new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
-
- // reset integrals
- delta_ = deltaZero();
- delta_cov_ . setZero();
- delta_integrated_ = deltaZero();
- delta_integrated_cov_ . setZero();
- jacobian_calib_ . setZero();
-
- // reset derived things
- resetDerived();
-
- // Update pointers
- origin_ptr_ = last_ptr_;
- last_ptr_ = new_capture_ptr;
-
- // callback to other processors
- getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), params_motion_->time_tolerance);
- }
-
- resetDerived();
-
- // clear incoming just in case
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
-
- postProcess();
-}
-
-bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
-{
- CaptureMotionPtr capture_motion;
- if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
- // timestamp found in the current processor buffer
- capture_motion = last_ptr_;
- else
- // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
- capture_motion = findCaptureContainingTimeStamp(_ts);
-
- if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
- {
- // Get origin state and calibration
- VectorXs state_0 = capture_motion->getOriginFrame()->getState();
- CaptureBasePtr cap_orig = capture_motion->getOriginFrame()->getCaptureOf(getSensor());
- VectorXs calib = cap_orig->getCalibration();
-
- // Get delta and correct it with new calibration params
- VectorXs calib_preint = capture_motion->getBuffer().getCalibrationPreint();
- Motion motion = capture_motion->getBuffer().getMotion(_ts);
-
- VectorXs delta_step = motion.jacobian_calib_ * (calib - calib_preint);
- VectorXs delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
-
- // Compose on top of origin state using the buffered time stamp, not the query time stamp
- // TODO Interpolate the delta to produce a state at the query time stamp _ts
- Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; // = _ts - capture_motion->getOrigin()->getTimeStamp();
- statePlusDelta(state_0, delta, dt, _x);
- }
- else
- {
- // We could not find any CaptureMotion for the time stamp requested
- WOLF_ERROR("Could not find any Capture for the time stamp requested. ");
- WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?");
- return false;
- }
- return true;
-}
-
-FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
-{
- FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _x_origin, _ts_origin);
- setOrigin(key_frame_ptr);
-
- return key_frame_ptr;
-}
-
-void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
-{
- assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
- assert(_origin_frame->getTrajectory() != nullptr
- && "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
- assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
-
- // -------- ORIGIN ---------
- // emplace (empty) origin Capture
- origin_ptr_ = emplaceCapture(_origin_frame,
- getSensor(),
- _origin_frame->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
- getSensor()->getCalibration(),
- getSensor()->getCalibration(),
- nullptr);
-
- // ---------- LAST ----------
- // Make non-key-frame for last Capture
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
- _origin_frame->getState(),
- _origin_frame->getTimeStamp());
- // emplace (emtpy) last Capture
- last_ptr_ = emplaceCapture(new_frame_ptr,
- getSensor(),
- _origin_frame->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
- getSensor()->getCalibration(),
- getSensor()->getCalibration(),
- _origin_frame);
-
- // clear and reset buffer
- getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
-
- // Reset integrals
- delta_ = deltaZero();
- delta_cov_ . setZero();
- delta_integrated_ = deltaZero();
- delta_integrated_cov_ . setZero();
- jacobian_calib_ . setZero();
-
- // Reset derived things
- resetDerived();
-}
-
-void ProcessorMotion::integrateOneStep()
-{
- // Set dt
- dt_ = updateDt();
- assert(dt_ >= 0 && "Time interval _dt is negative!");
-
- // get vector of parameters to calibrate
- calib_preint_ = getBuffer().getCalibrationPreint();
-
- // get data and convert it to delta, and obtain also the delta covariance
- computeCurrentDelta(incoming_ptr_->getData(),
- incoming_ptr_->getDataCovariance(),
- calib_preint_,
- dt_,
- delta_,
- delta_cov_,
- jacobian_delta_calib_);
-
- // integrate the current delta to pre-integrated measurements, and get Jacobians
- deltaPlusDelta(getBuffer().get().back().delta_integr_,
- delta_,
- dt_,
- delta_integrated_,
- jacobian_delta_preint_,
- jacobian_delta_);
-
- // integrate Jacobian wrt calib
- if ( hasCalibration() )
- jacobian_calib_.noalias()
- = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
- + jacobian_delta_ * jacobian_delta_calib_;
-
- // Integrate covariance
- delta_integrated_cov_.noalias()
- = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
- + jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
-
- // push all into buffer
- getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
- incoming_ptr_->getData(),
- incoming_ptr_->getDataCovariance(),
- delta_,
- delta_cov_,
- delta_integrated_,
- delta_integrated_cov_,
- jacobian_delta_,
- jacobian_delta_preint_,
- jacobian_calib_);
-}
-
-void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
-{
- // start with empty motion
- _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFrame()->getTimeStamp()));
-
- VectorXs calib = _capture_ptr->getCalibrationPreint();
-
- // Iterate all the buffer
- auto motion_it = _capture_ptr->getBuffer().get().begin();
- auto prev_motion_it = motion_it;
- motion_it++;
- while (motion_it != _capture_ptr->getBuffer().get().end())
- {
- // get dt
- const Scalar dt = motion_it->ts_ - prev_motion_it->ts_;
-
- // re-convert data to delta with the new calibration parameters
- computeCurrentDelta(motion_it->data_,
- motion_it->data_cov_,
- calib,
- dt,
- motion_it->delta_,
- motion_it->delta_cov_,
- jacobian_delta_calib_);
-
- // integrate delta into delta_integr, and rewrite the buffer
- deltaPlusDelta(prev_motion_it->delta_integr_,
- motion_it->delta_,
- dt,
- motion_it->delta_integr_,
- motion_it->jacobian_delta_integr_,
- motion_it->jacobian_delta_);
-
- // integrate Jacobian wrt calib
- if ( hasCalibration() )
- motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * prev_motion_it->jacobian_calib_ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
-
- // Integrate covariance
- motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * prev_motion_it->delta_integr_cov_ * motion_it->jacobian_delta_integr_.transpose()
- + motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
-
- // advance in buffer
- motion_it++;
- prev_motion_it++;
- }
-}
-
-Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
-{
- // Check time bounds
- assert(_ref.ts_ <= _second.ts_ && "Interpolation bounds not causal.");
- assert(_ts >= _ref.ts_ && "Interpolation time is before the _ref motion.");
- assert(_ts <= _second.ts_ && "Interpolation time is after the _second motion.");
-
- // Fraction of the time interval
- Scalar tau = (_ts - _ref.ts_) / (_second.ts_ - _ref.ts_);
-
- if (tau < 0.5)
- {
- // _ts is closest to _ref
- Motion interpolated ( _ref );
- interpolated.ts_ = _ts;
- interpolated.data_ . setZero();
- interpolated.data_cov_ . setZero();
- interpolated.delta_ = deltaZero();
- interpolated.delta_cov_ . setZero();
- interpolated.jacobian_delta_integr_ . setIdentity();
- interpolated.jacobian_delta_ . setZero();
-
- return interpolated;
- }
- else
- {
- // _ts is closest to _second
- Motion interpolated ( _second );
- interpolated.ts_ = _ts;
- _second.data_ . setZero();
- _second.data_cov_ . setZero();
- _second.delta_ = deltaZero();
- _second.delta_cov_ . setZero();
- _second.jacobian_delta_integr_ . setIdentity();
- _second.jacobian_delta_ . setZero();
-
- return interpolated;
- }
-
-}
-
-Motion ProcessorMotion::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
-{
- // Check time bounds
- assert(_ref1.ts_ <= _ref2.ts_ && "Interpolation bounds not causal.");
- assert(_ts >= _ref1.ts_ && "Interpolation time is before the _ref1 motion.");
- assert(_ts <= _ref2.ts_ && "Interpolation time is after the _ref2 motion.");
-
- // Fraction of the time interval
- Scalar tau = (_ts - _ref1.ts_) / (_ref2.ts_ - _ref1.ts_);
-
- if (tau < 0.5)
- {
- // _ts is closest to _ref1
- Motion interpolated ( _ref1 );
- interpolated.ts_ = _ts;
- interpolated.data_ . setZero();
- interpolated.data_cov_ . setZero();
- interpolated.delta_ = deltaZero();
- interpolated.delta_cov_ . setZero();
- interpolated.jacobian_delta_integr_ . setIdentity();
- interpolated.jacobian_delta_ . setZero();
-
- _second = _ref2;
-
- return interpolated;
- }
- else
- {
- // _ts is closest to _ref2
- Motion interpolated ( _ref2 );
- interpolated.ts_ = _ts;
-
- _second = _ref2;
- _second.data_ . setZero();
- _second.data_cov_ . setZero();
- _second.delta_ = deltaZero();
- _second.delta_cov_ . setZero();
- _second.jacobian_delta_integr_ . setIdentity();
- _second.jacobian_delta_ . setZero();
-
- return interpolated;
- }
-
-}
-
-CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
-{
- // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
- // Note: since the buffer goes from a KF in the past until the next KF, we need to:
- // 1. See that the KF contains a CaptureMotion
- // 2. See that the TS is smaller than the KF's TS
- // 3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer
- FrameBasePtr frame = nullptr;
- CaptureBasePtr capture = nullptr;
- CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend();
- ++frame_rev_iter)
- {
- frame = *frame_rev_iter;
- capture = frame->getCaptureOf(getSensor());
- if (capture != nullptr)
- {
- // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
- capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
- if (capture_motion->getOriginFrame() && !capture_motion->getBuffer().get().empty() && _ts >= capture_motion->getBuffer().get().front().ts_)
- // Found time stamp satisfying rule 3 above !! ==> break for loop
- break;
- else
- capture_motion = nullptr;
- }
- }
- return capture_motion;
-}
-
-CaptureMotionPtr ProcessorMotion::emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
- const FrameBasePtr& _frame_origin)
-{
- CaptureMotionPtr capture = createCapture(_ts,
- _sensor,
- _data,
- _data_cov,
- _frame_origin);
-
- capture->setCalibration(_calib);
- capture->setCalibrationPreint(_calib_preint);
-
- // add to wolf tree
- _frame_own->addCapture(capture);
- return capture;
-}
-
-FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureBasePtr feature = createFeature(_capture_motion);
- _capture_motion->addFeature(feature);
- return feature;
-}
-
-PackKeyFramePtr ProcessorMotion::computeProcessingStep()
-{
- if (!getProblem()->priorIsSet())
- {
- WOLF_WARN ("||*||");
- WOLF_INFO (" ... It seems you missed something!");
- WOLF_ERROR("ProcessorMotion received data before being initialized.");
- WOLF_INFO ("Did you forget to issue a Problem::setPrior()?");
- throw std::runtime_error("ProcessorMotion received data before being initialized.");
- }
-
- PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
-
- if (pack)
- {
- if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
- {
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN;
- }
- else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
- processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
-
- // ensure pack is before incoming_ptr_
- else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
- processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
-
- }
- else
- processing_step_ = RUNNING_WITHOUT_PACK;
-
- return pack;
-}
-
-}
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
deleted file mode 100644
index a474ed1f3a3bd6d015624d4e3cb5165de005dd16..0000000000000000000000000000000000000000
--- a/src/processor/processor_odom_2D.cpp
+++ /dev/null
@@ -1,198 +0,0 @@
-#include "base/processor/processor_odom_2D.h"
-namespace wolf
-{
-
-ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
- ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params),
- params_odom_2D_(_params)
-{
- unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3s::Identity();
-}
-
-ProcessorOdom2D::~ProcessorOdom2D()
-{
-}
-
-void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib, const Scalar _dt, Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _jacobian_calib)
-{
- assert(_data.size() == data_size_ && "Wrong _data vector size");
- assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
- assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
-
- // data is [dr, dtheta]
- // delta is [dx, dy, dtheta]
- // motion model is 1/2 turn + straight + 1/2 turn
- _delta(0) = cos(_data(1) / 2) * _data(0);
- _delta(1) = sin(_data(1) / 2) * _data(0);
- _delta(2) = _data(1);
-
- // Fill delta covariance
- Eigen::MatrixXs J(delta_cov_size_, data_size_);
- J(0, 0) = cos(_data(1) / 2);
- J(1, 0) = sin(_data(1) / 2);
- J(2, 0) = 0;
- J(0, 1) = -_data(0) * sin(_data(1) / 2) / 2;
- J(1, 1) = _data(0) * cos(_data(1) / 2) / 2;
- J(2, 1) = 1;
-
- // Since input data is size 2, and delta is size 3, the noise model must be given by:
- // 1. Covariance of the input data: J*Q*J.tr
- // 2. Fixed variance term to be added: var*Id
- _delta_cov = J * _data_cov * J.transpose() + unmeasured_perturbation_cov_;
-}
-
-void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2)
-{
- assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
- assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
- assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
-
- // This is just a frame composition in 2D
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
- _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
-}
-
-void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2)
-{
- assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
- assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
- assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- assert(_jacobian1.rows() == delta_cov_size_ && "Wrong _jacobian1 size");
- assert(_jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
- assert(_jacobian2.rows() == delta_cov_size_ && "Wrong _jacobian2 size");
- assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
-
- // This is just a frame composition in 2D
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
- _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
-
- // Jac wrt delta_integrated
- _jacobian1 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
- _jacobian1(0, 2) = -sin(_delta1(2)) * _delta2(0) - cos(_delta1(2)) * _delta2(1);
- _jacobian1(1, 2) = cos(_delta1(2)) * _delta2(0) - sin(_delta1(2)) * _delta2(1);
-
- // jac wrt delta
- _jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
- _jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
-}
-
-void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta)
-{
- assert(_x.size() == x_size_ && "Wrong _x vector size");
- assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
-
- // This is just a frame composition in 2D
- _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Ds(_x(2)).matrix() * _delta.head<2>();
- _x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
-}
-
-Motion ProcessorOdom2D::interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
-{
- // TODO: Implement actual interpolation
- // Implementation: motion ref keeps the same
- //
-// Motion _interpolated(_ref);
-// _interpolated.ts_ = _ts;
-// _interpolated.data_ = Vector3s::Zero();
-// _interpolated.data_cov_ = Matrix3s::Zero();
-// _interpolated.delta_ = deltaZero();
-// _interpolated.delta_cov_ = Eigen::MatrixXs::Zero(delta_size_, delta_size_);
-// _interpolated.delta_integr_ = _ref.delta_integr_;
-// _interpolated.delta_integr_cov_ = _ref.delta_integr_cov_;
-// _interpolated.jacobian_delta_integr_.setIdentity();
-// _interpolated.jacobian_delta_.setZero();
-// _interpolated.jacobian_calib_.setZero();
-// return _interpolated;
-
- return ProcessorMotion::interpolate(_ref, _second, _ts);
-
-}
-
-Motion ProcessorOdom2D::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
-{
- return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
-}
-
-bool ProcessorOdom2D::voteForKeyFrame()
-{
- // Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2D_->dist_traveled)
- {
- return true;
- }
- if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2D_->angle_turned)
- {
- return true;
- }
- // Uncertainty criterion
- if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2D_->cov_det)
- {
- return true;
- }
- // Time criterion
- WOLF_TRACE("orig t: ", origin_ptr_->getFrame()->getTimeStamp().get(), " current t: ", getBuffer().get().back().ts_.get() , " dt: ", getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get());
- if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2D_->max_time_span)
- {
- return true;
- }
- return false;
-}
-
-CaptureMotionPtr ProcessorOdom2D::createCapture(const TimeStamp& _ts, const SensorBasePtr& _sensor,
- const VectorXs& _data, const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
-{
- CaptureOdom2DPtr capture_odom = std::make_shared<CaptureOdom2D>(_ts, _sensor, _data, _data_cov, _frame_origin);
- return capture_odom;
-}
-
-FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
-{
- FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
- shared_from_this());
- _feature->addFactor(fac_odom);
- _capture_origin->getFrame()->addConstrainedBy(fac_odom);
- return fac_odom;
-}
-
-FeatureBasePtr ProcessorOdom2D::createFeature(CaptureMotionPtr _capture_motion)
-{
- Eigen::MatrixXs covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
- makePosDef(covariance);
-
- FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>("ODOM 2D",
- _capture_motion->getBuffer().get().back().delta_integr_,
- covariance);
- return key_feature_ptr;
-}
-
-ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
-
- ProcessorOdom2DPtr prc_ptr;
-
- std::shared_ptr<ProcessorParamsOdom2D> params;
- if (_params)
- params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params);
- else
- params = std::make_shared<ProcessorParamsOdom2D>();
-
- prc_ptr = std::make_shared<ProcessorOdom2D>(params);
- prc_ptr->setName(_unique_name);
-
- return prc_ptr;
-}
-
-}
-
-// Register in the ProcessorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("ODOM 2D", ProcessorOdom2D)
-} // namespace wolf
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
deleted file mode 100644
index a51145b9ad807b64176902a9fd8f404648622be0..0000000000000000000000000000000000000000
--- a/src/processor/processor_odom_3D.cpp
+++ /dev/null
@@ -1,451 +0,0 @@
-#include "base/processor/processor_odom_3D.h"
-namespace wolf
-{
-
-ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr) :
- ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params),
- params_odom_3D_(_params),
- p1_(nullptr), p2_(nullptr), p_out_(nullptr),
- q1_(nullptr), q2_(nullptr), q_out_(nullptr)
- {
- configure(_sensor_ptr);
- delta_ = deltaZero();
- delta_integrated_ = deltaZero();
- jacobian_delta_preint_.setZero(delta_cov_size_, delta_cov_size_);
- jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_);
- }
-
-ProcessorOdom3D::~ProcessorOdom3D()
-{
-}
-
-void ProcessorOdom3D::configure(SensorBasePtr _sensor)
-{
- if (_sensor)
- {
- SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor);
- // we steal the parameters from the provided odom3D sensor.
- k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
- k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor();
- k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor();
- min_disp_var_ = sen_ptr->getMinDispVar();
- min_rot_var_ = sen_ptr->getMinRotVar();
- }
- else
- {
- // we put default params.
- k_disp_to_disp_ = 0;
- k_disp_to_rot_ = 0;
- k_rot_to_rot_ = 0;
- min_disp_var_ = 0.1; // around 30cm error
- min_rot_var_ = 0.1; // around 9 degrees error
- }
-}
-
-void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib)
-{
- assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3D.");
- Scalar disp, rot; // displacement and rotation scalars of this motion step
- Vector3s vtheta; // rotation vector
- if (_data.size() == (long int)6)
- {
- _delta.head<3>() = _data.head<3>();
- new (&q_out_) Eigen::Map<Eigen::Quaternions>(_delta.data() + 3);
-
- vtheta = _data.tail<3>();
- q_out_ = v2q(vtheta);
- disp = _data.head<3>().norm();
- rot = vtheta.norm();
- }
- else
- {
- // rotation in quaternion form
- _delta = _data;
- vtheta = q2v(_data.tail<4>());
- disp = _data.head<3>().norm();
- rot = 2 * acos(_data(3));
- }
- /* Jacobians of d = computeCurrentDelta(data, dt)
- * with: d = [Dp Dq]
- * data = [dp do]
- *
- * Dp = dp
- * Dq = v2q(do) = exp(do)
- *
- * J = dD/dd, with:
- *
- * dDp/ddp = I
- * dDp/ddo = 0
- * dDo/ddp = 0
- * dDo/ddo = Jr(do)
- *
- * so, delta_cov = J * _data_cov * J\tr
- */
-
-
- // Static covariance from data_cov
-// Matrix6s J(Matrix6s::Identity());
-// J.block(3,3,3,3) = wolf::jac_SO3_right(vtheta);
-// _delta_cov = J * _data_cov * J.transpose();
-
- // We discard _data_cov and create a new one from the measured motion
- Scalar disp_var = min_disp_var_ + k_disp_to_disp_ * disp;
- Scalar rot_var = min_rot_var_ + k_disp_to_rot_ * disp + k_rot_to_rot_ * rot;
- Eigen::Matrix6s data_cov(Eigen::Matrix6s::Identity());
- data_cov(0, 0) = data_cov(1, 1) = data_cov(2, 2) = disp_var;
- data_cov(3, 3) = data_cov(4, 4) = data_cov(5, 5) = rot_var;
- _delta_cov = data_cov;
-}
-
-void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2)
-{
- assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
- assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
- assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- assert(_jacobian1.rows() == delta_cov_size_ && _jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
- assert(_jacobian2.rows() == delta_cov_size_ && _jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
- remap(_delta1, _delta2, _delta1_plus_delta2);
- /* Jacobians of D' = D (+) d
- * with: D = [Dp Dq]
- * d = [dp dq]
- *
- * dDp'/dDp = I
- * dDp'/dDo = -DR * skew(dp) // (Sola 16, ex. B.3.2 and Sec. 7.2.3)
- * dDo'/dDp = 0
- * dDo'/dDo = dR.tr // (Sola 16, Sec. 7.2.3)
- *
- * dDp'/ddp = DR
- * dDp'/ddo = 0
- * dDo'/ddp = 0
- * dDo'/ddo = I
- */
-
- // temporaries
- Eigen::Matrix3s DR = q1_.matrix();
- Eigen::Matrix3s dR = q2_.matrix();
-
- // fill Jacobians
- _jacobian1.setIdentity();
- _jacobian1.block<3, 3>(0, 3) = -DR * skew(p2_); // (Sola 16, ex. B.3.2 and Sec. 7.2.3)
- _jacobian1.block<3, 3>(3, 3) = dR.transpose(); // (Sola 16, Sec. 7.2.3)
-
- _jacobian2.setIdentity();
- _jacobian2.block<3, 3>(0, 0) = DR; // (Sola 16, Sec. 7.2.3)
-
- // perform composition here to avoid aliasing problems if _delta1 and _delta_plus_delta share the same storage
- p_out_ = p1_ + q1_ * p2_;
- q_out_ = q1_ * q2_;
-}
-
-void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2)
-{
- assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
- assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
- assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- remap(_delta1, _delta2, _delta1_plus_delta2);
- p_out_ = p1_ + q1_ * p2_;
- q_out_ = q1_ * q2_;
-}
-
-void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta)
-{
- assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
- assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
- assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
- remap(_x.head(x_size_), _delta, _x_plus_delta); //we take only the x_sixe_ first elements of the state Vectors (Position + Orientation)
- p_out_ = p1_ + q1_ * p2_;
- q_out_ = q1_ * q2_;
-}
-
-Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref,
- Motion& _motion_second,
- TimeStamp& _ts)
-{
-
-// WOLF_TRACE("motion ref ts: ", _motion_ref.ts_.get());
-// WOLF_TRACE("interp ts : ", _ts.get());
-// WOLF_TRACE("motion ts : ", _motion_second.ts_.get());
-//
-// WOLF_TRACE("ref delta size: ", _motion_ref.delta_.size(), " , cov size: ", _motion_ref.delta_cov_.size());
-// WOLF_TRACE("ref Delta size: ", _motion_ref.delta_integr_.size(), " , cov size: ", _motion_ref.delta_integr_cov_.size());
-// WOLF_TRACE("sec delta size: ", _motion_second.delta_.size(), " , cov size: ", _motion_second.delta_cov_.size());
-// WOLF_TRACE("sec Delta size: ", _motion_second.delta_integr_.size(), " , cov size: ", _motion_second.delta_integr_cov_.size());
-
- // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
- if (_ts <= _motion_ref.ts_) // behave as if _ts == _motion_ref.ts_
- { // return null motion. Second stays the same.
- Motion motion_int(_motion_ref);
- motion_int.delta_ = deltaZero();
- motion_int.delta_cov_.setZero();
- return motion_int;
- }
- if (_motion_second.ts_ <= _ts) // behave as if _ts == _motion_second.ts_
- { // return original second motion. Second motion becomes null motion
- Motion motion_int(_motion_second);
- _motion_second.delta_ = deltaZero();
- _motion_second.delta_cov_.setZero();
- return motion_int;
- }
-
- assert(_motion_ref.ts_ <= _ts && "Interpolation time stamp out of bounds");
- assert(_ts <= _motion_second.ts_ && "Interpolation time stamp out of bounds");
-
- assert(_motion_ref.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_.size() == delta_size_ && "Wrong delta size");
-// assert(_motion_ref.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_motion_ref.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_.size() == delta_size_ && "Wrong delta size");
-// assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- using namespace Eigen;
- // Interpolate between motion_ref and motion, as in:
- //
- // motion_ref ------ ts_ ------ motion
- // return
- //
- // and return the value at the given time_stamp ts_.
- //
- // The position receives linear interpolation:
- // p_ret = (ts - t_ref) / dt * (p - p_ref)
- //
- // the quaternion receives a slerp interpolation
- // q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
- //
- // See extensive documentation in ProcessorMotion::interpolate().
-
- // reference
- TimeStamp t_ref = _motion_ref.ts_;
-
- // final
- TimeStamp t_sec = _motion_second.ts_;
- Map<VectorXs> dp_sec(_motion_second.delta_.data(), 3);
- Map<Quaternions> dq_sec(_motion_second.delta_.data() + 3);
-
- // interpolated
- Motion motion_int = motionZero(_ts);
- Map<VectorXs> dp_int(motion_int.delta_.data(), 3);
- Map<Quaternions> dq_int(motion_int.delta_.data() + 3);
-
- // Jacobians for covariance propagation
- MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
- MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
- // interpolate delta
- Scalar tau = (_ts - t_ref) / (t_sec - t_ref); // interpolation factor (0 to 1)
- motion_int.ts_ = _ts;
- dp_int = tau * dp_sec;
- dq_int = Quaternions::Identity().slerp(tau, dq_sec);
- deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, (t_sec - t_ref), motion_int.delta_integr_, J_ref, J_int);
-
- // interpolate covariances
- motion_int.delta_cov_ = tau * _motion_second.delta_cov_;
-// motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose() + J_int * _motion_second.delta_cov_ * J_int.transpose();
-
- // update second delta ( in place update )
- dp_sec = dq_int.conjugate() * ((1 - tau) * dp_sec);
- dq_sec = dq_int.conjugate() * dq_sec;
- _motion_second.delta_cov_ = (1 - tau) * _motion_second.delta_cov_; // easy interpolation // TODO check for correctness
- //Dp = Dp; // trivial, just leave the code commented
- //Dq = Dq; // trivial, just leave the code commented
- //_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
-
- return motion_int;
-}
-
-Motion ProcessorOdom3D::interpolate(const Motion& _ref1,
- const Motion& _ref2,
- const TimeStamp& _ts,
- Motion& _second)
-{
- // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
- if (_ts <= _ref1.ts_ || _ref2.ts_ <= _ts)
- return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
-
- assert(_ref1.ts_ <= _ts && "Interpolation time stamp out of bounds");
- assert(_ts <= _ref2.ts_ && "Interpolation time stamp out of bounds");
-
- assert(_ref1.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_ref1.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref1.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref1.delta_integr_.size() == delta_size_ && "Wrong delta size");
- // assert(_motion_ref.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- // assert(_motion_ref.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref2.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_ref2.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref2.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref2.delta_integr_.size() == delta_size_ && "Wrong delta size");
- // assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- // assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
-
- using namespace Eigen;
- // Interpolate between ref1 and ref2, as in:
- //
- // ref1 ------ ts ------ ref2
- // return second
- //
- // and return the value at the given time_stamp ts_, and the second motion.
- //
- // The position receives linear interpolation:
- // p_ret = (ts - t_ref) / dt * (p - p_ref)
- //
- // the quaternion receives a slerp interpolation
- // q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
- //
- // See extensive documentation in ProcessorMotion::interpolate().
-
- // reference
- TimeStamp t1 = _ref1.ts_;
-
- // final
- TimeStamp t2 = _ref2.ts_;
- Map<const VectorXs> dp2(_ref2.delta_.data(), 3);
- Map<const Quaternions> dq2(_ref2.delta_.data() + 3);
-
- // interpolated
- Motion motion_int = motionZero(_ts);
- Map<VectorXs> dp_int(motion_int.delta_.data(), 3);
- Map<Quaternions> dq_int(motion_int.delta_.data() + 3);
-
- // Jacobians for covariance propagation
- MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
- MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
- // interpolate delta
- Scalar tau = (_ts - t1) / (t2 - t1); // interpolation factor (0 to 1)
- motion_int.ts_ = _ts;
- dp_int = tau * dp2;
- dq_int = Quaternions::Identity().slerp(tau, dq2);
- deltaPlusDelta(_ref1.delta_integr_, motion_int.delta_, (t2 - t1), motion_int.delta_integr_, J_ref, J_int);
-
- // interpolate covariances
- motion_int.delta_cov_ = tau * _ref2.delta_cov_;
- // motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose() + J_int * _motion_second.delta_cov_ * J_int.transpose();
-
- // update second delta ( in place update )
- _second = _ref2;
- Map<VectorXs> dp_sec(_second.delta_.data(), 3);
- Map<Quaternions> dq_sec(_second.delta_.data() + 3);
- dp_sec = dq_int.conjugate() * ((1 - tau) * dp2);
- dq_sec = dq_int.conjugate() * dq2;
- _second.delta_cov_ = (1 - tau) * _ref2.delta_cov_; // easy interpolation // TODO check for correctness
- //Dp = Dp; // trivial, just leave the code commented
- //Dq = Dq; // trivial, just leave the code commented
- //_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
-
- return motion_int;
-}
-
-
-ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
-{
- // cast inputs to the correct type
- std::shared_ptr<ProcessorParamsOdom3D> prc_odo_params = std::static_pointer_cast<ProcessorParamsOdom3D>(_params);
-
- SensorOdom3DPtr sen_odo =std::static_pointer_cast<SensorOdom3D>(_sen_ptr);
-
- // construct processor
- ProcessorOdom3DPtr prc_odo = std::make_shared<ProcessorOdom3D>(prc_odo_params, sen_odo);
-
- // setup processor
- prc_odo->setName(_unique_name);
-
- return prc_odo;
-}
-
-bool ProcessorOdom3D::voteForKeyFrame()
-{
- //WOLF_DEBUG( "Time span : " , getBuffer().get().back().ts_ - getBuffer().get().front().ts_ );
- //WOLF_DEBUG( " last ts : ", getBuffer().get().back().ts_);
- //WOLF_DEBUG( " first ts : ", getBuffer().get().front().ts_);
- //WOLF_DEBUG( "BufferLength: " , getBuffer().get().size() );
- //WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
- //WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
- // time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3D_->max_time_span)
- {
- WOLF_DEBUG( "PM: vote: time span" );
- return true;
- }
- // buffer length
- if (getBuffer().get().size() > params_odom_3D_->max_buff_length)
- {
- WOLF_DEBUG( "PM: vote: buffer size" );
- return true;
- }
- // distance traveled
- Scalar dist = getMotion().delta_integr_.head(3).norm();
- if (dist > params_odom_3D_->dist_traveled)
- {
- WOLF_DEBUG( "PM: vote: distance traveled" );
- return true;
- }
- // angle turned
- Scalar angle = q2v(Quaternions(getMotion().delta_integr_.data()+3)).norm(); // 2.0 * acos(getMotion().delta_integr_(6));
- if (angle > params_odom_3D_->angle_turned)
- {
- WOLF_DEBUG( "PM: vote: angle turned" );
- return true;
- }
- //WOLF_DEBUG( "PM: do not vote" );
- return false;
-}
-
-CaptureMotionPtr ProcessorOdom3D::createCapture(const TimeStamp& _ts, const SensorBasePtr& _sensor,
- const VectorXs& _data, const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
-{
- CaptureOdom3DPtr capture_odom = std::make_shared<CaptureOdom3D>(_ts, _sensor, _data, _data_cov, _frame_origin);
- return capture_odom;
-}
-
-FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
-{
- FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
- shared_from_this());
- _feature_motion->addFactor(fac_odom);
- _capture_origin->getFrame()->addConstrainedBy(fac_odom);
- return fac_odom;
-}
-
-FeatureBasePtr ProcessorOdom3D::createFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>(
- "ODOM 3D", _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_);
- return key_feature_ptr;
-}
-
-void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x2, Eigen::VectorXs& _x_out)
-{
- new (&p1_) Eigen::Map<const Eigen::Vector3s>(_x1.data());
- new (&q1_) Eigen::Map<const Eigen::Quaternions>(_x1.data() + 3);
- new (&p2_) Eigen::Map<const Eigen::Vector3s>(_x2.data());
- new (&q2_) Eigen::Map<const Eigen::Quaternions>(_x2.data() + 3);
- new (&p_out_) Eigen::Map<Eigen::Vector3s>(_x_out.data());
- new (&q_out_) Eigen::Map<Eigen::Quaternions>(_x_out.data() + 3);
-}
-
-}
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("ODOM 3D", ProcessorOdom3D)
-} // namespace wolf
diff --git a/src/processor/processor_polyline.cpp b/src/processor/processor_polyline.cpp
deleted file mode 100644
index 38c9794c87182363cbca8eb56346ecf0d830ca39..0000000000000000000000000000000000000000
--- a/src/processor/processor_polyline.cpp
+++ /dev/null
@@ -1,925 +0,0 @@
-/*
- * processor_polyline.cpp
- *
- * Created on: Sep 14, 2017
- * Author: jvallve
- */
-
-#include "base/processor/processor_polyline.h"
-#include <algorithm> // std::swap
-
-namespace wolf
-{
-
-ProcessorPolyline::ProcessorPolyline(ProcessorParamsPolylinePtr _params) :
- ProcessorBase("POLYLINES",_params),
- last_ptr_(nullptr),
- incoming_ptr_(nullptr),
- line_finder_(_params->line_finder_params),
- params_(_params),
- extrinsics_transformation_computed_(false)
-{
-}
-
-ProcessorPolyline::~ProcessorPolyline()
-{
-}
-
-// ALGORITHM //////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
-{
- //WOLF_DEBUG( "PROCESS ------------------" );
- incoming_ptr_ = _incoming_ptr;
-
- // extract corners of incoming
- extractPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), new_features_incoming_);
- //std::cout << new_features_incoming_.size() << " polylines extracted" << std::endl;
-
- // compute transformations
- computeTransformations(incoming_ptr_->getTimeStamp());
-
- // FIRST TIME: create KF
- if (last_ptr_ == nullptr)
- {
- //WOLF_DEBUG( "FIRST TIME" );
-
- // advance this
- last_ptr_ = incoming_ptr_;
- new_features_last_ = std::move(new_features_incoming_);
- incoming_ptr_ = nullptr;
-
- // keyframe for last
- PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( last_ptr_, params_->time_tolerance);
- // Exist a KF within the time tolerance
- if (KF_pack)
- {
- KF_pack->key_frame->addCapture(last_ptr_);
- WOLF_TRACE( "PR ",getName() ," - FIRST TIME: Last capture ", last_ptr_->id(), " appended to existing KF: " , KF_pack->key_frame->id() );
- }
- else
- {
- // Make KF
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, last_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
- WOLF_TRACE( "PR ",getName() ," - FIRST TIME: Last capture ", last_ptr_->id(), " appended to a new KF: " , new_frame_ptr->id() );
- getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_->time_tolerance);
- }
-
- // Create landmarks from new_features_last_
- processNew();
-
- // Establish factors from last
- establishFactors();
- }
- // OTHER TIMES
- else
- {
- assert(last_ptr_->getFrame() && "last_ not linked to any frame in OTHER TIMES");
- //WOLF_DEBUG("OTHER TIMES");
-
- // 1. First we track the known Features and create new factors as needed
-
- // Find landmarks in new_features_incoming_ (and extract from it)
- findLandmarks(getProblem()->getMap()->getLandmarkList(), known_features_incoming_, matches_landmark_from_incoming_);
- //WOLF_DEBUG("incoming new_features: " , new_features_incoming_.size());
- //WOLF_DEBUG("incoming known features: ", known_features_incoming_.size());
-
- // 2 CASES in which last should be moved to a (new or existing) KF:
- // - voteForKF() && permittedKF() --> CREATE KF
- // - There's a new KF within last time_tolerance --> EXISTING KF
- // 2 NECESSARY CONDITIONS:
- // - Last is not KF
- // - KF has not another capture of the same sensor
- PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( last_ptr_, params_->time_tolerance);
- bool KF_has_capture = KF_pack && (KF_pack->key_frame->getCaptureOf(getSensor()) != nullptr);
- bool create_KF = !KF_has_capture && !last_ptr_->getFrame()->isKey() && voteForKeyFrame() && permittedKeyFrame();
- bool existing_KF = !KF_has_capture && !last_ptr_->getFrame()->isKey() && KF_pack;
-
- FrameBasePtr closest_key_frm_to_last = getProblem()->getTrajectory()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
-
- //WOLF_DEBUG("last is KF? ", (last_is_KF ? "YES" : "NO"));
-
- // KEYFRAME (already created or decided to be created)
- if (create_KF || existing_KF )
- {
- // Append all new Features to the Capture's list of Features
- last_ptr_->addFeatureList(known_features_last_);
-
- // Create landmarks from new_features_last_, find them from new_features_incoming_
- processNew();
-
- // create KF
- if (create_KF)
- {
- // Make a non-key-frame to hold incoming
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(incoming_ptr_);
- //WOLF_DEBUG( "Incoming adhered to new F" , new_frame_ptr->id() );
-
- // Make the last Capture's Frame a KeyFrame
- setKeyFrame(last_ptr_);
-
- WOLF_DEBUG("PR ",getName() ," - Set KEY to last F", last_ptr_->getFrame()->id());
- }
- // existing KF within the time tolerance
- else
- {
- // Move incomming to the last F
- last_ptr_->getFrame()->addCapture(incoming_ptr_);
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
-
- // Move last to the existing KF
- last_ptr_->getFrame()->unlinkCapture(last_ptr_);
- KF_pack->key_frame->addCapture(last_ptr_);
-
- WOLF_DEBUG("PR ",getName() ," - Last capture ", last_ptr_->id(), " appended to existing KF ", last_ptr_->getFrame()->id());
- }
-
- // Establish factors for last matches
- establishFactors();
-
- // Reset the Tracker
- // Move matches and new features from incoming to last
- matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- known_features_last_ = std::move(known_features_incoming_);
-
- // Reset this
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
- }
- // NOT KEYFRAME -> Advance the tracker
- else
- {
- // Move matches and new features from incoming to last
- matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- known_features_last_ = std::move(known_features_incoming_);
- //WOLF_DEBUG("new_features_last_ " , new_features_last_.size());
-
- if (last_ptr_->getFrame()->isKey())
- {
- //WOLF_DEBUG("Last is in a KF \n");
- // Make a non-key-frame to hold incoming
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(incoming_ptr_);
- //WOLF_DEBUG( "Incoming adhered to new F" , new_frame_ptr->id() );
- }
- else
- {
- //WOLF_DEBUG("Last is NOT in a KF \n");
- // Add incoming Capture to the tracker's last Frame
- last_ptr_->getFrame()->addCapture(incoming_ptr_);
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
- // discard last capture
- last_ptr_->remove();
- }
-
- // Incoming Capture takes the place of last Capture
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
-
- //WOLF_DEBUG("last <-- incoming");
- }
- }
- //WOLF_DEBUG( "-----End of process():");
-
- // post-process
- // try to merge landmarks (stored in merge_candidates_list_)
- while (!merge_candidates_list_.empty())
- {
- // load next list of candidates
- LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front());
- merge_candidates_list_.pop_front();
-
- // change already merged lmks with the corresponding remaining lmk
- for (auto&& lmk : merge_candidates)
- if (lmk->getMergedInLandmark() != nullptr)
- lmk = lmk->getMergedInLandmark();
-
- // remove repeated lmks
- merge_candidates.sort();
- merge_candidates.unique();
-
- if (merge_candidates.size() == 1)
- continue;
-
- // Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
- LandmarkPolyline2D::tryMergeLandmarks(merge_candidates, params_->position_error_th_);
- }
- //WOLF_DEBUG( "-----End of post-process():");
-}
-
-void ProcessorPolyline::processNew()
-{
- // create landmarks from new_features_last
- createNewLandmarks();
-
- // Find the new landmarks in known_features_incoming_ (if it's not nullptr)
- if (incoming_ptr_ != nullptr && !new_landmarks_.empty())
- findLandmarks(new_landmarks_, known_features_incoming_, matches_landmark_from_incoming_);
-
- // Append all new Features to the Capture's list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
- // Append new landmarks to the map
- getProblem()->addLandmarkList(new_landmarks_);
-}
-
-bool ProcessorPolyline::voteForKeyFrame()
-{
- //std::cout << "------------- ProcessorPolyline::voteForKeyFrame:" << std::endl;
- // option 1: more than TH new features in last
- if (new_features_last_.size() >= params_->new_features_th)
- {
- WOLF_DEBUG("PR ",getName(),"------------- NEW KEY FRAME: Option 1 - Enough new features: ", new_features_last_.size(), " >= ", params_->new_features_th);
- return true;
- }
- // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
- for (auto new_ftr : last_ptr_->getFeatureList())
- {
- assert(matches_landmark_from_last_.find(new_ftr) != matches_landmark_from_last_.end());
- assert(matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().size() > 0);
- if (std::fabs(last_ptr_->getTimeStamp() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getTimeStamp()) > params_->loop_time_th)
- {
- WOLF_DEBUG("PR ",getName(),"------------- NEW KEY FRAME: Option 2 - Loop closure");
- return true;
- }
- }
- return false;
-}
-
-// IMPLEMENTATION /////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list)
-{
- //WOLF_DEBUG("ProcessorPolyline::extractPolylines: ");
- // TODO: sort corners by bearing
- std::list<laserscanutils::Polyline> polylines;
-
- line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
- //WOLF_DEBUG("Polylines extracted: ", polylines.size());
-
- for (auto&& pl : polylines)
- {
- //WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ );
- //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
- _polyline_list.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
- //WOLF_DEBUG("new polyline detected: ");
- }
-}
-
-unsigned int ProcessorPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
- FeatureBasePtrList& _features_found,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- //std::cout << "ProcessorPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << new_features_incoming_.size() << std::endl;
-
- //std::map<LandmarkPolyline2DPtr,LandmarkPolyline2DPtr> merged_lmks;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkPolyline2DPtr, Eigen::MatrixXs> expected_features;
- std::map<LandmarkPolyline2DPtr, Eigen::MatrixXs> expected_features_covs;
- for (auto lmk : _landmarks_searched)
- if (lmk->getType() == "POLYLINE 2D" && !lmk->isRemoving())
- {
- LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk);
- expected_features[pl_lmk] = Eigen::MatrixXs(3, pl_lmk->getNPoints());
- expected_features_covs[pl_lmk] = Eigen::MatrixXs(2, 2*pl_lmk->getNPoints());
- expectedFeature(pl_lmk, expected_features[pl_lmk], expected_features_covs[pl_lmk]);
- }
-
- // NAIVE NEAREST NEIGHBOR MATCHING
- LandmarkMatchPolyline2DPtr best_match = nullptr;
- FeaturePolyline2DPtr pl_ftr;
- LandmarkPolyline2DPtr pl_lmk;
- LandmarkPolyline2DPtrList merging_candidates;
- auto feature_it = new_features_incoming_.begin();
-
- // iterate over all polylines features
- while (feature_it != new_features_incoming_.end())
- {
- pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*feature_it);
-
- // Check with all landmarks
- for (auto landmark_it = _landmarks_searched.begin(); landmark_it != _landmarks_searched.end(); landmark_it++)
- {
- pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(*landmark_it);
-
- if (pl_lmk->isRemoving())
- continue;
-
- bool print = false;//(pl_lmk->id() == 13);
- if (print)
- std::cout << "\tcompute Best Sq Dist of landmark " << pl_lmk->id() << " (" << pl_lmk->getNPoints()
- << ") and feature " << pl_ftr->id() << "(" << pl_ftr->getNPoints() << ")\n";
- LandmarkMatchPolyline2DPtr best_correspondence = tryMatch(expected_features[pl_lmk],pl_lmk,pl_ftr);
-
- // best match
- if (best_correspondence != nullptr &&
- (best_match == nullptr ||
- (best_correspondence->feature_to_id_-best_correspondence->feature_from_id_ >= best_match->feature_to_id_-best_match->feature_from_id_ &&
- best_correspondence->normalized_score_ < best_match->normalized_score_ ) ) )
- {
- if (print)
- std::cout << "BEST MATCH" << std::endl;
- // More than one match -> landmark candidates to be merged
- if (best_match != nullptr)
- {
- if (merging_candidates.empty()) // add first best match
- merging_candidates.push_back(std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_));
- merging_candidates.push_back(pl_lmk); // add current best match
- }
- best_match = best_correspondence;
- }
- }
-
- // next feature
- feature_it++;
-
- // Match found for this feature
- if (best_match)
- {
- // make a copy of current iterator
- auto it_found = std::prev(feature_it);
-
- assert(best_match->feature_from_id_ == 0 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
- assert(best_match->feature_to_id_ == std::static_pointer_cast<FeaturePolyline2D>(*it_found)->getNPoints()-1 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
-
- bool print = false;//best_match->landmark_ptr_->id() == 13;
- if (print)
- {
- std::cout << "\tlandmark : " << best_match->landmark_ptr_->id() << std::endl;
- std::cout << "\tlandmark version: " << best_match->landmark_version_ << std::endl;
- std::cout << "\tfeature : " << (*it_found)->id() << std::endl;
- std::cout << "\tlandmark from id: " << best_match->landmark_from_id_ << std::endl;
- std::cout << "\tlandmark to id : " << best_match->landmark_to_id_ << std::endl;
- std::cout << "\tfeature from id : " << best_match->feature_from_id_ << std::endl;
- std::cout << "\tfeature to id : " << best_match->feature_to_id_ << std::endl;
- }
- // match
- _feature_landmark_correspondences[*it_found] = best_match;
- // move corner feature to output list
- _features_found.splice(_features_found.end(), new_features_incoming_, it_found);
- // reset match
- best_match = nullptr;
- }
-
- // Store landmarks that matched with the same feature as candidates to be merged
- if (!merging_candidates.empty())
- merge_candidates_list_.push_back(std::move(merging_candidates));
- }
-
- //std::cout << matches_landmark_from_incoming_.size() << " landmarks found from " << getProblem()->getMap()->getLandmarkList().size() << std::endl;
- //std::cout << "done" << std::endl;
- return matches_landmark_from_incoming_.size();
-}
-
-void ProcessorPolyline::createNewLandmarks()
-{
- //std::cout << "ProcessorPolyline::createNewLandmarks - new_features_last_" << new_features_last_.size() << std::endl;
- FeaturePolyline2DPtr polyline_ft_ptr;
- LandmarkBasePtr new_lmk_ptr;
- for (auto new_feature_ptr : new_features_last_)
- {
- // create new landmark
- new_lmk_ptr = createLandmark(new_feature_ptr);
- //WOLF_DEBUG("PR ",getName()," - New Landmark ", new_lmk_ptr->id());
- //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl;
- //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< std::static_pointer_cast<LandmarkPolyline2D>(new_lmk_ptr)->getNPoints() << " points" << std::endl;
-
- // cast
- polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr);
- // create new correspondence
- LandmarkMatchPolyline2DPtr match = std::make_shared<LandmarkMatchPolyline2D>();
- match->feature_from_id_= 0; // all points match
- match->landmark_from_id_ = 0;
- match->feature_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match
- match->landmark_to_id_ = polyline_ft_ptr->getNPoints()-1;
- match->normalized_score_ = 1.0; // max score
- match->landmark_ptr_ = new_lmk_ptr;
- match->landmark_version_ = 0;
-
- // store match
- matches_landmark_from_last_[new_feature_ptr] = match;
-
- // store new landmark
- new_landmarks_.push_back(new_lmk_ptr);
-
- // std::cout << "new match: feature " << new_feature_ptr->id() << " with landmark " << new_lmk_ptr->id() << std::endl;
- // std::cout << "\tfeature_from_id_ = " << match->feature_from_id_ << std::endl;
- // std::cout << "\tlandmark_from_id_ = " << match->landmark_from_id_ << std::endl;
- // std::cout << "\tfeature_to_id_ = " << match->feature_to_id_ << std::endl;
- // std::cout << "\tlandmark_to_id_ = " << match->landmark_to_id_ << std::endl;
- // std::cout << "\tnormalized_score_ = " << match->normalized_score_ << std::endl;
- }
- //std::cout << "done" << std::endl;
-}
-
-LandmarkBasePtr ProcessorPolyline::createLandmark(FeatureBasePtr _feature_ptr)
-{
- //std::cout << "ProcessorPolyline::createLandmark" << std::endl;
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
- assert(_feature_ptr->getType() == "POLYLINE 2D");
-
-
- FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
- // compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
- // std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- // std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
- // Create new landmark
- return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
- //std::cout << "done" << std::endl;
-}
-
-void ProcessorPolyline::establishFactors()
-{
- //TODO: update with new index in landmarks
- //std::cout << "ProcessorPolyline::establishFactors" << std::endl;
- //std::cout << "\tlast_ id: " << last_ptr_->id() << " linked to frame: " << last_ptr_->getFrame()->id() << " is linked to problem? " << (last_ptr_->getProblem() ? "YES" : "NO") << std::endl;
- unsigned int N_factors = 0;
- LandmarkMatchPolyline2DPtr pl_match;
- FeaturePolyline2DPtr pl_ftr;
- LandmarkPolyline2DPtr pl_lmk;
- LandmarkPolyline2DPtrList modified_lmks;
-
- for (auto last_ftr : last_ptr_->getFeatureList())
- {
- pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(last_ftr);
- pl_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(matches_landmark_from_last_[last_ftr]);
- pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(pl_match->landmark_ptr_);
-
- assert(pl_match->feature_from_id_ >= 0 && pl_match->feature_to_id_ >= 0);
- assert(pl_lmk != nullptr && pl_match != nullptr);
-
- if (pl_lmk->isRemoving())
- continue;
-
- bool print = false; // pl_lmk->id() == 13
-
- // CHECK IF LANDMARK CHANGED AFTER MATCHING
- if (pl_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
- {
- // get the lmk in which lmk was merged
- auto merged_in_lmk = pl_lmk->getMergedInLandmark();
- if (merged_in_lmk)
- pl_lmk = merged_in_lmk;
-
- // redo matching
- LandmarkMatchPolyline2DPtr updated_match = tryMatch(pl_lmk, pl_ftr);
-
- // if still matching, overwrite match
- if (updated_match)
- pl_match = updated_match;
-
- // if not matching, erase match and jump to next match
- else
- {
- matches_landmark_from_last_.erase(last_ftr);
- continue;
- }
-
- assert(pl_match->feature_from_id_ >= 0 && pl_match->feature_to_id_ >= 0);
- }
-
- // MODIFY LANDMARK (only for not closed and not recently created)
- if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
- {
- if (print)
- {
- std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl;
- std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl;
- std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl;
- std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl;
- }
- Eigen::MatrixXs points_global = R_world_sensor_ * pl_ftr->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
-
- // MODIFY LANDMARK
- // -----------------GROW Front-----------------
- // Add new front points (if any feature point not matched)
- if ( pl_match->feature_from_id_ > 0 ) // && !pl_lmk->isClosed()
- {
- assert(!pl_lmk->isClosed() && "feature has not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add new front points. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- pl_match->feature_from_id_-1, // last feature point index to be added
- pl_ftr->isFirstDefined(), // is defined
- false); // front
-
- modified_lmks.push_back(pl_lmk);
-
- pl_match->landmark_from_id_ = pl_lmk->getFirstId();
- pl_match->feature_from_id_ = 0;
- //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- // -----------------CHANGE Front-----------------
- // Change first not defined point if landmark first not defined matched with first feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (pl_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
- {
- if (print)
- {
- std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isFirstDefined() ||
- // case b
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))).squaredNorm() >
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm())
- {
- pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
- modified_lmks.push_back(pl_lmk);
- }
- }
- assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- // -----------------GROW Back-----------------
- // Add new back points (if any feature point not matched)
- if (pl_match->feature_to_id_ < pl_ftr->getNPoints()-1)
- {
- assert(!pl_lmk->isClosed() && "feature not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add back points. Defined: " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- pl_match->feature_to_id_+1, // last feature point index to be added
- pl_ftr->isLastDefined(), // is defined
- true); // back
-
- modified_lmks.push_back(pl_lmk);
-
- pl_match->landmark_to_id_ = pl_lmk->getLastId();
- pl_match->feature_to_id_ = pl_ftr->getNPoints()-1;
- //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- // -----------------CHANGE Back-----------------
- // Change last not defined point if landmark last not defined matched with last feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (pl_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
- {
- if (print)
- {
- std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isLastDefined() ||
- // case b
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))).squaredNorm() >
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm())
- {
- pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined());
- modified_lmks.push_back(pl_lmk);
- }
- }
- assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
-
- if (print)
- {
- std::cout << "MODIFIED LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- }
- }
- else
- {
- // add recently created landmarks to the list of modified landmark
- if (pl_lmk->getHits() == 0)
- modified_lmks.push_back(pl_lmk);
-
- assert((pl_lmk->getHits() != 0 || pl_match->feature_from_id_ == 0) && "recently created landmark with not full feature match");
- assert((pl_lmk->getHits() != 0 || pl_match->feature_to_id_ == pl_ftr->getNPoints()-1) && "recently created landmark with not full feature match");
- assert((!pl_lmk->isClosed() || pl_match->feature_from_id_ == 0) && "closed landmark with not full feature match");
- assert((!pl_lmk->isClosed() || pl_match->feature_to_id_ == pl_ftr->getNPoints()-1) && "closed landmark with not full feature match");
- }
-
- // ESTABLISH CONSTRAINTS ------------------------------------------------------------------------
- assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + pl_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly");
- //std::cout << "Establishing factor between" << std::endl;
- //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is liked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
- //std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
- //std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl;
-
- // Factors for all feature points
- int lmk_point_id = pl_match->landmark_from_id_;
-
- for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
- {
- //std::cout << "feature point " << ftr_point_id << std::endl;
- //std::cout << "landmark point " << lmk_point_id << std::endl;
-
- // First not defined point
- if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
- // first point to line factor
- {
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
- assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
- N_factors++;
- //std::cout << "factor added" << std::endl;
- }
-
- // Last not defined point
- else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
- // last point to line factor
- {
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
- assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
- N_factors++;
- //std::cout << "factor added" << std::endl;
- }
-
- // Defined point
- else
- // point to point factor
- {
- //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
- //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
- //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
- emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
- //std::cout << "factor added" << std::endl;
- }
-
- if (ftr_point_id < pl_ftr->getNPoints()-1)
- lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
- }
- }
- //std::cout << N_factors << " factors established, trying to close and classify " << modified_lmks.size() << std::endl;
-
- // Try to close & classify modified landmarks
- for (auto lmk_ptr : modified_lmks)
- {
- assert(!lmk_ptr->isRemoving());
-
- // try classify
- if (lmk_ptr->getClassification().type == UNCLASSIFIED && lmk_ptr->getNDefinedPoints() >= 3 && lmk_ptr->getNDefinedPoints() <= 4)
- lmk_ptr->tryClassify(params_->class_position_error_th, params_->polyline_classes);
- // try close
- if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 3 || lmk_ptr->getNPoints() >= 4))
- lmk_ptr->tryClose(params_->class_position_error_th);
- }
-
- // Outlier rejection
- FactorBasePtrList new_fac_list;
- last_ptr_->getFactorList(new_fac_list);
- for (auto fac : new_fac_list)
- rejectOutlier(fac);
-
- //std::cout << "ProcessorPolyline::establishFactors: done\n";
-}
-
-void ProcessorPolyline::classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list)
-{
- //std::cout << "ProcessorPolyline::classifyPolylineLandmarks: " << _lmk_list.size() << std::endl;
-
- LandmarkPolyline2DPtr lmk_polyline_ptr;
- for (auto lmk_ptr : _lmk_list)
- {
- if (lmk_ptr->isRemoving())
- continue;
-
- //std::cout << "landmark " << lmk_ptr->id() << " of type " << lmk_ptr->getType() << std::endl;
- if ( (lmk_polyline_ptr = std::dynamic_pointer_cast<LandmarkPolyline2D>(lmk_ptr)) &&
- lmk_polyline_ptr->getClassification().type == UNCLASSIFIED &&
- lmk_polyline_ptr->getNDefinedPoints() > 2 && lmk_polyline_ptr->getNDefinedPoints() < 5)
- lmk_polyline_ptr->tryClassify(params_->class_position_error_th, params_->polyline_classes);
- }
- //std::cout << "ProcessorPolyline::classifyPolylineLandmarks: done\n";
-}
-
-
-
-LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr)
-{
- // updated expected feature
- Eigen::MatrixXs exp_feat, exp_cov;
- expectedFeature(_lmk_ptr, exp_feat, exp_cov);
-
- // try match
- return tryMatch(exp_feat,_lmk_ptr, _feat_ptr);
-}
-
-LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
-{
- LandmarkMatchPolyline2DPtr lmk_match = nullptr;
-
- // compute best sq distance matching
- MatchPolyline2DPtr match = computeBestSqDist(_lmk_expected_feature_points, // <--landmark points in local coordinates
- _lmk_ptr->isFirstDefined(),
- _lmk_ptr->isLastDefined(),
- _lmk_ptr->isClosed(),
- _feat_ptr->getPoints(), // <--feature points
- _feat_ptr->isFirstDefined(),
- _feat_ptr->isLastDefined(),
- false, // <--feature is not closed for sure
- params_->position_error_th_*params_->position_error_th_);
- // valid match
- if (match)
- {
- lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
- lmk_match->landmark_ptr_=_lmk_ptr;
- lmk_match->landmark_version_=_lmk_ptr->getVersion();
- // landmark point id's
- std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
- lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_];
- lmk_match->landmark_to_id_ = lmk_valid_ids[match->to_A_id_];
- // feature point id's
- lmk_match->feature_from_id_ = match->from_B_id_;
- lmk_match->feature_to_id_ = match->to_B_id_;
-
- assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
- assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
- }
-
- return lmk_match;
-}
-
-bool ProcessorPolyline::rejectOutlier(FactorBasePtr& fac_ptr)
-{
- // Cast Point2Line/Point2Point factor
- auto fac_p2l_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
- auto fac_p2p_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
-
- // Cast feature and landmark
- auto pl_ftr_ptr = std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature());
- auto pl_lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(fac_ptr->getLandmarkOther());
-
- // copy states
- Eigen::VectorXs frameP(pl_ftr_ptr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXs frameO(pl_ftr_ptr->getCapture()->getFrame()->getO()->getState());
- Eigen::VectorXs lmkOriginP(pl_lmk_ptr->getP()->getState());
- Eigen::VectorXs lmkOriginO(pl_lmk_ptr->getO()->getState());
- Eigen::VectorXs lmkPoint(pl_lmk_ptr->getPointVector((fac_p2l_ptr ? fac_p2l_ptr->getLandmarkPointId() : fac_p2p_ptr->getLandmarkPointId())));
- Eigen::VectorXs lmkPointAux((fac_p2l_ptr ? pl_lmk_ptr->getPointVector(fac_p2l_ptr->getLandmarkPointAuxId()) : Eigen::VectorXs::Zero(1)));
-
- // create Scalar* array of a copy of the state
- Scalar* parameters[6] = {frameP.data(),
- frameO.data(),
- lmkOriginP.data(),
- lmkOriginO.data(),
- lmkPoint.data(),
- lmkPointAux.data()}; // last wont be used in p2p evaluate()
-
- // create residuals pointer
- Eigen::VectorXs residuals(fac_p2l_ptr ? 1 : 2);
-
- // evaluate the factor in this state (without jacobians, no need)
- fac_ptr->evaluate(parameters, residuals.data(), nullptr);
-
- // discard if residual too high evaluated at the current estimation
- if (residuals.norm() > 1e3)
- {
- WOLF_WARN((fac_p2l_ptr ? "Discarding Point 2 Line Polyline Factor, considered OUTLIER" : "Discarding Point 2 Point Polyline Factor, considered OUTLIER"));
- fac_ptr->remove();
- return true;
- }
- return false;
-}
-
-// GETS & SETS ////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorPolyline::setKeyFrame(CaptureBasePtr _capture_ptr)
-{
-
- assert(_capture_ptr != nullptr && _capture_ptr->getFrame() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame");
- if (!_capture_ptr->getFrame()->isKey())
- {
- // Set key
- _capture_ptr->getFrame()->setKey();
- // Set state to the keyframe
- _capture_ptr->getFrame()->setState(getProblem()->getState(_capture_ptr->getTimeStamp()));
- // Call the new keyframe callback in order to let the other processors to establish their factors
- getProblem()->keyFrameCallback(_capture_ptr->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_->time_tolerance);
- }
-}
-
-// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorPolyline::computeTransformations(const TimeStamp& _ts)
-{
- Eigen::Vector3s vehicle_pose = getProblem()->getState(_ts);
- t_world_robot_ = vehicle_pose.head<2>();
-
- // world_robot
- Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->isExtrinsicDynamic() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_ = R_world_robot * R_robot_sensor_;
- t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_ = R_robot_sensor_.transpose() * R_world_robot.transpose();
- t_sensor_world_ = -R_robot_sensor_.transpose() * t_robot_sensor_ -R_sensor_world_ * t_world_robot_ ;
-
- //std::cout << "t_world_robot_ " << t_world_robot_.transpose() << std::endl;
- //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
- //std::cout << "t_world_sensor_ " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_world_sensor_ " << std::endl << R_world_sensor_ << std::endl;
- //std::cout << "t_sensor_world_ " << t_sensor_world_.transpose() << std::endl;
- //std::cout << "R_sensor_world_ " << std::endl << R_sensor_world_ << std::endl;
-
-}
-
-void ProcessorPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_ftr_, Eigen::MatrixXs& expected_ftr_cov_)
-{
- assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorPolyline::expectedFeature: Bad landmark type");
- LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
-
- //std::cout << "ProcessorPolyline::expectedFeature" << std::endl;
- //std::cout << "t_world_sensor_: " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_sensor_world_: " << std::endl << R_sensor_world_ << std::endl;
-
- expected_ftr_ = pl_lmk->computePointsGlobal();
- expected_ftr_.topRows(2) = R_sensor_world_ * (expected_ftr_.topRows(2).colwise() - t_world_sensor_);
- expected_ftr_cov_ = pl_lmk->computePointsCovGlobal();
- // TODO Rotate covariances
-}
-
-ProcessorBasePtr ProcessorPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
- ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params);
- ProcessorPolylinePtr prc_ptr = std::make_shared<ProcessorPolyline>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} /* namespace wolf */
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorPolyline)
-} /* namespace wolf */
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
deleted file mode 100644
index 9c76de0b084dc74d5528695fffa95e6106b917c2..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker.cpp
+++ /dev/null
@@ -1,284 +0,0 @@
-/*
- * ProcessorTracker.cpp
- *
- * Created on: Apr 7, 2016
- * Author: jvallve
- */
-
-// wolf
-#include "base/processor/processor_tracker.h"
-
-// std
-#include <cmath>
-
-namespace wolf
-{
-
-ProcessorTracker::ProcessorTracker(const std::string& _type,
- ProcessorParamsTrackerPtr _params_tracker) :
- ProcessorBase(_type, _params_tracker),
- params_tracker_(_params_tracker),
- processing_step_(FIRST_TIME_WITHOUT_PACK),
- origin_ptr_(nullptr),
- last_ptr_(nullptr),
- incoming_ptr_(nullptr),
- number_of_tracks_(0)
-{
- //
-}
-
-ProcessorTracker::~ProcessorTracker()
-{
- //
-}
-
-void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
-{
- using std::abs;
-
- if (_incoming_ptr == nullptr)
- {
- WOLF_ERROR("Received capture is nullptr.");
- return;
- }
-
- incoming_ptr_ = _incoming_ptr;
-
- preProcess(); // Derived class operations
-
- computeProcessingStep();
-
- switch(processing_step_)
- {
- case FIRST_TIME_WITH_PACK :
- {
- WOLF_DEBUG("PT: FIRST_TIME_WITH_PACK");
- PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
-
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
-
- // Append incoming to KF
- pack->key_frame->addCapture(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 given a map, all landmarks in the map are know. Process them.
- processKnown();
-
- // Update pointers
- resetDerived();
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
-
- break;
- }
- case FIRST_TIME_WITHOUT_PACK :
- {
- WOLF_DEBUG("PT: FIRST_TIME_WITHOUT_PACK");
- FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp());
- kfrm->addCapture(incoming_ptr_);
-
- // Process info
- processKnown();
- // We only process new features in Last, here last = nullptr, so we do not have anything to do.
-
- // Issue KF callback with new KF
- getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
-
- // Update pointers
- resetDerived();
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
-
- break;
- }
- case SECOND_TIME_WITH_PACK :
- {
- WOLF_DEBUG("PT: SECOND_TIME_WITH_PACK");
- // No-break case only for debug. Next case will be executed too.
- PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
- } // @suppress("No break at end of case")
- case SECOND_TIME_WITHOUT_PACK :
- {
- WOLF_DEBUG("PT: SECOND_TIME_WITHOUT_PACK");
-
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- frm->addCapture(incoming_ptr_);
-
- // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
-
- // Process info
- processKnown();
- processNew(params_tracker_->max_new_features);
-
- // Establish factors
- establishFactors();
-
- // Update pointers
- resetDerived();
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
-
- break;
- }
- case RUNNING_WITH_PACK :
- {
- WOLF_DEBUG("PT: RUNNING_WITH_PACK");
-
- PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
-
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
-
- processKnown();
-
- // Capture last_ is added to the new keyframe
- FrameBasePtr last_old_frame = last_ptr_->getFrame();
- last_old_frame->unlinkCapture(last_ptr_);
- last_old_frame->remove();
- pack->key_frame->addCapture(last_ptr_);
-
- // Create new frame
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- frm->addCapture(incoming_ptr_);
-
- // Detect new Features, initialize Landmarks, create Factors, ...
- processNew(params_tracker_->max_new_features);
-
- // Establish factors
- establishFactors();
-
- // Update pointers
- resetDerived();
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
-
- break;
- }
- case RUNNING_WITHOUT_PACK :
- {
- WOLF_DEBUG("PT: RUNNING_WITHOUT_PACK");
- processKnown();
-
- if (voteForKeyFrame() && permittedKeyFrame())
- {
- WOLF_DEBUG("PT: if (voteForKeyFrame() && permittedKeyFrame())");
- // We create a KF
-
- // set KF on last
- last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->setKey();
-
- // make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- frm->addCapture(incoming_ptr_);
-
- // process
- processNew(params_tracker_->max_new_features);
-
- // // Set key
- // last_ptr_->getFrame()->setKey();
- //
- // Set state to the keyframe
- last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-
- // Establish factors
- establishFactors();
-
- // Call the new keyframe callback in order to let the other processors to establish their factors
- getProblem()->keyFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
-
- // Update pointers
- resetDerived();
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
-
- }
- else
- {
- WOLF_DEBUG("PT: else (voteForKeyFrame() && permittedKeyFrame())");
- // We do not create a KF
-
- // Advance this
- last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
- last_ptr_->remove();
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
-
- // Update pointers
- advanceDerived();
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
- }
- break;
- }
- default :
- break;
- }
-
- number_of_tracks_ = last_ptr_->getFeatureList().size();
- postProcess();
-}
-
-void ProcessorTracker::computeProcessingStep()
-{
- // First determine the processing phase by checking the status of the tracker pointers
- enum {FIRST_TIME, SECOND_TIME, RUNNING} step;
- if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
- step = FIRST_TIME;
- else if (origin_ptr_ == last_ptr_)
- step = SECOND_TIME;
- else
- step = RUNNING;
-
- // Then combine with the existence (or not) of a keyframe callback pack
- switch (step)
- {
- case FIRST_TIME :
-
- if (kf_pack_buffer_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
- processing_step_ = FIRST_TIME_WITH_PACK;
- else // ! last && ! pack(incoming)
- processing_step_ = FIRST_TIME_WITHOUT_PACK;
- break;
-
- case SECOND_TIME :
-
- if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
- processing_step_ = SECOND_TIME_WITH_PACK;
- else
- processing_step_ = SECOND_TIME_WITHOUT_PACK;
- break;
-
- case RUNNING :
- default :
-
- if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
- {
- if (last_ptr_->getFrame()->isKey())
- {
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
- WOLF_INFO("Check the following for correctness:");
- WOLF_INFO(" - You have all processors installed before starting receiving any data");
- WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed ---> ", (getProblem()->priorIsSet() ? "OK" : "NOK"));
- WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
- WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances).");
- }
- processing_step_ = RUNNING_WITH_PACK;
- }
- else
- processing_step_ = RUNNING_WITHOUT_PACK;
- break;
- }
-}
-
-} // namespace wolf
-
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
deleted file mode 100644
index 9d7b334af4b2d7a014e7b8a072cba108811a7469..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature.cpp
+++ /dev/null
@@ -1,168 +0,0 @@
-/*
- * \processor_tracker_feature.cpp
- *
- * Created on: 27/02/2016
- * \author: jsola
- */
-
-#include "base/processor/processor_tracker_feature.h"
-
-namespace wolf
-{
-
-ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
- ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTracker(_type, _params_tracker_feature),
- params_tracker_feature_(_params_tracker_feature)
-{
-}
-
-ProcessorTrackerFeature::~ProcessorTrackerFeature()
-{
-}
-
-unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
-{
- /* Rationale: A keyFrame will be created using the last Capture.
- * First, we work on the last Capture to detect new Features,
- * When done, we need to track these new Features to the incoming Capture.
- * At the end, all new Features are appended to the lists of known Features in
- * the last and incoming Captures.
- */
-
- // Populate the last Capture with new Features. The result is in new_features_last_.
- unsigned int n = detectNewFeatures(_max_new_features, new_features_last_);
- for (auto ftr : new_features_last_)
- track_matrix_.newTrack(last_ptr_, ftr);
-
- // Track new features from last to incoming. This will append new correspondences to matches_last_incoming
- trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
- for (auto ftr : new_features_incoming_)
- {
- ftr->setProblem(this->getProblem());
- SizeStd trk_id_from_last = matches_last_from_incoming_[ftr]->feature_ptr_->trackId();
- track_matrix_.add(trk_id_from_last, incoming_ptr_, ftr);
- }
-
- // Append all new Features to the incoming Captures' list of Features
- WOLF_DEBUG("PTF ", getName(), ": ", incoming_ptr_->getFeatureList().size(), " in incoming_ptr_)");
- WOLF_DEBUG("PTF ", getName(), ": ", new_features_incoming_.size(), " in new_features_incoming_)");
- incoming_ptr_->addFeatureList(new_features_incoming_);
- WOLF_DEBUG("PTF ", getName(), ": ", incoming_ptr_->getFeatureList().size(), " in incoming_ptr_)");
- WOLF_DEBUG("PTF ", getName(), ": ", new_features_incoming_.size(), " in new_features_incoming_)");
-
- // Append all new Features to the last Captures' list of Features
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
- WOLF_DEBUG("PTF ", getName(), ": ", new_features_last_.size(), " in new_features_last_)");
- last_ptr_->addFeatureList(new_features_last_);
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
- WOLF_DEBUG("PTF ", getName(), ": ", new_features_last_.size(), " in new_features_last_)");
-
- // return the number of new features detected in \b last
- return n;
-}
-
-unsigned int ProcessorTrackerFeature::processKnown()
-{
- assert(incoming_ptr_->getFeatureList().size() == 0
- && "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
- assert(matches_last_from_incoming_.size() == 0
- && "In ProcessorTrackerFeature::processKnown(): match list from last to incoming must be empty before processKnown()");
-
- if (!last_ptr_ || last_ptr_->getFeatureList().empty())
- {
- WOLF_DEBUG("PTF ", getName(), "::processKnown: null or empty last capture, returning");
- return 0;
- }
-
- // Track features from last_ptr_ to incoming_ptr_
- trackFeatures(last_ptr_->getFeatureList(), known_features_incoming_, matches_last_from_incoming_);
- for (auto match : matches_last_from_incoming_)
- {
- match.first->setProblem(this->getProblem());
- SizeStd trk_id_from_last = match.second->feature_ptr_->trackId();
- track_matrix_.add(trk_id_from_last, incoming_ptr_, match.first);
- }
-
- // Check/correct incoming-origin correspondences
- if (origin_ptr_ != nullptr)
- {
- for (auto feature_in_incoming : known_features_incoming_)
- {
- SizeStd track_id = feature_in_incoming->trackId();
- FeatureBasePtr feature_in_last = track_matrix_.feature(track_id, last_ptr_);
- FeatureBasePtr feature_in_origin = track_matrix_.feature(track_id, origin_ptr_);
-
- if (correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming) == false)
- {
- // Remove this feature from many places:
- matches_last_from_incoming_ .erase (feature_in_incoming); // remove match
- track_matrix_ .remove(feature_in_incoming); // remove from track matrix
- known_features_incoming_ .remove(feature_in_incoming); // remove from known features list
- feature_in_incoming ->remove(); // remove from wolf tree
- }
- }
- }
-
- // Add to wolf tree and clear
- incoming_ptr_->addFeatureList(known_features_incoming_);
- known_features_incoming_.clear();
-
- // Print resulting list of matches
-// for (auto match : matches_last_from_incoming_)
-// WOLF_DEBUG("Known track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
-
- return matches_last_from_incoming_.size();
-}
-
-void ProcessorTrackerFeature::advanceDerived()
-{
- // Reset here the list of correspondences.
- matches_last_from_incoming_.clear();
-
- // We set problem here because we could not determine Problem from incoming capture at the time of adding the features to incoming's feature list.
- for (auto ftr : incoming_ptr_->getFeatureList())
- ftr->setProblem(getProblem());
-
-// // remove last from track matrix in case you want to have only KF in the track matrix
-// track_matrix_.remove(last_ptr_);
-}
-
-void ProcessorTrackerFeature::resetDerived()
-{
- // Reset here the list of correspondences.
- matches_last_from_incoming_.clear();
-
- // Update features according to the move above.
- TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
-
- for (auto const & pair_trkid_pair : matches_origin_last)
- {
- FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
- FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- feature_in_last->setProblem(getProblem()); // Since these features were in incoming_, they had no Problem assigned.
-
- //WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", feature_in_origin->id(), " last: ", feature_in_last->id());
- }
-}
-
-void ProcessorTrackerFeature::establishFactors()
-{
- TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
-
- for (auto const & pair_trkid_pair : matches_origin_last)
- {
- FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
- FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
-
- auto fac_ptr = createFactor(feature_in_last, feature_in_origin);
- feature_in_last ->addFactor(fac_ptr);
- feature_in_origin->addConstrainedBy(fac_ptr);
-
- WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
- }
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
deleted file mode 100644
index 9a90676b67408ba352704ecb89ce0e06fc0b7838..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ /dev/null
@@ -1,204 +0,0 @@
-/**
- * \file processor_tracker_feature_corner.cpp
- *
- * Created on: Apr 11, 2016
- * \author: jvallve
- */
-
-#include "base/processor/processor_tracker_feature_corner.h"
-#include "base/feature/feature_corner_2D.h"
-
-namespace wolf
-{
-
-ProcessorTrackerFeatureCorner::ProcessorTrackerFeatureCorner(ProcessorParamsTrackerFeatureCornerPtr _params_tracker_feature_corner) :
- ProcessorTrackerFeature("TRACKER FEATURE CORNER", _params_tracker_feature_corner),
- params_tracker_feature_corner_(_params_tracker_feature_corner),
- R_world_sensor_(Eigen::Matrix3s::Identity()),
- R_robot_sensor_(Eigen::Matrix3s::Identity()),
- extrinsics_transformation_computed_(false)
-{
- //
-}
-
-ProcessorTrackerFeatureCorner::~ProcessorTrackerFeatureCorner()
-{
- for (auto corner : corners_last_)
- corner->remove();
- for (auto corner : corners_incoming_)
- corner->remove();
-}
-
-void ProcessorTrackerFeatureCorner::preProcess()
-{
- // extract corners of incoming
- extractCorners(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), corners_incoming_);
-
- // store previous transformations
- R_world_sensor_prev_ = R_world_sensor_;
- t_world_sensor_prev_ = t_world_sensor_;
-
- // compute transformations
- t_world_robot_ = getProblem()->getState(incoming_ptr_->getTimeStamp());
-
- // world_robot
- Eigen::Matrix3s R_world_robot = Eigen::Matrix3s::Identity();
- R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
- t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
- R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_.topLeftCorner<2, 2>() = R_world_robot.topLeftCorner<2, 2>() * R_robot_sensor_.topLeftCorner<2, 2>();
- t_world_sensor_ = t_world_robot_ + R_robot_sensor_ * t_robot_sensor_;
-
- // current_prev
- R_current_prev_ = R_world_sensor_.transpose() * R_world_sensor_prev_;
- t_current_prev_ = R_world_sensor_.transpose() * (t_world_sensor_prev_ - t_world_sensor_);
-}
-
-void ProcessorTrackerFeatureCorner::advanceDerived()
-{
- ProcessorTrackerFeature::advanceDerived();
- corners_last_ = std::move(corners_incoming_);
-}
-
-unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
-{
- std::cout << "tracking " << _features_last_in.size() << " features..." << std::endl;
-
- Eigen::Vector3s expected_feature_pose;
- for (auto feat_in_ptr : _features_last_in)
- {
- expected_feature_pose = R_current_prev_ * feat_in_ptr->getMeasurement().head<3>() + t_current_prev_;
-
- auto feat_out_next = corners_incoming_.begin();
- auto feat_out_it = feat_out_next++; // next is used to obtain the next iterator after splice
- while (feat_out_it != corners_incoming_.end()) //runs over extracted feature
- {
- if (((*feat_out_it)->getMeasurement().head<3>() - expected_feature_pose).squaredNorm() > params_tracker_feature_corner_->position_error_th*params_tracker_feature_corner_->position_error_th)
- {
- // match
- _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
-
- // move matched feature to list
- _features_incoming_out.splice(_features_incoming_out.end(), corners_incoming_, feat_out_it);
-
- std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl;
- }
- feat_out_it = feat_out_next++;
- }
- }
-
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
-{
- return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
-}
-
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
-{
- // in corners_last_ remain all not tracked corners
- _features_last_out = std::move(corners_last_);
- if (_max_features != -1 && _features_last_out.size() > _max_features)
- _features_last_out.resize(_max_features);
-
- return _features_last_out.size();
-}
-
-FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
-{
- // Getting landmark ptr
- LandmarkCorner2DPtr landmark_ptr = nullptr;
- for (auto factor : _feature_other_ptr->getFactorList())
- if (factor->getLandmarkOther() != nullptr && factor->getLandmarkOther()->getType() == "CORNER 2D")
- landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(factor->getLandmarkOther());
-
- if (landmark_ptr == nullptr)
- {
- // Create new landmark
- Eigen::Vector3s feature_global_pose = R_world_sensor_ * _feature_ptr->getMeasurement() + t_world_sensor_;
- landmark_ptr = std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(feature_global_pose.head(2)),
- std::make_shared<StateBlock>(feature_global_pose.tail(1)),
- _feature_ptr->getMeasurement()(3));
-
- // Add landmark factor to the other feature
- _feature_other_ptr->addFactor(std::make_shared<FactorCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this()));
- }
-
-// std::cout << "creating factor: last feature " << _feature_ptr->getMeasurement()
-// << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl
-// << " corresponding to landmark " << landmark_ptr->id() << std::endl;
- return std::make_shared<FactorCorner2D>(_feature_ptr, landmark_ptr, shared_from_this());
-}
-
-void ProcessorTrackerFeatureCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBasePtrList& _corner_list)
-{
- // TODO: sort corners by bearing
- std::list<laserscanutils::CornerPoint> corners;
-
- std::cout << "Extracting corners..." << std::endl;
- corner_finder_.findCorners(_capture_laser_ptr->getScan(),
- (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(),
- line_finder_,
- corners);
-
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head<2>() = corner.point_.head<2>();
- measurement(2)=corner.orientation_;
- measurement(3)=corner.aperture_;
-
- _corner_list.push_back(std::make_shared<FeatureCorner2D>(measurement, corner.covariance_));
- }
-
-/* //variables
- std::list<laserscanutils::Corner> corners;
- //extract corners from range data
- laserscanutils::extractCorners(scan_params_, corner_alg_params_, _capture_laser_ptr->getRanges(), corners);
- //std::cout << corners.size() << " corners extracted" << std::endl;
- Eigen::Matrix4s measurement_cov;
- Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head(2) = corner.pt_.head(2);
- measurement(2) = corner.orientation_;
- measurement(3) = corner.aperture_;
- Scalar L1 = corner.line_1_.length();
- Scalar L2 = corner.line_2_.length();
- Scalar cov_angle_line1 = 12 * corner.line_1_.error_
- / (pow(L1, 2) * (pow(corner.line_1_.np_, 3) - pow(corner.line_1_.np_, 2)));
- Scalar cov_angle_line2 = 12 * corner.line_2_.error_
- / (pow(L2, 2) * (pow(corner.line_2_.np_, 3) - pow(corner.line_2_.np_, 2)));
- //init cov in corner coordinates
- measurement_cov << corner.line_1_.error_ + cov_angle_line1 * L1 * L1 / 4, 0, 0, 0, 0, corner.line_2_.error_
- + cov_angle_line2 * L2 * L2 / 4, 0, 0, 0, 0, cov_angle_line1 + cov_angle_line2, 0, 0, 0, 0, cov_angle_line1
- + cov_angle_line2;
- measurement_cov = 10 * measurement_cov;
- //std::cout << "New feature: " << meas.transpose() << std::endl;
- //std::cout << measurement_cov << std::endl;
- // Rotate covariance
- R.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(corner.orientation_).matrix();
- measurement_cov.topLeftCorner<3, 3>() = R.transpose() * measurement_cov.topLeftCorner<3, 3>() * R;
- //std::cout << "rotated covariance: " << std::endl;
- //std::cout << measurement_cov << std::endl;
- _corner_list.push_back(new FeatureCorner2D(measurement, measurement_cov));
- }*/
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
deleted file mode 100644
index aea025a2daea5962cf98ed8e0afea2330a8759f8..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-/**
- * \file processor_tracker_feature_dummy.cpp
- *
- * Created on: Apr 11, 2016
- * \author: jvallve
- */
-
-#include "base/processor/processor_tracker_feature_dummy.h"
-#include "base/feature/feature_base.h"
-
-namespace wolf
-{
-
-unsigned int ProcessorTrackerFeatureDummy::count_ = 0;
-
-unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
-{
- WOLF_INFO("tracking " , _features_last_in.size() , " features...");
-
- for (auto feat_in : _features_last_in)
- {
- if (++count_ % 3 == 2) // lose one every 3 tracks
- {
- WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost");
- }
- else
- {
- FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in->getMeasurement(), feat_in->getMeasurementCovariance()));
- _features_incoming_out.push_back(ftr);
- _feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,1.0}));
-
- WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id());
- }
- }
-
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
-{
- WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() );
-
- bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
-
- WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
-
- return vote;
-}
-
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
-{
- unsigned int max_features = _max_features;
-
- if (max_features == -1)
- {
- max_features = 10;
- WOLF_INFO("max_features unlimited, setting it to " , max_features);
- }
- WOLF_INFO("Detecting " , _max_features , " new features..." );
-
- // detecting new features
- for (unsigned int i = 0; i < max_features; i++)
- {
- n_feature_++;
- FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
- n_feature_* Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Identity(1,1)));
- _features_last_out.push_back(ftr);
-
- WOLF_INFO("feature " , ftr->id() , " detected --> new track" );
- }
-
- WOLF_INFO(_features_last_out.size() , " features detected");
-
- return _features_last_out.size();
-}
-
-FactorBasePtr ProcessorTrackerFeatureDummy::createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
-{
- WOLF_INFO( "creating factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
- , " with origin feature " , _feature_other_ptr->id() );
-
- auto ctr = std::make_shared<FactorEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
-
- return ctr;
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
deleted file mode 100644
index e8092dafb1454e53bd6ab3eb22345c76e468026a..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_image.cpp
+++ /dev/null
@@ -1,399 +0,0 @@
-// Wolf includes
-#include "base/processor/processor_tracker_feature_image.h"
-
-// Vision utils
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
-
-// standard libs
-#include <bitset>
-#include <algorithm>
-
-namespace wolf
-{
-
-ProcessorTrackerFeatureImage::ProcessorTrackerFeatureImage(ProcessorParamsTrackerFeatureImagePtr _params_tracker_feature_image) :
- ProcessorTrackerFeature("IMAGE", _params_tracker_feature_image),
- cell_width_(0), cell_height_(0), // These will be initialized to proper values taken from the sensor via function configure()
- params_tracker_feature_image_(_params_tracker_feature_image)
-{
- // Detector
- std::string det_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_feature_image_->yaml_file_params_vision_utils);
-
- if (det_name.compare("ORB") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
- else if (det_name.compare("FAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorFAST>(det_ptr_);
- else if (det_name.compare("SIFT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSIFT>(det_ptr_);
- else if (det_name.compare("SURF") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSURF>(det_ptr_);
- else if (det_name.compare("BRISK") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorBRISK>(det_ptr_);
- else if (det_name.compare("MSER") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorMSER>(det_ptr_);
- else if (det_name.compare("GFTT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorGFTT>(det_ptr_);
- else if (det_name.compare("HARRIS") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorHARRIS>(det_ptr_);
- else if (det_name.compare("SBD") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSBD>(det_ptr_);
- else if (det_name.compare("KAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorKAZE>(det_ptr_);
- else if (det_name.compare("AKAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAKAZE>(det_ptr_);
- else if (det_name.compare("AGAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
-
- // Descriptor
- std::string des_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_feature_image_->yaml_file_params_vision_utils);
-
- if (des_name.compare("ORB") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
- else if (des_name.compare("SIFT") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSIFT>(des_ptr_);
- else if (des_name.compare("SURF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSURF>(des_ptr_);
- else if (des_name.compare("BRISK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRISK>(des_ptr_);
- else if (des_name.compare("KAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorKAZE>(des_ptr_);
- else if (des_name.compare("AKAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorAKAZE>(des_ptr_);
- else if (des_name.compare("LATCH") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLATCH>(des_ptr_);
- else if (des_name.compare("FREAK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorFREAK>(des_ptr_);
- else if (des_name.compare("BRIEF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRIEF>(des_ptr_);
- else if (des_name.compare("DAISY") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorDAISY>(des_ptr_);
- else if (des_name.compare("LUCID") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
-
- // Matcher
- std::string mat_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_feature_image_->yaml_file_params_vision_utils);
-
- if (mat_name.compare("FLANNBASED") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_L1") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_L1>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING_2") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
-
- // Active search grid
- vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_tracker_feature_image_->yaml_file_params_vision_utils);
- active_search_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmACTIVESEARCH>(alg_ptr);
-}
-
-//Destructor
-ProcessorTrackerFeatureImage::~ProcessorTrackerFeatureImage()
-{
- //
-}
-
-void ProcessorTrackerFeatureImage::configure(SensorBasePtr _sensor)
-{
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(_sensor);
-
- image_.width_ = camera->getImgWidth();
- image_.height_ = camera->getImgHeight();
-
- active_search_ptr_->initAlg(camera->getImgWidth(), camera->getImgHeight() , det_ptr_->getPatternRadius());
-
- params_tracker_feature_image_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
-
- cell_width_ = image_.width_ / params_tracker_feature_image_activesearch_ptr_->n_cells_h;
- cell_height_ = image_.height_ / params_tracker_feature_image_activesearch_ptr_->n_cells_v;
-}
-
-void ProcessorTrackerFeatureImage::preProcess()
-{
- image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_)->getImage();
-
- active_search_ptr_->renew();
-
- //The visualization functions and variables
- if(last_ptr_ != nullptr)
- resetVisualizationFlag(last_ptr_->getFeatureList());
-
- tracker_roi_.clear();
- detector_roi_.clear();
- tracker_target_.clear();
-}
-
-void ProcessorTrackerFeatureImage::postProcess()
-{
-}
-
-unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_matches)
-{
- KeyPointVector candidate_keypoints;
- cv::Mat candidate_descriptors;
- DMatchVector cv_matches;
-
- for (auto feature_base_ptr : _features_last_in)
- {
- FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr);
-
- cv::Rect roi = vision_utils::setRoi(feature_ptr->getKeypoint().pt.x, feature_ptr->getKeypoint().pt.y, cell_width_, cell_height_);
-
- active_search_ptr_->hitCell(feature_ptr->getKeypoint());
-
- cv::Mat target_descriptor = feature_ptr->getDescriptor();
-
- //lists used to debug
- tracker_target_.push_back(feature_ptr->getKeypoint().pt);
- tracker_roi_.push_back(roi);
-
- if (detect(image_incoming_, roi, candidate_keypoints, candidate_descriptors))
- {
- Scalar normalized_score = match(target_descriptor,candidate_descriptors,cv_matches);
- if ( normalized_score > mat_ptr_->getParams()->min_norm_score )
- {
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_matches[0].trainIdx],
- cv_matches[0].trainIdx,
- (candidate_descriptors.row(cv_matches[0].trainIdx)),
- Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
- incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
- _features_incoming_out.push_back(incoming_point_ptr);
-
- _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
- }
- else
- {
- //lists used to debug
- tracker_target_.pop_back();
- tracker_roi_.pop_back();
- }
- }
- else
- {
- //lists used to debug
- tracker_target_.pop_back();
- tracker_roi_.pop_back();
- }
- }
-// std::cout << "TrackFeatures - Number of Features tracked: " << _features_incoming_out.size() << std::endl;
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
-{
- DMatchVector matches_mat;
- FeaturePointImagePtr feat_incoming_ptr = std::static_pointer_cast<FeaturePointImage>(_incoming_feature);
- FeaturePointImagePtr feat_origin_ptr = std::static_pointer_cast<FeaturePointImage>(_origin_feature);
-
- cv::Mat origin_descriptor = feat_origin_ptr->getDescriptor();
- cv::Mat incoming_descriptor = feat_incoming_ptr->getDescriptor();
-
- KeyPointVector origin_keypoint;
- origin_keypoint.push_back(feat_origin_ptr->getKeypoint());
-
- Scalar normalized_score = match(origin_descriptor,incoming_descriptor,matches_mat);
-
- if(normalized_score > mat_ptr_->getParams()->min_norm_score)
- return true;
- else
- {
- /* CORRECT */
-
- unsigned int roi_width = cell_width_;
- unsigned int roi_heigth = cell_height_;
- unsigned int roi_x;
- unsigned int roi_y;
-
- KeyPointVector correction_keypoints;
- cv::Mat correction_descriptors;
- DMatchVector correction_matches;
-
- FeaturePointImagePtr feat_last_ptr = std::static_pointer_cast<FeaturePointImage>(_last_feature);
-
- active_search_ptr_->hitCell(feat_last_ptr->getKeypoint());
-
- roi_x = (feat_last_ptr->getKeypoint().pt.x) - (roi_heigth / 2);
- roi_y = (feat_last_ptr->getKeypoint().pt.y) - (roi_width / 2);
- cv::Rect roi(roi_x, roi_y, roi_width, roi_heigth);
-
- if (detect(image_incoming_, roi, correction_keypoints, correction_descriptors))
- {
- Scalar normalized_score_correction = match(origin_descriptor,correction_descriptors,correction_matches);
- if(normalized_score_correction > mat_ptr_->getParams()->min_norm_score )
- {
- feat_incoming_ptr->setKeypoint(correction_keypoints[correction_matches[0].trainIdx]);
- feat_incoming_ptr->setDescriptor(correction_descriptors.row(correction_matches[0].trainIdx));
- return true;
- }
- else
- {
- return false;
- }
- }
- return false;
- }
-}
-
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out)
-{
- cv::Rect roi;
- KeyPointVector new_keypoints;
- cv::Mat new_descriptors;
- cv::KeyPointsFilter keypoint_filter;
- unsigned int n_new_features = 0;
-
- for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; n_iterations++)
- {
-
- if (active_search_ptr_->pickEmptyRoi(roi))
- {
- detector_roi_.push_back(roi);
- if (detect(image_last_, roi, new_keypoints, new_descriptors))
- {
- KeyPointVector list_keypoints = new_keypoints;
- unsigned int index = 0;
- keypoint_filter.retainBest(new_keypoints,1);
- for(unsigned int i = 0; i < list_keypoints.size(); i++)
- {
- if(list_keypoints[i].pt == new_keypoints[0].pt)
- index = i;
- }
- if(new_keypoints[0].response > params_tracker_feature_image_activesearch_ptr_->min_response_new_feature)
- {
- std::cout << "response: " << new_keypoints[0].response << std::endl;
- FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
- new_keypoints[0],
- 0,
- new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
- point_ptr->setIsKnown(false);
- _features_last_out.push_back(point_ptr);
-
- active_search_ptr_->hitCell(new_keypoints[0]);
-
- n_new_features++;
- }
- }
- else
- {
- active_search_ptr_->blockCell(roi);
- }
- }
- else
- break;
- }
-
- WOLF_DEBUG( "DetectNewFeatures - Number of new features detected: " , n_new_features );
- return n_new_features;
-}
-
-//============================================================
-
-Scalar ProcessorTrackerFeatureImage::match(cv::Mat _target_descriptor, cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
-{
- mat_ptr_->match(_target_descriptor, _candidate_descriptors, des_ptr_->getSize(), _cv_matches);
- Scalar normalized_score = 1 - (Scalar)(_cv_matches[0].distance)/(des_ptr_->getSize()*8);
- return normalized_score;
-}
-
-FactorBasePtr ProcessorTrackerFeatureImage::createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
-{
- FactorEpipolarPtr const_epipolar_ptr = std::make_shared<FactorEpipolar>(_feature_ptr, _feature_other_ptr,
- shared_from_this());
- // _feature_ptr->addFactor(const_epipolar_ptr);
- // _feature_other_ptr->addConstrainedBy(const_epipolar_ptr);
- return const_epipolar_ptr;
-}
-
-unsigned int ProcessorTrackerFeatureImage::detect(cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,
- cv::Mat& _new_descriptors)
-{
- _new_keypoints = det_ptr_->detect(_image, _roi);
- _new_descriptors = des_ptr_->getDescriptor(_image, _new_keypoints);
- return _new_keypoints.size();
-}
-
-void ProcessorTrackerFeatureImage::resetVisualizationFlag(FeatureBasePtrList& _feature_list_last)
-{
- for (auto feature_base_last_ptr : _feature_list_last)
- {
- FeaturePointImagePtr feature_last_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_last_ptr);
- feature_last_ptr->setIsKnown(true);
- }
-}
-
-// draw functions ===================================================================
-
-void ProcessorTrackerFeatureImage::drawTarget(cv::Mat _image, std::list<cv::Point> _target_list)
-{
- if (last_ptr_!=nullptr)
- {
- // draw the target of the tracking
- for(auto target_point : _target_list)
- cv::circle(_image, target_point, 7, cv::Scalar(255.0, 0.0, 255.0), 1, 3, 0);
- }
-}
-
-void ProcessorTrackerFeatureImage::drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color)
-{
- if (last_ptr_!=nullptr)
- {
- for (auto roi : _roi_list)
- cv::rectangle(_image, roi, _color, 1, 8, 0);
- }
-}
-
-void ProcessorTrackerFeatureImage::drawFeatures(cv::Mat _image)
-{
- if (last_ptr_!=nullptr)
- {
- unsigned int known_feature_counter = 0;
- unsigned int new_feature_counter = 0;
-
- for (auto feature_ptr : last_ptr_->getFeatureList())
- {
- FeaturePointImagePtr point_ptr = std::static_pointer_cast<FeaturePointImage>(feature_ptr);
- if (point_ptr->isKnown())
- {
- cv::circle(_image, point_ptr->getKeypoint().pt, 4, cv::Scalar(51.0, 255.0, 51.0), -1, 3, 0);
- known_feature_counter++;
- }
- else
- {
- cv::circle(_image, point_ptr->getKeypoint().pt, 4, cv::Scalar(0.0, 0.0, 255.0), -1, 3, 0);
- new_feature_counter++;
- }
-
- cv::putText(_image, std::to_string(feature_ptr->trackId()), point_ptr->getKeypoint().pt,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 255.0, 0.0));
- }
- std::cout << "\nKnown: " << known_feature_counter << "\tNew: " << new_feature_counter << std::endl;
- }
-}
-
-ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr)
-{
- ProcessorTrackerFeatureImagePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureImage>(std::static_pointer_cast<ProcessorParamsTrackerFeatureImage>(_params));
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("IMAGE FEATURE", ProcessorTrackerFeatureImage)
-} // namespace wolf
-
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
deleted file mode 100644
index 9c2fa6ac19d996cc682613de624af5bdae655846..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ /dev/null
@@ -1,1140 +0,0 @@
-/*
- * processor_tracker_feature_polyline_2D.cpp
- *
- * Created on: Mar 11, 2019
- * Author: jvallve
- */
-
-#include "base/processor/processor_tracker_feature_polyline_2D.h"
-
-namespace wolf
-{
-
-ProcessorTrackerFeaturePolyline2D::ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params) :
- ProcessorTrackerFeature("TRACKER FEATURE POLYLINE",_params),
- params_tracker_feature_polyline_(_params),
- line_finder_(_params->line_finder_params),
- extrinsics_transformation_computed_(false)
-{
-
-}
-
-ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D()
-{
-}
-
-unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
-{
- WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_last_in.size());
-
- if (_features_last_in.empty())
- return 0;
-
- // prior transformations
- Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity());
- T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_;
- T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
-
- // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: all_features_incoming_)
- auto ftr_it = all_features_incoming_.begin();
-
- // iterate over all polylines features
- while (ftr_it != all_features_incoming_.end())
- {
- bool matched = false;
- auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- FeatureMatchPolyline2DScalarMap best_matches;
-
- std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n";
-
- // Check matching with all features in last
- // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior
- for (auto ftr_last_ : _features_last_in)
- tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior);
-
- std::cout << "\t" << best_matches.size() << " matches with features last found\n";
-
- // Try the match for all candidates (sorted by difference from movement prior)
- for (auto best_ftr_match_pair : best_matches)
- {
- auto best_ftr_match = best_ftr_match_pair.second;
- auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(best_ftr_match->feature_ptr_);
-
- std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << "\n";
-
- // If it has not been created in processNew: check/update/remove landmark match
- if(find(new_features_last_.begin(), new_features_last_.end(), pl_last) == new_features_last_.end())
- {
- // the match with this last_ feature was removed after any updateMatchWithLandmark
- if(landmark_match_map_.find(pl_last) == landmark_match_map_.end())
- {
- std::cout << "\t\tremoved feature last\n";
- continue;
- }
-
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
-
- // Landmark changed or was merged -> Redo match with last feature
- if (landmark_match_map_[pl_last]->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
- {
- std::cout << "\t\tupdating match...\n";
- if (!updateMatchWithLandmark(landmark_match_map_[pl_last],pl_lmk,pl_last))
- {
- // not successful update -> remove match and feature
- std::cout << "\t\t\tNot success: removing feature " << pl_last->id() << " and landmark match\n";
- landmark_match_map_.erase(pl_last);
- last_ptr_->getFeatureList().remove(pl_last);
- continue;
- }
- }
- // removed -> not valid match with landmark
- else if (pl_lmk->isRemoving())
- continue;
-
- // try to concatenate landmark match
- auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
- best_ftr_match,
- landmark_match_map_[pl_last]);
- // Add the incoming match to landmark_match_map
- if (pl_lmk_match_incoming != nullptr)
- landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
- else
- {
- WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming track lost common points with landmark");
- continue;
- }
- }
-
- // TRACK valid
- std::cout << "\t\tvalid match, storing...\n";
- // add best match to match_map _feature_correspondences[feature_out_ptr] = feature_in_ptr
- _feature_correspondences[pl_incoming] = best_ftr_match;
-
- // add feature to list of tracked features
- _features_incoming_out.push_back(pl_incoming);
-
- // match for this feature has been found
- matched = true;
- std::cout << "\t\tmatch found\n";
- break;
- }
-
- // next feature
- if (matched)
- ftr_it = all_features_incoming_.erase(ftr_it); // remove from all_features_incoming
- else
- ftr_it++;
- }
-
- WOLF_DEBUG(_feature_correspondences.size(), " features tracked");
- return _feature_correspondences.size();
-}
-
-bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
-{
- //WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
- // If incoming observed new landmark points & landmark has points
- // Check if match is still valid and update the match
- // TODO
-
-// int& new_first_points = best_ftr_match->feature_incoming_from_id_;
-// int new_last_points = pl_incoming->getNPoints() - best_ftr_match->feature_incoming_to_id_ - 1;
-// int overlapping = best_ftr_match->feature_incoming_to_id_ - best_ftr_match->feature_incoming_from_id_;
-
- return true;
-}
-
-bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame()
-{
- //WOLF_DEBUG("PTFP ", getName(), "::voteForKeyFrame: ");
- // TODO
- /* IDEAS:
- * - Any feature incoming derived too much from projected Landmark
- * - Any feature incoming overlapping with Landmark less than param (1 or 2) defined points
- * - Any feature last would define extreme or add points to the Landmark
- * - Some (more than param) tracks lost
- */
- return false;
-}
-
-unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _max_features)
-{
- WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
- unsigned int n = ProcessorTrackerFeature::processNew(_max_features);
-
- WOLF_DEBUG("Processing ", n, " new last features");
-
- // prior transformations
- Eigen::Matrix3s T_sensor_world_last(Eigen::Matrix3s::Identity());
- T_sensor_world_last.topLeftCorner(2,2) = R_sensor_world_last_;
- T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_;
-
- // For each new feature: Either create a landmark or match with an existent landmark
- LandmarkBasePtrList new_landmarks;
- for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n);
- ftr_it != last_ptr_->getFeatureList().end();
- ftr_it++)
- {
- auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- LandmarkMatchPolyline2DScalarMap best_lmk_matches;
- WOLF_DEBUG("Processing feature: ", pl_ftr->id());
-
- // Check matching with all existing polyline landmarks
- for (auto lmk : getProblem()->getMap()->getLandmarkList())
- // Check for polylines landmarks that hasn't been just created
- if (lmk->getType() == "POLYLINE 2D" && std::find(new_landmarks.begin(), new_landmarks.end(), lmk) == new_landmarks.end())
- // Store all matches consistent with T_sensor_world_last in best_lmk_matches sorted by difference from T_sensor_world_last
- tryMatchWithLandmark(best_lmk_matches, std::static_pointer_cast<LandmarkPolyline2D>(lmk), pl_ftr, T_sensor_world_last);
-
- // If not matched with any existing landmark: create a new one
- if (best_lmk_matches.empty())
- {
- // create a landmark
- auto new_lmk_ptr = createLandmark(pl_ftr);
-
- // Add a new match to landmark_match_map_
- auto new_lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
- new_lmk_match->landmark_ptr_ = new_lmk_ptr;
- new_lmk_match->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(new_lmk_ptr)->getVersion();
- new_lmk_match->feature_from_id_= 0; // all points match
- new_lmk_match->landmark_from_id_ = 0; // all points match
- new_lmk_match->feature_to_id_= pl_ftr->getNPoints()-1; // all points match
- new_lmk_match->landmark_to_id_ = pl_ftr->getNPoints()-1; // all points match
- new_lmk_match->normalized_score_ = 1.0; // max score
- new_lmk_match->T_feature_landmark_ = Eigen::Matrix3s::Identity();
- new_lmk_match->T_feature_landmark_.topLeftCorner(2,2) = R_sensor_world_last_;
- new_lmk_match->T_feature_landmark_.topRightCorner(2,1) = t_sensor_world_last_;
-
- // Add the new landmark to the map
- getProblem()->addLandmark(new_lmk_ptr);
- // Store in new_landmarks
- new_landmarks.push_back(new_lmk_ptr);
- // store match in map
- landmark_match_map_[pl_ftr] = new_lmk_match;
- }
- // If matched
- else
- {
- // store best match (lowest difference from prior) in map
- landmark_match_map_[pl_ftr] = best_lmk_matches.begin()->second;
-
- // store landmark candidates to be merged
- if (best_lmk_matches.size() > 1)
- {
- LandmarkPolyline2DPtrList merge_candidates;
- for (auto lmk_match_pair_it : best_lmk_matches)
- merge_candidates.push_back(std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_pair_it.second->landmark_ptr_));
- merge_candidates_list_.push_back(merge_candidates);
- }
- }
- }
-
- // store in landmark_match_map_ the incoming features
- auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n);
- while (ftr_it != incoming_ptr_->getFeatureList().end())
- {
- auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- assert(matches_last_from_incoming_.find(pl_incoming) != matches_last_from_incoming_.end() && "last-incoming match not found");
- auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[pl_incoming]);
- assert(landmark_match_map_.find(pl_ftr_match->feature_ptr_) != landmark_match_map_.end() && "last-lmk match not found");
- auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
- pl_ftr_match,
- landmark_match_map_[pl_ftr_match->feature_ptr_]);
- if (pl_lmk_match_incoming != nullptr)
- {
- landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
- ftr_it++;
- }
- else
- {
- WOLF_DEBUG("PTFP ", getName(), "::processNew: incoming track lost common points with landmark");
- matches_last_from_incoming_.erase(pl_incoming);
- // move feature from known to all features
- all_features_incoming_.push_back(pl_incoming);
- ftr_it = incoming_ptr_->getFeatureList().erase(ftr_it);
- }
- }
- return n;
-}
-
-unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _max_new_features,
- FeatureBasePtrList& _features_last_out)
-{
- WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
-
- _features_last_out = std::move(all_features_last_);
- if (_max_new_features != -1 && _features_last_out.size() > _max_new_features)
- _features_last_out.resize(_max_new_features);
-
- WOLF_DEBUG(_features_last_out.size(), " were provided");
-
- return _features_last_out.size();
-}
-
-void ProcessorTrackerFeaturePolyline2D::establishFactors()
-{
- WOLF_DEBUG("PTFP ", getName(), "::establishFactors: ");
- unsigned int N_factors = 0;
-
- // Create a factor for each match in last features
- auto ftr_it = last_ptr_->getFeatureList().begin();
- while (ftr_it != last_ptr_->getFeatureList().end())
- {
- WOLF_DEBUG("\tLast feature: ", (*ftr_it)->id());
- assert(landmark_match_map_.find(*ftr_it) != landmark_match_map_.end() && "feature without landmark match in last features");
- auto lmk_match = landmark_match_map_[*ftr_it];
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
- auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- WOLF_DEBUG("\tLandmark: ", pl_lmk->id());
-
- // LANDMARK CHANGED: update match
- WOLF_DEBUG("\tLandmark changed?");
- if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
- {
- WOLF_DEBUG("\tLandmark changed");
- // not successful update
- if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr))
- {
- std::cout << "\t\tNot success: removing feature " << pl_ftr->id() << " and landmark match\n";
- // remove from match map
- landmark_match_map_.erase(*ftr_it);
- // remove from last feature list
- ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
- continue;
- }
- }
-
- // LANDMARK REMOVED: remove match and feature
- else
- {
- WOLF_DEBUG("\tLandmark removed?");
- if (pl_lmk->isRemoving())
- {
- std::cout << "\t\tLandmark was removed: removing feature " << pl_ftr->id() << " and landmark match\n";
- // remove from match map
- landmark_match_map_.erase(*ftr_it);
- // remove from last feature list
- ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
- continue;
- }
- }
-
- // MODIFY LANDMARK (only for not closed and not recently created)
- WOLF_DEBUG("\tModifying..");
- if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
- // If modified, add lmk to modified_lmks_ list
- if (modifyLandmarkAndMatch(lmk_match, pl_ftr))
- modified_lmks_.push_back(pl_lmk);
-
- // ESTABLISH CONSTRAINTS
- WOLF_DEBUG("\tEstablishing factors..");
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
-
- std::cout << "Establishing factor between" << std::endl;
- std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
- std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
- std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
-
- // Factors for all feature points
- int lmk_point_id = lmk_match->landmark_from_id_;
- for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
- {
- std::cout << "feature point " << ftr_point_id << std::endl;
- std::cout << "landmark point " << lmk_point_id << std::endl;
-
- // First not defined point
- if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
- // first point to line factor
- {
- std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
- assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
- N_factors++;
- std::cout << "factor added" << std::endl;
- }
-
- // Last not defined point
- else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
- // last point to line factor
- {
- std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl;
- assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
- N_factors++;
- std::cout << "factor added" << std::endl;
- }
-
- // Defined point
- else
- // point to point factor
- {
- std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
- //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
- //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
- emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
- N_factors++;
- std::cout << "factor added" << std::endl;
- }
-
- // Next lmk point
- if (ftr_point_id < pl_ftr->getNPoints()-1)
- lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
- }
- // next ftr
- ftr_it++;
- }
- std::cout << N_factors << " factors established" << std::endl;
-}
-
-void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id)
-{
- assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
-
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id)
-{
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr)
-{
- WOLF_DEBUG("PTFP ", getName(), "::createLandmark: ");
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
- assert(_feature_ptr->getType() == "POLYLINE 2D");
- assert(std::find(last_ptr_->getFeatureList().begin(),last_ptr_->getFeatureList().end(),_feature_ptr) != last_ptr_->getFeatureList().end() && "creating a landmark for a feature which is not in last capture");
-
-
- FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
- // compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_last_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_last_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
- // std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- // std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
- // Create new landmark
- return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true),
- std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true),
- points_global,
- polyline_ptr->isFirstDefined(),
- polyline_ptr->isLastDefined());
- //std::cout << "done" << std::endl;
-}
-
-bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
-{
- WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
-
- bool print = true;
- bool modified = false;
-
- if (print)
- {
- std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
- }
- //Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
- // t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
- Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
-
- // MODIFY LANDMARK
- // -----------------GROW Front-----------------
- // Add new front points (if any feature point not matched)
- if ( lmk_match->feature_from_id_ > 0 ) // && !pl_lmk->isClosed()
- {
- assert(!pl_lmk->isClosed() && "feature has not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add new front points. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- lmk_match->feature_from_id_-1, // last feature point index to be added
- pl_ftr->isFirstDefined(), // is defined
- false); // front
-
- lmk_match->landmark_from_id_ = pl_lmk->getFirstId();
- lmk_match->feature_from_id_ = 0;
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- modified = true;
- if (print)
- {
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
-
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- // -----------------CHANGE Front-----------------
- // Change first not defined point if landmark first not defined matched with first feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
- {
- if (print)
- {
- std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isFirstDefined() ||
- // case b
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))).squaredNorm() >
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm())
- {
- pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
- modified = true;
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- }
-
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- // -----------------GROW Back-----------------
- // Add new back points (if any feature point not matched)
- if (lmk_match->feature_to_id_ < pl_ftr->getNPoints()-1)
- {
- assert(!pl_lmk->isClosed() && "feature not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add back points. Defined: " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- lmk_match->feature_to_id_+1, // last feature point index to be added
- pl_ftr->isLastDefined(), // is defined
- true); // back
-
- lmk_match->landmark_to_id_ = pl_lmk->getLastId();
- lmk_match->feature_to_id_ = pl_ftr->getNPoints()-1;
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- modified = true;
- if (print)
- {
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- // -----------------CHANGE Back-----------------
- // Change last not defined point if landmark last not defined matched with last feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (lmk_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
- {
- if (print)
- {
- std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isLastDefined() ||
- // case b
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))).squaredNorm() >
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm())
- {
- pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined());
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- modified = true;
- }
-
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
-
- if (print)
- {
- std::cout << "MODIFIED LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- }
-
- return modified;
-}
-
-void ProcessorTrackerFeaturePolyline2D::advanceDerived()
-{
- WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: ");
- // remove last features from landmark_match_map_
- if (last_ptr_ != nullptr)
- for (auto ftr : last_ptr_->getFeatureList())
- {
- assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
- landmark_match_map_.erase(ftr);
- }
-
- WOLF_DEBUG("removing ", all_features_last_.size() , " features in all_features_last_");
- all_features_last_.clear();
- all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
- if (last_ptr_)
- WOLF_DEBUG("all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
-
- ProcessorTrackerFeature::advanceDerived();
-}
-
-void ProcessorTrackerFeaturePolyline2D::resetDerived()
-{
- WOLF_DEBUG("PTFP ", getName(), "::resetDerived: ");
- // remove last features from landmark_match_map_
- if (last_ptr_ != nullptr)
- for (auto ftr : last_ptr_->getFeatureList())
- {
- assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
- landmark_match_map_.erase(ftr);
- }
-
- WOLF_DEBUG("PTF ", getName(), ": ", "removing ", all_features_last_.size() , " features in all_features_last_");
- all_features_last_.clear();
- all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
- WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
- if (last_ptr_)
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
-
- ProcessorTrackerFeature::resetDerived();
-}
-
-void ProcessorTrackerFeaturePolyline2D::preProcess()
-{
- WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: ");
- // Extract all polylines from incoming_
- // TODO: sort corners by bearing
- std::list<laserscanutils::Polyline> polylines;
-
- line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
- WOLF_DEBUG("PTFP ", getName(), " Polylines extracted: ", polylines.size());
-
- for (auto&& pl : polylines)
- {
- //WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ );
- //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
- all_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
- //WOLF_DEBUG("new polyline detected: ");
- }
-
- // compute transformations
- if (last_ptr_ != nullptr && incoming_ptr_ != nullptr)
- computeTransformations();
-
- WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
- if (last_ptr_)
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
-}
-
-void ProcessorTrackerFeaturePolyline2D::postProcess()
-{
- WOLF_DEBUG("PTFP ", getName(), "::postProcess: ");
- // Try to close & classify modified landmarks
- while (!modified_lmks_.empty())
- {
- auto lmk_ptr = modified_lmks_.front();
- modified_lmks_.pop_front();
-
- assert(!lmk_ptr->isRemoving());
-
- // try classify
- if (lmk_ptr->getClassification().type == UNCLASSIFIED && lmk_ptr->getNDefinedPoints() >= 3 && lmk_ptr->getNDefinedPoints() <= 4)
- lmk_ptr->tryClassify(params_tracker_feature_polyline_->class_position_error_th, params_tracker_feature_polyline_->polyline_classes);
- // try close
- if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 3 || lmk_ptr->getNPoints() >= 4))
- lmk_ptr->tryClose(params_tracker_feature_polyline_->class_position_error_th);
- }
-
- // Try to merge landmarks
- while (!merge_candidates_list_.empty())
- {
- // load next list of candidates
- LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front());
- merge_candidates_list_.pop_front();
-
- // change already merged lmks with the corresponding remaining lmk
- for (auto&& lmk : merge_candidates)
- if (lmk->getMergedInLandmark() != nullptr)
- lmk = lmk->getMergedInLandmark();
-
- // remove repeated lmks
- merge_candidates.sort();
- merge_candidates.unique();
-
- if (merge_candidates.size() == 1)
- continue;
-
- // Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
- LandmarkPolyline2D::tryMergeLandmarks(merge_candidates, params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
- }
- WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
- if (last_ptr_)
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
-}
-
-void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
- const FeaturePolyline2DPtr& _ftr_L,
- const FeaturePolyline2DPtr& _ftr_I,
- const Eigen::Matrix3s& _T_last_incoming_prior) const
-{
- //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: ");
-
- // compute best sq distance matching
- Eigen::MatrixXs last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
- MatchPolyline2DPtr best_match = computeBestSqDist(_ftr_I->getPoints(), // <--feature incoming points
- _ftr_I->isFirstDefined(),
- _ftr_I->isLastDefined(),
- false, // <--feature is not closed for sure
- last_expected_points, // <--feature last points
- _ftr_L->isFirstDefined(),
- _ftr_L->isLastDefined(),
- false, // <--feature is not closed for sure
- params_tracker_feature_polyline_->match_feature_pose_sq_norm_th);
- //valid match
- if (best_match != nullptr)
- {
-
- auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
- // feature incoming point id's
- ftr_match->feature_incoming_from_id_ = best_match->from_A_id_;
- ftr_match->feature_incoming_to_id_ = best_match->to_A_id_;
- // feature last point id's
- ftr_match->feature_last_from_id_ = best_match->from_B_id_;
- ftr_match->feature_last_to_id_ = best_match->to_B_id_;
- // incoming last transformation
- bool from_defined = (ftr_match->feature_incoming_from_id_ != 0 || _ftr_I->isFirstDefined()) &&
- (ftr_match->feature_last_from_id_ != 0 || _ftr_L->isFirstDefined());
- bool to_defined = (ftr_match->feature_incoming_to_id_ != _ftr_I->getNPoints()-1 || _ftr_I->isLastDefined()) &&
- (ftr_match->feature_last_to_id_ != _ftr_L->getNPoints()-1 || _ftr_L->isLastDefined());
- int N_points = ftr_match->feature_incoming_to_id_ - ftr_match->feature_incoming_from_id_;
- int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1);
- if (N_def_points > 0 && N_points > 2)
- ftr_match->T_incoming_last_ = pose2T2D(computeRigidTrans(_ftr_I->getPoints().middleCols(ftr_match->feature_incoming_from_id_,N_points),
- _ftr_L->getPoints().middleCols(ftr_match->feature_last_from_id_,N_points),
- from_defined,
- to_defined));
- else
- ftr_match->T_incoming_last_ = _T_last_incoming_prior;
- // score
- ftr_match->normalized_score_ = best_match->normalized_score_;
- // feature correspondence
- ftr_match->feature_ptr_ = _ftr_L;
- // store in list
- ftr_matches[ftr_match->normalized_score_] = ftr_match;
- WOLF_DEBUG("match stored!");
- }
-
-// WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose());
-// WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose());
-// // compute best rigid transformations
-// MatchPolyline2DPtrList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points
-// _ftr_I->isFirstDefined(),
-// _ftr_I->isLastDefined(),
-// false, // <--feature is not closed for sure
-// _ftr_L->getPoints(), // <--feature last points
-// _ftr_L->isFirstDefined(),
-// _ftr_L->isLastDefined(),
-// false, // <--feature is not closed for sure
-// params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
-// // valid match
-// for (auto match : matches)
-// {
-// WOLF_DEBUG("match with pose ", T2pose2D(match->T_A_B_).transpose());
-// Scalar sq_norm = T2pose2D(_T_last_incoming_prior * match->T_A_B_).squaredNorm();
-//
-// if (sq_norm > params_tracker_feature_polyline_->match_feature_pose_sq_norm_th)
-// {
-// WOLF_DEBUG("match with squared norm ", sq_norm, " discarded");
-// continue;
-// }
-// WOLF_DEBUG("match with squared norm ", sq_norm, " stored!");
-//
-// auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
-// // feature incoming point id's
-// ftr_match->feature_incoming_from_id_ = match->from_A_id_;
-// ftr_match->feature_incoming_to_id_ = match->to_A_id_;
-// // feature last point id's
-// ftr_match->feature_last_from_id_ = match->from_B_id_;
-// ftr_match->feature_last_to_id_ = match->to_B_id_;
-// // incoming last transformation
-// ftr_match->T_incoming_last_ = match->T_A_B_;
-// // score
-// ftr_match->normalized_score_ = match->normalized_score_;
-// // feature correspondence
-// ftr_match->feature_ptr_ = _ftr_L;
-// // store in list
-// ftr_matches[sq_norm] = ftr_match;
-// }
-}
-
-void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const
-{
- //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
-
- // compute best sq distance matching
- Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
- MatchPolyline2DPtr best_match = computeBestSqDist(lmk_expected_points, // <--landmark points in local coordinates
- _lmk_ptr->isFirstDefined(),
- _lmk_ptr->isLastDefined(),
- _lmk_ptr->isClosed(),
- _feat_ptr->getPoints(), // <--feature points
- _feat_ptr->isFirstDefined(),
- _feat_ptr->isLastDefined(),
- false, // <--feature is not closed for sure
- params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
- //valid match
- if (best_match != nullptr)
- {
- auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
-
- // landmark
- lmk_match->landmark_ptr_=_lmk_ptr;
- lmk_match->landmark_version_=_lmk_ptr->getVersion();
- // landmark point id's
- std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
- lmk_match->landmark_from_id_ = lmk_valid_ids[best_match->from_A_id_];
- lmk_match->landmark_to_id_ = lmk_valid_ids[best_match->to_A_id_];
- // feature point id's
- lmk_match->feature_from_id_ = best_match->from_B_id_;
- lmk_match->feature_to_id_ = best_match->to_B_id_;
-
- // feature landmark transformation
- lmk_match->T_feature_landmark_ = best_match->T_A_B_.inverse().eval();
-
- bool from_defined = (lmk_match->landmark_from_id_ != _lmk_ptr->getFirstId() || _lmk_ptr->isFirstDefined()) &&
- (lmk_match->feature_from_id_ != 0 || _feat_ptr->isFirstDefined());
- bool to_defined = (lmk_match->landmark_to_id_ != _lmk_ptr->getLastId() || _lmk_ptr->isLastDefined()) &&
- (lmk_match->feature_to_id_ != _feat_ptr->getNPoints()-1 || _feat_ptr->isLastDefined());
- int N_points = lmk_match->feature_to_id_ - lmk_match->feature_from_id_;
- int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1);
- if (N_def_points > 0 && N_points > 2)
- lmk_match->T_feature_landmark_ = pose2T2D(computeRigidTrans(_feat_ptr->getPoints().middleCols(lmk_match->feature_from_id_,N_points),
- _lmk_ptr->computePointsGlobal().middleCols(lmk_match->landmark_from_id_,N_points),
- from_defined,
- to_defined));
- else
- lmk_match->T_feature_landmark_ = _T_feature_landmark_prior;
- // score
- lmk_match->normalized_score_ = best_match->normalized_score_;
-
- assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
- assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
-
- // store in list
- lmk_matches[lmk_match->normalized_score_]= lmk_match;
- WOLF_DEBUG("match stored!");
- assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
- }
-
-// // compute best rigid transformation matching
-// MatchPolyline2DPtrList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates
-// _lmk_ptr->isFirstDefined(),
-// _lmk_ptr->isLastDefined(),
-// _lmk_ptr->isClosed(),
-// _feat_ptr->getPoints(), // <--feature points
-// _feat_ptr->isFirstDefined(),
-// _feat_ptr->isLastDefined(),
-// false, // <--feature is not closed for sure
-// params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
-// // valid match
-// for (auto match : matches)
-// {
-// Scalar sq_norm = T2pose2D(_T_feature_landmark_prior * match->T_A_B_).squaredNorm();
-//
-// if (sq_norm > params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th)
-// continue;
-//
-// auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
-//
-// // landmark
-// lmk_match->landmark_ptr_=_lmk_ptr;
-// lmk_match->landmark_version_=_lmk_ptr->getVersion();
-// // landmark point id's
-// std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
-// lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_];
-// lmk_match->landmark_to_id_ = lmk_valid_ids[match->to_A_id_];
-// // feature point id's
-// lmk_match->feature_from_id_ = match->from_B_id_;
-// lmk_match->feature_to_id_ = match->to_B_id_;
-//
-// // incoming last transformation
-// lmk_match->T_feature_landmark_ = match->T_A_B_.inverse().eval();
-// // score
-// lmk_match->normalized_score_ = match->normalized_score_;
-//
-// assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
-// assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
-//
-// // store in list
-// lmk_matches[sq_norm]= lmk_match;
-// }
-}
-
-bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match,
- LandmarkPolyline2DPtr& _lmk_ptr,
- FeaturePolyline2DPtr& _feat_ptr) const
-{
- assert(_lmk_match->check() && "updateMatchWithLandmark: input _lmk_match wrong");
- WOLF_DEBUG("PTFP ", getName(), "::updateMatchWithLandmark: ");
- assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id());
-
- // merged: update the pl_lmk pointer
- if (_lmk_ptr->getMergedInLandmark() != nullptr)
- _lmk_ptr = _lmk_ptr->getMergedInLandmark();
-
- // try match with _feat_ptr again
- LandmarkMatchPolyline2DScalarMap new_lmk_matches;
- tryMatchWithLandmark(new_lmk_matches,_lmk_ptr,_feat_ptr, _lmk_match->T_feature_landmark_);
-
- // not valid match
- if (new_lmk_matches.empty())
- {
- _lmk_match = nullptr;
- return false;
- }
-
- // valid match: update landmark match of last
- assert(_lmk_match->check() && "updateMatchWithLandmark: wrongly updated");
- _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match
- return true;
-}
-
-LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
- FeatureMatchPolyline2DPtr ftr_match,
- LandmarkMatchPolyline2DPtr lmk_match_last) const
-{
- assert(lmk_match_last->check() && "input lmk_match_last wrong");
- WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: ");
- std::cout << "ftr_match:";
- std::cout << "\n\tincoming_from: " << ftr_match->feature_incoming_from_id_;
- std::cout << "\n\tincoming_to: " << ftr_match->feature_incoming_to_id_;
- std::cout << "\n\tlast_from: " << ftr_match->feature_last_from_id_;
- std::cout << "\n\tlast_to: " << ftr_match->feature_last_to_id_;
- std::cout << "\nlmk_match_last:";
- std::cout << "\n\tlast_from: " << lmk_match_last->feature_from_id_;
- std::cout << "\n\tlast_to: " << lmk_match_last->feature_to_id_;
- std::cout << "\n\tlmk_from: " << lmk_match_last->landmark_from_id_;
- std::cout << "\n\tlmk_to: " << lmk_match_last->landmark_to_id_;
-
- auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
- auto lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_);
-
- // feature correspondence from&to
- lmk_match_incoming->feature_from_id_ = ftr_match->feature_incoming_from_id_;
- lmk_match_incoming->feature_to_id_ = ftr_match->feature_incoming_to_id_;
-
- // landmark correspondence from&to
- // Not adding new points nor checks (done in correctFeatureDrift)
- int from_offset = ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
- int to_offset = ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_;
- std::cout << "\nfrom_offset: " << from_offset;
- std::cout << "\nto_offset: " << from_offset;
- lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_;
- lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_;
- while (from_offset > 0)
- {
- if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
- return nullptr;
- lmk_match_incoming->landmark_from_id_ = lmk_ptr->getNextValidId(lmk_match_incoming->landmark_from_id_);
- from_offset--;
- }
- while (from_offset < 0)
- {
- lmk_match_incoming->feature_from_id_++;
- from_offset++;
- }
- while (to_offset < 0)
- {
- if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
- return nullptr;
- lmk_match_incoming->landmark_to_id_ = lmk_ptr->getPrevValidId(lmk_match_incoming->landmark_to_id_);
- to_offset++;
- }
- while (to_offset > 0)
- {
- lmk_match_incoming->feature_to_id_--;
- to_offset--;
- }
- std::cout << "\nlmk_match_incoming:";
- std::cout << "\n\tincoming_from: " << lmk_match_incoming->feature_from_id_;
- std::cout << "\n\tincoming_to: " << lmk_match_incoming->feature_to_id_;
- std::cout << "\n\tlmk_from: " << lmk_match_incoming->landmark_from_id_;
- std::cout << "\n\tlmk_to: " << lmk_match_incoming->landmark_to_id_ << std::endl;
-
- // other match attributes
- lmk_match_incoming->landmark_ptr_ = lmk_ptr;
- lmk_match_incoming->landmark_version_ = lmk_ptr->getVersion();
- lmk_match_incoming->normalized_score_ = ftr_match->normalized_score_;
- lmk_match_incoming->T_feature_landmark_ = ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
-
- assert(lmk_match_incoming->check() && "lmk_match_incoming wrongly concatenated");
-
- return lmk_match_incoming;
-}
-
-// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorTrackerFeaturePolyline2D::computeTransformations()
-{
- //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
- Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
- Eigen::Vector3s vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp());
-
- // world_robot
- Eigen::Matrix2s R_world_robot_incoming = Eigen::Rotation2Ds(vehicle_pose_incoming(2)).matrix();
- Eigen::Matrix2s R_world_robot_last = Eigen::Rotation2Ds(vehicle_pose_last(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->isExtrinsicDynamic() ||
- !getSensor()->getP()->isFixed() ||
- !getSensor()->getO()->isFixed() ||
- !extrinsics_transformation_computed_)
- {
- t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // INCOMING
- // global_sensor
- R_world_sensor_incoming_ = R_world_robot_incoming * R_robot_sensor_;
- t_world_sensor_incoming_ = vehicle_pose_incoming.head<2>() + R_world_robot_incoming * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_incoming_ = R_world_sensor_incoming_.transpose();
- t_sensor_world_incoming_ = -R_sensor_world_incoming_ * t_world_sensor_incoming_;
-
- // LAST
- // global_sensor
- R_world_sensor_last_ = R_world_robot_last * R_robot_sensor_;
- t_world_sensor_last_ = vehicle_pose_last.head<2>() + R_world_robot_last * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_last_ = R_world_sensor_last_.transpose();
- t_sensor_world_last_ = -R_sensor_world_last_ * t_world_sensor_last_;
-
- // INCOMING-LAST
- R_incoming_last_ = R_sensor_world_incoming_ * R_world_sensor_last_;
- t_incoming_last_ = t_sensor_world_incoming_ + R_sensor_world_incoming_ * t_world_sensor_last_;
-
- // last_incoming
- R_last_incoming_ = R_world_sensor_last_.transpose() * R_world_sensor_incoming_;
- t_last_incoming_ = t_sensor_world_last_ + R_sensor_world_last_ * t_world_sensor_incoming_;
-
- //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
- //std::cout << "t_world_sensor_incoming_ " << t_world_sensor_incoming_.transpose() << std::endl;
- //std::cout << "R_world_sensor_incoming_ " << std::endl << R_world_sensor_incoming_ << std::endl;
- //std::cout << "t_sensor_world_incoming_ " << t_sensor_world_incoming_.transpose() << std::endl;
- //std::cout << "R_sensor_world_incoming_ " << std::endl << R_sensor_world_incoming_ << std::endl;
- //std::cout << "t_world_sensor_last_ " << t_world_sensor_last_.transpose() << std::endl;
- //std::cout << "R_world_sensor_last_ " << std::endl << R_world_sensor_last_ << std::endl;
- //std::cout << "t_sensor_world_last_ " << t_sensor_world_last_.transpose() << std::endl;
- //std::cout << "R_sensor_world_last_ " << std::endl << R_sensor_world_last_ << std::endl;
- //std::cout << "t_incoming_last_ " << t_incoming_last_.transpose() << std::endl;
- //std::cout << "R_incoming_last_ " << std::endl << R_incoming_last_ << std::endl;
- //std::cout << "t_last_incoming_ " << t_last_incoming_.transpose() << std::endl;
- //std::cout << "R_last_incoming_ " << std::endl << R_last_incoming_ << std::endl;
-
-}
-
-ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr)
-{
- auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline2D>(_params);
- auto prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline2D>(params);
- prc_ptr->setName(_unique_name);
- prc_ptr->configure(_sensor_ptr);
- return prc_ptr;
-}
-
-} /* namespace wolf */
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D)
-} /* namespace wolf */
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
deleted file mode 100644
index cfb691cda6eb2c2eadd9bb0726db6aaec9a2a39e..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark.cpp
+++ /dev/null
@@ -1,144 +0,0 @@
-/**
- * \file processor_tracker_landmark.cpp
- *
- * Created on: Apr 7, 2016
- * \author: jvallve
- */
-
-#include "base/processor/processor_tracker_landmark.h"
-#include "base/map_base.h"
-
-#include <utility>
-
-namespace wolf
-{
-
-ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTracker(_type, _params_tracker_landmark),
- params_tracker_landmark_(_params_tracker_landmark)
-{
- //
-}
-
-ProcessorTrackerLandmark::~ProcessorTrackerLandmark()
-{
- // All is shared_ptr: no need to destruct explicitly
- //
- // for ( auto match : matches_landmark_from_incoming_)
- // {
- // match.second.reset(); // : Should we just remove the entries? What about match.first?
- // }
- // for ( auto match : matches_landmark_from_last_)
- // {
- // match.second.reset(); // : Should we just remove the entries? What about match.first?
- // }
-}
-
-void ProcessorTrackerLandmark::advanceDerived()
-{
- for (auto match : matches_landmark_from_last_)
- {
- match.second.reset(); // TODO: Should we just remove the entries? What about match.first?
- }
- matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- // for (auto match : matches_landmark_from_last_)
- // std::cout << "\t" << match.first->id() << " to " << match.second->landmark_ptr_->id() << std::endl;
-}
-
-void ProcessorTrackerLandmark::resetDerived()
-{
- //std::cout << "ProcessorTrackerLandmark::reset" << std::endl;
- for (auto match : matches_landmark_from_last_)
- {
- match.second.reset(); // TODO: Should we just remove the entries? What about match.first?
- }
- matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- // for (auto match : matches_landmark_from_last_)
- // std::cout << "\t" << match.first->id() << " to " << match.second.landmark_ptr_->id() << std::endl;
-}
-
-unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
-{
- /* Rationale: A keyFrame will be created using the last Capture.
- * First, we work on this Capture to detect new Features,
- * eventually create Landmarks with them,
- * and in such case create the new Factors feature-landmark.
- * When done, we need to track these new Features to the incoming Capture.
- * At the end, all new Features are appended to the lists of known Features in
- * the last and incoming Captures.
- */
-
- // We first need to populate the \b last Capture with new Features
- unsigned int n = detectNewFeatures(_max_features, new_features_last_);
-
- createNewLandmarks();
-
- // Find the new landmarks in incoming_ptr_ (if it's not nullptr)
- if (incoming_ptr_ != nullptr)
- {
- findLandmarks(new_landmarks_, new_features_incoming_, matches_landmark_from_incoming_);
-
- // Append all new Features to the Capture's list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
- }
-
- // Append all new Features to the Capture's list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
- // Append new landmarks to the map
- getProblem()->addLandmarkList(new_landmarks_);
-
- // return the number of new features detected in \b last
- return n;
-}
-
-void ProcessorTrackerLandmark::createNewLandmarks()
-{
- // First, make sure the list is empty and will only contain new lmks
- new_landmarks_.clear();
-
- // Create a landmark for each new feature in last Capture:
- for (auto new_feature_ptr : new_features_last_)
- {
- // create new landmark
- LandmarkBasePtr new_lmk_ptr = createLandmark(new_feature_ptr);
-
- new_landmarks_.push_back(new_lmk_ptr);
-
- // create new correspondence
- matches_landmark_from_last_[new_feature_ptr] = std::make_shared<LandmarkMatch>(new_lmk_ptr, 1); // max score
- }
-}
-
-unsigned int ProcessorTrackerLandmark::processKnown()
-{
- // Find landmarks in incoming_ptr_
- FeatureBasePtrList known_features_list_incoming;
- unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
- known_features_list_incoming, matches_landmark_from_incoming_);
- // Append found incoming features
- incoming_ptr_->addFeatureList(known_features_list_incoming);
-
- return n;
-}
-
-void ProcessorTrackerLandmark::establishFactors()
-{
- // Loop all features in last_ptr_
- for (auto last_feature : last_ptr_->getFeatureList())
- {
- auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
- FactorBasePtr fac_ptr = createFactor(last_feature,
- lmk);
- if (fac_ptr != nullptr) // factor links
- {
- last_feature->addFactor(fac_ptr);
- lmk->addConstrainedBy(fac_ptr);
- }
- }
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
deleted file mode 100644
index 24a59a5f535873ea0a7f7a8d8f8545fab9238092..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ /dev/null
@@ -1,431 +0,0 @@
-#include "base/processor/processor_tracker_landmark_corner.h"
-#include "base/rotations.h"
-
-namespace wolf
-{
-
-void ProcessorTrackerLandmarkCorner::preProcess()
-{
- // extract corners of incoming
- extractCorners(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), corners_incoming_);
-
- // compute transformations
- t_world_robot_ = getProblem()->getState(incoming_ptr_->getTimeStamp());
-
- // world_robot
- Eigen::Matrix3s R_world_robot = Eigen::Matrix3s::Identity();
- R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
- t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
- R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_.topLeftCorner<2, 2>() = R_world_robot.topLeftCorner<2, 2>() * R_robot_sensor_.topLeftCorner<2, 2>();
- t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_.topLeftCorner<2, 2>() = R_robot_sensor_.topLeftCorner<2, 2>().transpose() * R_world_robot.topLeftCorner<2, 2>().transpose();
- t_sensor_world_ = -R_sensor_world_ * t_world_robot_ - R_robot_sensor_.transpose() * t_robot_sensor_;
-
- //std::cout << "t_robot_sensor_" << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "t_world_robot_" << t_world_robot_.transpose() << std::endl;
- //std::cout << "t_world_sensor_" << t_world_sensor_.transpose() << std::endl;
- //std::cout << "PreProcess: incoming new features: " << corners_incoming_.size() << std::endl;
-}
-
-unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBasePtrList& _landmarks_corner_searched,
- FeatureBasePtrList& _features_corner_found,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- //std::cout << "ProcessorTrackerLandmarkCorner::findLandmarks: " << _landmarks_corner_searched.size() << " features: " << corners_incoming_.size() << std::endl;
-
- // NAIVE FIRST NEAREST NEIGHBOR MATCHING
- LandmarkBasePtrList not_matched_landmarks = _landmarks_corner_searched;
- Scalar dm2;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBasePtr, Eigen::Vector4s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Vector4s> > > expected_features;
- std::map<LandmarkBasePtr, Eigen::Matrix3s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Matrix3s> > > expected_features_covs;
- for (auto landmark : not_matched_landmarks)
- expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
-
- auto next_feature_it = corners_incoming_.begin();
- auto feature_it = next_feature_it++;
- while (feature_it != corners_incoming_.end())
- {
- LandmarkBaseIter closest_landmark = not_matched_landmarks.end();
- Scalar closest_dm2 = 1e3;
- for (auto landmark_it = not_matched_landmarks.begin(); landmark_it != not_matched_landmarks.end(); landmark_it++)
- {
- if ((*landmark_it)->getType() == "CORNER 2D" &&
- fabs(pi2pi((std::static_pointer_cast<FeatureCorner2D>(*feature_it))->getAperture() - (*landmark_it)->getDescriptor(0))) < aperture_error_th_)
- {
- dm2 = computeSquaredMahalanobisDistances((*feature_it), expected_features[*landmark_it],
- expected_features_covs[*landmark_it],
- Eigen::MatrixXs::Zero(3, 1))(0); //Mahalanobis squared
- if (dm2 < 0.5 && (closest_landmark == not_matched_landmarks.end() || dm2 < closest_dm2))
- {
- //std::cout << "pair feature " << (*feature_it)->id() << " & landmark " << (*landmark_it)->id() << std::endl;
- //std::cout << "pair SqMahalanobisDist = " << dm2 << std::endl;
- closest_dm2 = dm2;
- closest_landmark = landmark_it;
- }
- }
- }
- //std::cout << "all landmarks checked with feature " << (*feature_it)->id() << std::endl;
- if (closest_landmark != not_matched_landmarks.end())
- {
- //std::cout << "closest landmark: " << (*closest_landmark)->id() << std::endl;
- // match
- matches_landmark_from_incoming_[*feature_it] = std::make_shared<LandmarkMatch>(*closest_landmark, closest_dm2);
- // erase from the landmarks to be found
- not_matched_landmarks.erase(closest_landmark);
- // move corner feature to output list
- _features_corner_found.splice(_features_corner_found.end(), corners_incoming_, feature_it);
- }
- //else
- // std::cout << "no landmark close enough found." << std::endl;
- feature_it = next_feature_it++;
- }
-
-/*
- // MATCHING FROM MAP
- if (!corners_incoming_.empty() && !_landmarks_corner_searched.empty())
- {
- //local declarations
- Scalar prob, dm2;
- unsigned int ii, jj;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBasePtr, Eigen::Vector4s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Vector4s> > > expected_features;
- std::map<LandmarkBasePtr, Eigen::Matrix3s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Matrix3s> > > expected_features_covs;
- for (auto landmark : _landmarks_corner_searched)
- expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
- //std::cout << "expected features!" << std::endl;
-
- // SETTING ASSOCIATION TREE
- std::map<unsigned int, FeatureBaseIter> features_map;
- std::map<unsigned int, LandmarkBaseIter> landmarks_map;
- std::map<unsigned int, unsigned int> landmarks_index_map;
- //tree object allocation and sizing
- AssociationTree tree;
- tree.resize(corners_incoming_.size(), _landmarks_corner_searched.size());
- //set independent probabilities between feature-landmark pairs
- ii = 0;
- for (auto feature_it = corners_incoming_.begin(); feature_it != corners_incoming_.end();
- feature_it++, ii++) //ii runs over extracted feature
- {
- features_map[ii] = feature_it;
- //std::cout << "Feature: " << (*i_it)->id() << std::endl << (*i_it)->getMeasurement().head(3).transpose() << std::endl;
- jj = 0;
- for (auto landmark_it = _landmarks_corner_searched.begin(); landmark_it != _landmarks_corner_searched.end();
- landmark_it++, jj++)
- {
- if ((*landmark_it)->getType() == "CORNER 2D")
- {
- landmarks_map[jj] = landmark_it;
- landmarks_index_map[jj] = 0;
- //std::cout << "Landmark: " << (*j_it)->id() << " - jj: " << jj << std::endl;
- //If aperture difference is small enough, proceed with Mahalanobis distance. Otherwise Set prob to 0 to force unassociation
- if (fabs(
- pi2pi(((FeatureCorner2D*)(((*feature_it))))->getAperture()
- - (*landmark_it)->getDescriptor(0))) < aperture_error_th_)
- {
- dm2 = computeSquaredMahalanobisDistances(*feature_it, expected_features[*landmark_it],
- expected_features_covs[*landmark_it],
- Eigen::MatrixXs::Zero(3, 1))(0); //Mahalanobis squared
- //prob = (dm2 < 5 * 5 ? 5 * erfc(sqrt(dm2 / 2)) : 0); //prob = erfc( sqrt(dm2/2) ); //prob = erfc( sqrt(dm2)/1.4142136 );// sqrt(2) = 1.4142136
- prob = 10*erfc( sqrt(dm2/2) );
- if (dm2 < 0.5)
- {
- std::cout << "pair feature " << (*features_map[ii])->id() << " & landmark " << (*landmarks_map[jj])->id() << std::endl;
- std::cout << "pair SqMahalanobisDist = " << dm2 << std::endl;
- std::cout << "pair probability = " << prob << std::endl;
-
- }
- tree.setScore(ii, jj, prob);
- }
- else
- tree.setScore(ii, jj, 0.); //prob to 0
- }
- }
- }
- // Grows tree and make association pairs
- std::map<unsigned int, unsigned int> ft_lk_pairs;
- std::vector<bool> associated_mask;
- //std::cout << "solving tree" << std::endl;
- tree.solve(ft_lk_pairs, associated_mask);
- //print tree & score table
- //std::cout << "------------- TREE SOLVED ---------" << std::endl;
- //std::cout << corners_incoming_.size() << " new corners in incoming:" << std::endl;
- //for (auto new_feature : corners_incoming_)
- // std::cout << "\tcorner " << new_feature->id() << std::endl;
- std::cout << ft_lk_pairs.size() << " pairs:" << std::endl;
- for (auto pair : ft_lk_pairs)
- std::cout << "\tfeature " << (*features_map[pair.first])->id() << " & landmark " << (*landmarks_map[pair.second])->id() << std::endl;
- //tree.printTree();
- //tree.printScoreTable();
- // Vector of new landmarks to be created
- std::vector<FeatureCorner2D*> new_corner_landmarks(0);
-
- // ESTABLISH CORRESPONDENCES
- for (auto pair : ft_lk_pairs)
- {
- // match
- matches_landmark_from_incoming_[*features_map[pair.first]] = new LandmarkMatch(
- *landmarks_map[pair.second], tree.getScore(pair.first, pair.second));
- // move matched feature to list
- _features_corner_found.splice(_features_corner_found.end(), corners_incoming_, features_map[pair.first]);
- }
- //std::cout << corners_incoming_.size() << " remaining new corners in incoming:" << std::endl;
- //for (auto new_feature : corners_incoming_)
- // std::cout << "\tcorner " << new_feature->id() << std::endl;
- }
- else
- std::cout << "0 corners incoming or 0 landmarks to search" <<std::endl;
-*/
- return matches_landmark_from_incoming_.size();
-}
-
-bool ProcessorTrackerLandmarkCorner::voteForKeyFrame()
-{
- // option 1: more than TH new features in last
- if (corners_last_.size() >= new_corners_th_)
- {
- std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl;
- //std::cout << "\tnew features in last = " << corners_last_.size() << std::endl;
- return true;
- }
- // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
- for (auto new_feat : new_features_last_)
- {
- if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_feat]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > loop_frames_th_)
- {
- std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
- return true;
- }
- }
- //// option 3: less than half matched in origin, matched in incoming (more than half in last)
- //if (matches_landmark_from_incoming_.size()*2 < origin_ptr_->getFeatureList().size() && matches_landmark_from_last_.size()*2 > origin_ptr_->getFeatureList().size())
- //{
- // std::cout << "------------- NEW KEY FRAME: Option 3 - " << std::endl;
- // //std::cout << "\tmatches in incoming = " << matches_landmark_from_incoming_.size() << std::endl<< "\tmatches in origin = " << origin_ptr_->getFeatureList().size() << std::endl;
- // return true;
- //}
- return false;
-}
-
-void ProcessorTrackerLandmarkCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBasePtrList& _corner_list)
-{
- // TODO: sort corners by bearing
- std::list<laserscanutils::CornerPoint> corners;
-
- corner_finder_.findCorners(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), line_finder_, corners);
-
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head<2>() = corner.point_.head<2>();
- measurement(2)=corner.orientation_;
- measurement(3)=corner.aperture_;
-
- _corner_list.push_back(std::make_shared<FeatureCorner2D>(measurement, corner.covariance_));
-
- //std::cout << "new corner detected: " << measurement.head<3>().transpose() << std::endl;
- }
-
-/* //variables
- std::list<laserscanutils::Corner> corners;
- //extract corners from range data
- laserscanutils::extractCorners(scan_params_, corner_alg_params_, _capture_laser_ptr->getRanges(), corners);
- //std::cout << corners.size() << " corners extracted" << std::endl;
- Eigen::Matrix4s measurement_cov;
- Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head(2) = corner.pt_.head(2);
- measurement(2) = corner.orientation_;
- measurement(3) = corner.aperture_;
- // TODO: maybe in line object?
- Scalar L1 = corner.line_1_.length();
- Scalar L2 = corner.line_2_.length();
- Scalar cov_angle_line1 = 12 * corner.line_1_.error_
- / (pow(L1, 2) * (pow(corner.line_1_.np_, 3) - pow(corner.line_1_.np_, 2)));
- Scalar cov_angle_line2 = 12 * corner.line_2_.error_
- / (pow(L2, 2) * (pow(corner.line_2_.np_, 3) - pow(corner.line_2_.np_, 2)));
- //init cov in corner coordinates
- measurement_cov << corner.line_1_.error_ + cov_angle_line1 * L1 * L1 / 4, 0, 0, 0, 0, corner.line_2_.error_
- + cov_angle_line2 * L2 * L2 / 4, 0, 0, 0, 0, cov_angle_line1 + cov_angle_line2, 0, 0, 0, 0, cov_angle_line1
- + cov_angle_line2;
- measurement_cov = 10 * measurement_cov;
- //std::cout << "New feature: " << meas.transpose() << std::endl;
- //std::cout << measurement_cov << std::endl;
- // Rotate covariance
- R.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(corner.orientation_).matrix();
- measurement_cov.topLeftCorner<3, 3>() = R.transpose() * measurement_cov.topLeftCorner<3, 3>() * R;
- //std::cout << "rotated covariance: " << std::endl;
- //std::cout << measurement_cov << std::endl;
- _corner_list.push_back(new FeatureCorner2D(measurement, measurement_cov));
- }*/
-}
-
-void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Vector4s& expected_feature_,
- Eigen::Matrix3s& expected_feature_cov_)
-{
- //std::cout << "ProcessorTrackerLandmarkCorner::expectedFeature: " << std::endl;
- //std::cout << "Landmark global pose: " << _landmark_ptr->getP()->getVector().transpose() << " " << _landmark_ptr->getO()->getVector() << std::endl;
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
-
- // ------------ expected feature measurement
- expected_feature_.head<2>() = R_sensor_world_.topLeftCorner<2,2>() * (_landmark_ptr->getP()->getState() - t_world_sensor_.head<2>());
- expected_feature_(2) = _landmark_ptr->getO()->getState()(0) - t_world_sensor_(2);
- expected_feature_(3) = (std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr))->getAperture();
- //std::cout << "Expected feature pose: " << expected_feature_.head<3>().transpose() << std::endl;
-
- // ------------ expected feature covariance
- Eigen::MatrixXs Sigma = Eigen::MatrixXs::Zero(6, 6);
- // closer keyframe with computed covariance
- FrameBasePtr key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFrame() : nullptr);
- // If all covariance blocks are stored wolfproblem (filling upper diagonal only)
- if (key_frame_ptr != nullptr &&
- // Sigma_rr
- getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getP(), Sigma, 3, 3) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getO(), Sigma, 3, 5) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getO(), key_frame_ptr->getO(), Sigma, 5, 5) &&
- // Sigma_ll
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getP(), Sigma, 0, 0) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getO(), Sigma, 0, 2) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getO(), _landmark_ptr->getO(), Sigma, 2, 2) &&
- // Sigma_lr
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getP(), Sigma, 0, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getO(), Sigma, 0, 5) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getP(), Sigma, 2, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getO(), Sigma, 2, 5) )
- {
- // Jacobian
- Eigen::Vector2s p_robot_landmark = t_world_robot_.head<2>() - _landmark_ptr->getP()->getState();
- Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero();
- Jlr.block<3, 3>(0, 3) = -R_world_sensor_.transpose();
- Jlr.block<3, 3>(0, 3) = R_world_sensor_.transpose();
- Jlr(0, 2) = -p_robot_landmark(0) * sin(t_world_sensor_(2)) + p_robot_landmark(1) * cos(t_world_sensor_(2));
- Jlr(1, 2) = -p_robot_landmark(0) * cos(t_world_sensor_(2)) - p_robot_landmark(1) * sin(t_world_sensor_(2));
- // measurement covariance
- expected_feature_cov_ = Jlr * Sigma.selfadjointView<Eigen::Upper>() * Jlr.transpose();
- }
- else
- // Any covariance block is not stored in wolfproblem -> Identity()
- expected_feature_cov_ = Eigen::Matrix3s::Identity()*0.1;
-}
-
-Eigen::VectorXs ProcessorTrackerLandmarkCorner::computeSquaredMahalanobisDistances(const FeatureBasePtr _feature_ptr,
- const Eigen::Vector4s& _expected_feature,
- const Eigen::Matrix3s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
-{
- const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2);
- const Scalar& o_feature = _feature_ptr->getMeasurement()(2);
- // ------------------------ d
- Eigen::Vector3s d;
- d.head(2) = p_feature - _expected_feature.head(2);
- d(2) = pi2pi(o_feature - _expected_feature(2));
- // ------------------------ Sigma_d
- Eigen::Matrix3s iSigma_d =
- (_feature_ptr->getMeasurementCovariance().topLeftCorner<3, 3>() + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
- for (unsigned int i = 0; i < _mu.cols(); i++)
- {
- squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
- //if ((d - _mu.col(i)).norm() < 1)
- //{
- // std::cout << "distance: " << (d - _mu.col(i)).norm() << std::endl;
- // std::cout << "iSigma_d: " << std::endl << iSigma_d << std::endl;
- // std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl;
- //}
- }
-
- return squared_mahalanobis_distances;
-}
-
-ProcessorBasePtr ProcessorTrackerLandmarkCorner::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
-{
- ProcessorParamsLaserPtr params = std::static_pointer_cast<ProcessorParamsLaser>(_params);
- ProcessorTrackerLandmarkCornerPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkCorner>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _feature_ptr)
-{
- //std::cout << "ProcessorTrackerLandmarkCorner::createLandmark" << std::endl;
- // compute feature global pose
- Eigen::Vector3s feature_global_pose = R_world_sensor_ * _feature_ptr->getMeasurement().head<3>() + t_world_sensor_;
- // Create new landmark
- return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(feature_global_pose.head(2)),
- std::make_shared<StateBlock>(feature_global_pose.tail(1)),
- _feature_ptr->getMeasurement()(3));
-}
-
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
-{
- // in corners_last_ remain all not tracked corners, already computed in preprocess()
- _features_last_out = std::move(corners_last_);
- if (_max_features != -1 && _features_last_out.size() > _max_features)
- _features_last_out.resize(_max_features);
-
- return _features_last_out.size();
-}
-
-FactorBasePtr ProcessorTrackerLandmarkCorner::createFactor(FeatureBasePtr _feature_ptr,
- LandmarkBasePtr _landmark_ptr)
-{
- assert(_feature_ptr != nullptr && _landmark_ptr != nullptr
- && "ProcessorTrackerLandmarkCorner::createFactor: feature and landmark pointers can not be nullptr!");
- return std::make_shared<FactorCorner2D>(_feature_ptr,
- std::static_pointer_cast<LandmarkCorner2D>((_landmark_ptr)),
- shared_from_this());
-}
-
-ProcessorTrackerLandmarkCorner::~ProcessorTrackerLandmarkCorner()
-{
- while (!corners_last_.empty())
- {
- corners_last_.front()->remove();
- corners_last_.pop_front(); // TODO check if this is needed
- }
- while (!corners_incoming_.empty())
- {
- corners_incoming_.front()->remove();
- corners_incoming_.pop_front(); // TODO check if this is needed
- }
-}
-
-ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(ProcessorParamsLaserPtr _params_laser) :
- ProcessorTrackerLandmark("TRACKER LANDMARK CORNER", _params_laser),
- line_finder_(_params_laser->line_finder_params_),
- new_corners_th_(_params_laser->new_corners_th),
- loop_frames_th_(_params_laser->loop_frames_th),
- R_sensor_world_(Eigen::Matrix3s::Identity()),
- R_world_sensor_(Eigen::Matrix3s::Identity()),
- R_robot_sensor_(Eigen::Matrix3s::Identity()),
- extrinsics_transformation_computed_(false)
-{
- //
-}
-
-} //namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK CORNER", ProcessorTrackerLandmarkCorner)
-} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
deleted file mode 100644
index 07c47786a1477a434763f394455ca039a826d32a..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ /dev/null
@@ -1,102 +0,0 @@
-/**
- * \file processor_tracker_landmark_dummy.cpp
- *
- * Created on: Apr 12, 2016
- * \author: jvallve
- */
-
-#include "base/processor/processor_tracker_landmark_dummy.h"
-#include "base/landmark/landmark_corner_2D.h"
-#include "base/factor/factor_corner_2D.h"
-
-namespace wolf
-{
-
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark),
- n_feature_(0),
- landmark_idx_non_visible_(0)
-{
- //
-
-}
-
-ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
-{
- //
-}
-
-unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks" << std::endl;
- std::cout << "\t\t" << _landmarks_in.size() << " landmarks..." << std::endl;
-
- // loosing the track of the first 2 features
- auto landmarks_lost = 0;
- for (auto landmark_in_ptr : _landmarks_in)
- {
- if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_)
- {
- landmarks_lost++;
- std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " lost!" << std::endl;
- }
- else
- {
- _features_incoming_out.push_back(std::make_shared<FeatureBase>(
- "POINT IMAGE",
- landmark_in_ptr->getDescriptor(),
- Eigen::MatrixXs::Identity(1,1)));
- _feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
- std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl;
- }
- }
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
-{
- std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
- bool vote = incoming_ptr_->getFeatureList().size() < 4;
- std::cout << (vote ? "Vote ": "Not vote ") << "for KF" << std::endl;
- return incoming_ptr_->getFeatureList().size() < 4;
-}
-
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
-{
- std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
-
- unsigned int max_features = _max_features;
-
- if (max_features == -1)
- {
- max_features = 10;
- WOLF_INFO("max_features unlimited, setting it to " , max_features);
- }
-
- // detecting new features
- for (unsigned int i = 1; i <= max_features; i++)
- {
- n_feature_++;
- _features_last_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE", n_feature_ * Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
- std::cout << "\t\tfeature " << _features_last_out.back()->getMeasurement() << " detected!" << std::endl;
- }
- return _features_last_out.size();
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
-{
- //std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
- return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), _feature_ptr->getMeasurement(0));
-}
-
-FactorBasePtr ProcessorTrackerLandmarkDummy::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
- std::cout << "\tProcessorTrackerLandmarkDummy::createFactor" << std::endl;
- std::cout << "\t\tfeature " << _feature_ptr->getMeasurement() << std::endl;
- std::cout << "\t\tlandmark "<< _landmark_ptr->getDescriptor() << std::endl;
- return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr), shared_from_this());
-}
-
-} //namespace wolf
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
deleted file mode 100644
index 967319385bd8e14adbe5e1e665d2a8232a308c2f..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ /dev/null
@@ -1,508 +0,0 @@
-#include "base/processor/processor_tracker_landmark_image.h"
-
-#include "base/capture/capture_image.h"
-#include "base/factor/factor_AHP.h"
-#include "base/feature/feature_base.h"
-#include "base/feature/feature_point_image.h"
-#include "base/frame_base.h"
-#include "base/logging.h"
-#include "base/map_base.h"
-#include "base/pinhole_tools.h"
-#include "base/problem.h"
-#include "base/sensor/sensor_camera.h"
-#include "base/state_block.h"
-#include "base/time_stamp.h"
-
-// vision_utils
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
-
-#include <Eigen/Geometry>
-#include <iomanip> //setprecision
-
-namespace wolf
-{
-
-ProcessorTrackerLandmarkImage::ProcessorTrackerLandmarkImage(ProcessorParamsTrackerLandmarkImagePtr _params_tracker_landmark_image) :
- ProcessorTrackerLandmark("IMAGE LANDMARK", _params_tracker_landmark_image),
- cell_width_(0),
- cell_height_(0),
- params_tracker_landmark_image_(_params_tracker_landmark_image),
- n_feature_(0),
- landmark_idx_non_visible_(0)
-{
- // Detector
- std::string det_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_landmark_image_->yaml_file_params_vision_utils);
-
- if (det_name.compare("ORB") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
- else if (det_name.compare("FAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorFAST>(det_ptr_);
- else if (det_name.compare("SIFT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSIFT>(det_ptr_);
- else if (det_name.compare("SURF") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSURF>(det_ptr_);
- else if (det_name.compare("BRISK") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorBRISK>(det_ptr_);
- else if (det_name.compare("MSER") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorMSER>(det_ptr_);
- else if (det_name.compare("GFTT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorGFTT>(det_ptr_);
- else if (det_name.compare("HARRIS") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorHARRIS>(det_ptr_);
- else if (det_name.compare("SBD") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSBD>(det_ptr_);
- else if (det_name.compare("KAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorKAZE>(det_ptr_);
- else if (det_name.compare("AKAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAKAZE>(det_ptr_);
- else if (det_name.compare("AGAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
-
- // Descriptor
- std::string des_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_landmark_image_->yaml_file_params_vision_utils);
-
- if (des_name.compare("ORB") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
- else if (des_name.compare("SIFT") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSIFT>(des_ptr_);
- else if (des_name.compare("SURF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSURF>(des_ptr_);
- else if (des_name.compare("BRISK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRISK>(des_ptr_);
- else if (des_name.compare("KAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorKAZE>(des_ptr_);
- else if (des_name.compare("AKAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorAKAZE>(des_ptr_);
- else if (des_name.compare("LATCH") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLATCH>(des_ptr_);
- else if (des_name.compare("FREAK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorFREAK>(des_ptr_);
- else if (des_name.compare("BRIEF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRIEF>(des_ptr_);
- else if (des_name.compare("DAISY") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorDAISY>(des_ptr_);
- else if (des_name.compare("LUCID") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
-
- // Matcher
- std::string mat_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_landmark_image_->yaml_file_params_vision_utils);
-
- if (mat_name.compare("FLANNBASED") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_L1") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_L1>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING_2") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
-
- // Active search grid
- vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_tracker_landmark_image_->yaml_file_params_vision_utils);
- active_search_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmACTIVESEARCH>(alg_ptr);
-}
-
-ProcessorTrackerLandmarkImage::~ProcessorTrackerLandmarkImage()
-{
- //
-}
-
-void ProcessorTrackerLandmarkImage::configure(SensorBasePtr _sensor)
-{
- SensorCameraPtr camera(std::static_pointer_cast<SensorCamera>(_sensor));
- image_.width_ = camera->getImgWidth();
- image_.height_ = camera->getImgHeight();
-
- active_search_ptr_->initAlg(camera->getImgWidth(), camera->getImgHeight(), det_ptr_->getPatternRadius() );
-
- params_tracker_landmark_image_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
-
- cell_width_ = image_.width_ / params_tracker_landmark_image_activesearch_ptr_->n_cells_h;
- cell_height_ = image_.height_ / params_tracker_landmark_image_activesearch_ptr_->n_cells_v;
-}
-
-void ProcessorTrackerLandmarkImage::preProcess()
-{
- image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_)->getImage();
-
- active_search_ptr_->renew();
-
- detector_roi_.clear();
- feat_lmk_found_.clear();
-}
-
-void ProcessorTrackerLandmarkImage::postProcess()
-{
- detector_roi_.clear();
- feat_lmk_found_.clear();
-}
-
-unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- KeyPointVector candidate_keypoints;
- cv::Mat candidate_descriptors;
- DMatchVector cv_matches;
-
- Eigen::VectorXs current_state = getProblem()->getState(incoming_ptr_->getTimeStamp());
-
- for (auto landmark_in_ptr : _landmarks_in)
- {
-
- // project landmark into incoming capture
- LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_in_ptr);
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(this->getSensor());
- Eigen::Vector4s point3D_hmg;
- Eigen::Vector2s pixel;
-
- landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
-
- pixel = pinhole::projectPoint(camera->getIntrinsic()->getState(),
- camera->getDistortionVector(),
- point3D_hmg.head<3>());
-
- if(pinhole::isInImage(pixel, image_.width_, image_.height_))
- {
- cv::Rect roi = vision_utils::setRoi(pixel[0], pixel[1], cell_width_, cell_height_);
-
- active_search_ptr_->hitCell(pixel);
-
- cv::Mat target_descriptor = landmark_ptr->getCvDescriptor();
-
- if (detect(image_incoming_, roi, candidate_keypoints, candidate_descriptors))
- {
- Scalar normalized_score = match(target_descriptor,candidate_descriptors,cv_matches);
-
- if (normalized_score > mat_ptr_->getParams()->min_norm_score )
- {
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_matches[0].trainIdx],
- cv_matches[0].trainIdx,
- candidate_descriptors.row(cv_matches[0].trainIdx),
- Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
-
- incoming_point_ptr->setTrackId(landmark_in_ptr->id());
- incoming_point_ptr->setLandmarkId(landmark_in_ptr->id());
- incoming_point_ptr->setScore(normalized_score);
- incoming_point_ptr->setExpectation(pixel);
-
- _features_incoming_out.push_back(incoming_point_ptr);
-
- _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score);
-
- feat_lmk_found_.push_back(incoming_point_ptr);
-
- // To visualize
- cv::Rect roi2 = roi;
- vision_utils::trimRoi(image_.width_, image_.height_, roi2);
- incoming_point_ptr->setTrackerRoi(roi2);
- }
-// else
-// std::cout << "NOT FOUND" << std::endl;
- }
-// else
-// std::cout << "NOT DETECTED/FOUND" << std::endl;
- }
-// else
-// std::cout << "NOT in the image" << std::endl;
- }
-// std::cout << "\tNumber of Features tracked: " << _features_incoming_out.size() << std::endl;
- landmarks_tracked_ = _features_incoming_out.size();
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
-{
- return false;
-// return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
-}
-
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
-{
- cv::Rect roi;
- KeyPointVector new_keypoints;
- cv::Mat new_descriptors;
- cv::KeyPointsFilter keypoint_filter;
- unsigned int n_new_features = 0;
-
- for (unsigned int n_iterations = 0; _max_features == -1 || n_iterations < _max_features; n_iterations++)
- {
- if (active_search_ptr_->pickEmptyRoi(roi))
- {
- detector_roi_.push_back(roi);
- if (detect(image_last_, roi, new_keypoints, new_descriptors))
- {
- KeyPointVector list_keypoints = new_keypoints;
- unsigned int index = 0;
- keypoint_filter.retainBest(new_keypoints,1);
- for(unsigned int i = 0; i < list_keypoints.size(); i++)
- {
- if(list_keypoints[i].pt == new_keypoints[0].pt)
- index = i;
- }
-
- if(new_keypoints[0].response > params_tracker_landmark_image_activesearch_ptr_->min_response_new_feature)
- {
- list_response_.push_back(new_keypoints[0].response);
- FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
- new_keypoints[0],
- 0,
- new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
- point_ptr->setIsKnown(false);
- point_ptr->setTrackId(point_ptr->id());
- point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y));
- _features_last_out.push_back(point_ptr);
-
- active_search_ptr_->hitCell(new_keypoints[0]);
-
- n_new_features++;
- }
-
- }
- else
- active_search_ptr_->blockCell(roi);
- }
- else
- break;
- }
-
- WOLF_DEBUG( "Number of new features detected: " , n_new_features );
- return n_new_features;
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _feature_ptr)
-{
-
- FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
- FrameBasePtr anchor_frame = getLast()->getFrame();
-
- Eigen::Vector2s point2D;
- point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
- point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
-
- Scalar distance = params_tracker_landmark_image_->distance; // arbitrary value
- Eigen::Vector4s vec_homogeneous;
-
- point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2D);
-
- Eigen::Vector3s point3D;
- point3D.head<2>() = point2D;
- point3D(2) = 1;
-
- point3D.normalize();
-
- vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
-
- LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensor(), feat_point_image_ptr->getDescriptor());
- _feature_ptr->setLandmarkId(lmk_ahp_ptr->id());
- return lmk_ahp_ptr;
-}
-
-FactorBasePtr ProcessorTrackerLandmarkImage::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
-
- if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFrame())
- {
- return FactorBasePtr();
- }
- else
- {
- assert (last_ptr_ && "bad last ptr");
- assert (_landmark_ptr && "bad lmk ptr");
-
- LandmarkAHPPtr landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
-
- FactorAHPPtr factor_ptr = FactorAHP::create(_feature_ptr, landmark_ahp, shared_from_this(), true);
-
- return factor_ptr;
- }
-}
-
-// ==================================================================== My own functions
-
-void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorXs& _current_state,
- const LandmarkAHPPtr _landmark,
- Eigen::Vector4s& _point3D_hmg)
-{
- using namespace Eigen;
-
- /*
- * Rationale: we transform the landmark from anchor camera to current camera:
- *
- * C0 ---> R0 ---> W ---> R1 ---> C1
- *
- * where
- * '0' is 'anchor'
- * '1' is 'current',
- * 'R' is 'robot'
- * 'C' is 'camera'
- * 'W' is 'world',
- *
- * by concatenating the individual transforms:
- *
- * T_W_R0,
- * T_W_R1,
- * T_R0_C0,
- * T_R1_C1
- *
- * We use Eigen::Transform which is like using homogeneous transform matrices with a simpler API
- */
-
- // Assert frame is 3D with at least PQ
- assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU.");
-
- // ALL TRANSFORMS
- Transform<Scalar,3,Eigen::Affine> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
-
- // world to anchor robot frame
- Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getP()->getState()); // sadly we cannot put a Map over a translation
- const Quaternions q_w_r0(Eigen::Vector4s(_landmark->getAnchorFrame()->getO()->getState()));
- T_W_R0 = t_w_r0 * q_w_r0;
-
- // world to current robot frame
- Translation<Scalar,3> t_w_r1(_current_state.head<3>());
- Map<const Quaternions> q_w_r1(_current_state.data() + 3);
- T_W_R1 = t_w_r1 * q_w_r1;
-
- // anchor robot to anchor camera
- Translation<Scalar,3> t_r0_c0(_landmark->getAnchorSensor()->getP()->getState());
- const Quaternions q_r0_c0(Eigen::Vector4s(_landmark->getAnchorSensor()->getO()->getState()));
- T_R0_C0 = t_r0_c0 * q_r0_c0;
-
- // current robot to current camera
- Translation<Scalar,3> t_r1_c1(this->getSensor()->getP()->getState());
- const Quaternions q_r1_c1(Eigen::Vector4s(this->getSensor()->getO()->getState()));
- T_R1_C1 = t_r1_c1 * q_r1_c1;
-
- // Transform lmk from c0 to c1 and exit
- Vector4s landmark_hmg_c0 = _landmark->getP()->getState(); // lmk in anchor frame
- _point3D_hmg = T_R1_C1.inverse(Eigen::Affine) * T_W_R1.inverse(Eigen::Affine) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
-}
-
-Scalar ProcessorTrackerLandmarkImage::match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
-{
- std::vector<Scalar> normalized_scores = mat_ptr_->match(_target_descriptor, _candidate_descriptors, des_ptr_->getSize(), _cv_matches);
- return normalized_scores[0];
-}
-
-unsigned int ProcessorTrackerLandmarkImage::detect(const cv::Mat _image, cv::Rect& _roi, KeyPointVector& _new_keypoints, cv::Mat& new_descriptors)
-{
- _new_keypoints = det_ptr_->detect(_image, _roi);
- new_descriptors = des_ptr_->getDescriptor(_image, _new_keypoints);
- return _new_keypoints.size();
-}
-
-void ProcessorTrackerLandmarkImage::drawTrackerRoi(cv::Mat _image, cv::Scalar _color)
-{
- CaptureImagePtr _capture = std::static_pointer_cast<CaptureImage>(last_ptr_);
- if (last_ptr_!=nullptr)
- {
- for (auto feature : _capture->getFeatureList())
- cv::rectangle(_image, std::static_pointer_cast<FeaturePointImage>(feature)->getTrackerRoi(), _color, 1, 8, 0);
- }
-}
-
-void ProcessorTrackerLandmarkImage::drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color)
-{
- if (last_ptr_!=nullptr)
- {
- for (auto roi : _roi_list)
- cv::rectangle(_image, roi, _color, 1, 8, 0);
- }
-}
-
-void ProcessorTrackerLandmarkImage::drawFeaturesFromLandmarks(cv::Mat _image)
-{
- if (last_ptr_!=nullptr)
- {
- FeaturePointImagePtr ftr;
- for(auto feature_point : feat_lmk_found_)
- {
- ftr = std::static_pointer_cast<FeaturePointImage>(feature_point);
-
- cv::Point2f point = ftr->getKeypoint().pt;
- cv::circle(_image, point, 2, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0);
-
- cv::Point2f point2 = point;
- point2.x = point2.x - 16;
- cv::putText(_image, std::to_string(ftr->landmarkId()) + "/" + std::to_string((int)(100*ftr->getScore())), point2,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 255.0, 0.0));
- }
- }
-}
-
-void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
-{
- if (last_ptr_!=nullptr)
- {
- unsigned int num_lmks_in_img = 0;
-
- Eigen::VectorXs current_state = last_ptr_->getFrame()->getState();
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
-
- for (auto landmark_base_ptr : getProblem()->getMap()->getLandmarkList())
- {
- LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_base_ptr);
-
- Eigen::Vector4s point3D_hmg;
- landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
-
- Eigen::Vector2s point2D = pinhole::projectPoint(camera->getIntrinsic()->getState(), // k
- camera->getDistortionVector(), // d
- point3D_hmg.head(3)); // v
-
- if(pinhole::isInImage(point2D,image_.width_,image_.height_))
- {
- num_lmks_in_img++;
-
- cv::Point2f point;
- point.x = point2D[0];
- point.y = point2D[1];
-
- cv::circle(_image, point, 4, cv::Scalar(51.0, 51.0, 255.0), 1, 3, 0);
- cv::putText(_image, std::to_string(landmark_ptr->id()), point, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(100.0, 100.0, 255.0) );
- }
- }
- cv::Point label_for_landmark_point;
- label_for_landmark_point.x = 3;
- label_for_landmark_point.y = 10;
- cv::putText(_image, std::to_string(landmarks_tracked_), label_for_landmark_point,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 0.0, 255.0));
-
- cv::Point label_for_landmark_point2;
- label_for_landmark_point2.x = 3;
- label_for_landmark_point2.y = 20;
- cv::putText(_image, std::to_string(num_lmks_in_img), label_for_landmark_point2,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 0.0, 255.0));
-
-// std::cout << "\t\tTotal landmarks: " << counter << std::endl;
- }
-}
-
-//namespace wolf{
-
-ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
-{
- ProcessorTrackerLandmarkImagePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkImage>(std::static_pointer_cast<ProcessorParamsTrackerLandmarkImage>(_params));
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("IMAGE LANDMARK", ProcessorTrackerLandmarkImage)
-} // namespace wolf
-
diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt
deleted file mode 100644
index 661731d27c433e5b0d9e3c14a81d6c0dd0ac6737..0000000000000000000000000000000000000000
--- a/src/sensor/CMakeLists.txt
+++ /dev/null
@@ -1,27 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_SENSOR ${HDRS_SENSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
- ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
- ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h
- )
-
-SET(SRCS_SENSOR ${SRCS_SENSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_SENSOR ${HDRS_SENSOR} PARENT_SCOPE)
-SET(SRCS_SENSOR ${SRCS_SENSOR} PARENT_SCOPE)
-
\ No newline at end of file
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
deleted file mode 100644
index 323616be956041905389ad5b083c85c11af3707e..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_GPS.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#include "base/sensor/sensor_GPS.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPS::SensorGPS(StateBlockPtr _p_ptr, //GPS sensor position with respect to the car frame (base frame)
- StateBlockPtr _o_ptr, //GPS sensor orientation with respect to the car frame (base frame)
- StateBlockPtr _bias_ptr, //GPS sensor time bias
- StateBlockPtr _map_p_ptr, //initial position of vehicle's frame with respect to starting point frame
- StateBlockPtr _map_o_ptr) //initial orientation of vehicle's frame with respect to the starting point frame
- :
- SensorBase("GPS", _p_ptr, _o_ptr, _bias_ptr, 0)
-{
- getStateBlockVec().resize(5);
- setStateBlockPtrStatic(3, _map_p_ptr); // Map position
- setStateBlockPtrStatic(4, _map_o_ptr); // Map orientation
- //
-}
-
-SensorGPS::~SensorGPS()
-{
- //
-}
-
-StateBlockPtr SensorGPS::getMapP() const
-{
- return getStateBlockPtrStatic(3);
-}
-
-StateBlockPtr SensorGPS::getMapO() const
-{
- return getStateBlockPtrStatic(4);
-}
-
-// Define the factory method
-SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_p.size() == 3 && "Bad extrinsics vector length. Should be 3 for 3D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_p, true);
- StateBlockPtr ori_ptr = nullptr;
- SensorBasePtr sen = std::make_shared<SensorGPS>(pos_ptr, ori_ptr, nullptr, nullptr, nullptr);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS",SensorGPS)
-} // namespace wolf
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
deleted file mode 100644
index c69f99b6a2254b277dc250fed85b3f2105159db4..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_GPS_fix.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-#include "base/sensor/sensor_GPS_fix.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
- SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), nullptr, _intrinsics.noise_std)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
-}
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) :
- SensorGPSFix(_extrinsics, *_intrinsics_ptr)
-{
- //
-}
-
-SensorGPSFix::~SensorGPSFix()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
- const IntrinsicsBasePtr _intrinsics)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
- SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
- sen->getP()->fix();
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS FIX", SensorGPSFix)
-} // namespace wolf
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
deleted file mode 100644
index f97edf645e607b08534ca421db1bd09070bf8d70..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_IMU.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "base/sensor/sensor_IMU.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-
-namespace wolf {
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) :
- SensorIMU(_extrinsics, *_params)
-{
- //
-}
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) :
- SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
- a_noise(_params.a_noise),
- w_noise(_params.w_noise),
- ab_initial_stdev(_params.ab_initial_stdev),
- wb_initial_stdev(_params.wb_initial_stdev),
- ab_rate_stdev(_params.ab_rate_stdev),
- wb_rate_stdev(_params.wb_rate_stdev)
-{
- assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
-}
-
-SensorIMU::~SensorIMU()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
- IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
- SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("IMU", SensorIMU)
-} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
deleted file mode 100644
index a30f777fc109da531c0d56237a650ccad0090bf5..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_base.cpp
+++ /dev/null
@@ -1,407 +0,0 @@
-#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-#include "base/factor/factor_block_absolute.h"
-#include "base/factor/factor_quaternion_absolute.h"
-
-
-namespace wolf {
-
-unsigned int SensorBase::sensor_id_count_ = 0;
-
-SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const unsigned int _noise_size,
- const bool _extr_dyn,
- const bool _intr_dyn) :
- NodeBase("SENSOR", _type),
- hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- calib_size_(0),
- sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
- noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size)
-{
- noise_std_.setZero();
- noise_cov_.setZero();
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
- updateCalibSize();
-}
-
-SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn,
- const bool _intr_dyn) :
- NodeBase("SENSOR", _type),
- hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- calib_size_(0),
- sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
- noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size())
-{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
- setNoiseStd(_noise_std);
- updateCalibSize();
-}
-
-SensorBase::~SensorBase()
-{
- // Remove State Blocks
- removeStateBlocks();
-}
-
-void SensorBase::removeStateBlocks()
-{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr && !extrinsic_dynamic_)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlockPtrStatic(i, nullptr);
- }
- }
-}
-
-void SensorBase::fix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
- updateCalibSize();
-}
-
-void SensorBase::unfix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
- updateCalibSize();
-}
-
-void SensorBase::fixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void SensorBase::unfixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void SensorBase::fixIntrinsics()
-{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void SensorBase::unfixIntrinsics()
-{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
-{
- assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
- assert(_i < state_block_vec_.size() && "State block not found");
-
- StateBlockPtr _sb = getStateBlockPtrStatic(_i);
- bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
-
- assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
- (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
- assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
- assert(_size == -1 || _size == _x.size());
- assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion");
-
- // set StateBlock state
- if (_size == -1)
- _sb->setState(_x);
- else
- {
- Eigen::VectorXs new_x = _sb->getState();
- new_x.segment(_start_idx,_size) = _x;
- _sb->setState(new_x);
- }
-
- // remove previous prior (if any)
- if (params_prior_map_.find(_i) != params_prior_map_.end())
- params_prior_map_[_i]->remove();
-
- // create feature
- FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
-
- // set feature problem
- ftr_prior->setProblem(getProblem());
-
- // create & add factor absolute
- if (is_quaternion)
- ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
- else
- ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
-
- // store feature in params_prior_map_
- params_prior_map_[_i] = ftr_prior;
-}
-
-void SensorBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
- {
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
- {
- if (i < 2 && !isExtrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
- if (i >= 2 && !isIntrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
- }
- }
-}
-
-void SensorBase::setNoiseStd(const Eigen::VectorXs& _noise_std) {
- noise_std_ = _noise_std;
- noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
-}
-
-void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
- noise_std_ = _noise_cov.diagonal().array().sqrt();
- noise_cov_ = _noise_cov;
-}
-
-CaptureBasePtr SensorBase::lastKeyCapture(void)
-{
- // we search for the most recent Capture of this sensor which belongs to a KeyFrame
- CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
- {
- if ((*frame_rev_it)->isKey())
- {
- capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
- }
- frame_rev_it++;
- }
- return capture;
-}
-
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
-{
- // we search for the most recent Capture of this sensor before _ts
- CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-
- // We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
- {
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
- {
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
- }
- frame_rev_it++;
- }
- return capture;
-}
-
-StateBlockPtr SensorBase::getP(const TimeStamp _ts)
-{
- return getStateBlock(0, _ts);
-}
-
-StateBlockPtr SensorBase::getO(const TimeStamp _ts)
-{
- return getStateBlock(1, _ts);
-}
-
-StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
-{
- return getStateBlock(2, _ts);
-}
-
-StateBlockPtr SensorBase::getP()
-{
- return getStateBlock(0);
-}
-
-StateBlockPtr SensorBase::getO()
-{
- return getStateBlock(1);
-}
-
-StateBlockPtr SensorBase::getIntrinsic()
-{
- return getStateBlock(2);
-}
-
-SizeEigen SensorBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sb = state_block_vec_[i];
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXs SensorBase::getCalibration() const
-{
- SizeEigen index = 0;
- SizeEigen sz = getCalibSize();
- Eigen::VectorXs calib(sz);
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sb = getStateBlockPtrStatic(i);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
-bool SensorBase::process(const CaptureBasePtr capture_ptr)
-{
- capture_ptr->setSensor(shared_from_this());
-
- for (const auto processor : processor_list_)
- {
- processor->process(capture_ptr);
- }
-
- return true;
-}
-
-ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
-{
- processor_list_.push_back(_proc_ptr);
- _proc_ptr->setSensor(shared_from_this());
- _proc_ptr->setProblem(getProblem());
- return _proc_ptr;
-}
-
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
-{
- CaptureBasePtr cap;
-
- if (isStateBlockDynamic(_i, cap))
- return cap->getStateBlock(_i);
-
- return getStateBlockPtrStatic(_i);
-}
-
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
-{
- CaptureBasePtr cap;
-
- if (isStateBlockDynamic(_i, _ts, cap))
- return cap->getStateBlock(_i);
-
- return getStateBlockPtrStatic(_i);
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
-{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
- {
- cap = lastKeyCapture();
-
- return cap != nullptr;
- }
- else
- return false;
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap)
-{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
- {
- cap = lastCapture(_ts);
-
- return cap != nullptr;
- }
- else
- return false;
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i)
-{
- CaptureBasePtr cap;
-
- return isStateBlockDynamic(_i,cap);
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts)
-{
- CaptureBasePtr cap;
-
- return isStateBlockDynamic(_i,_ts,cap);
-}
-
-void SensorBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto prc : processor_list_)
- prc->setProblem(_problem);
-}
-
-} // namespace wolf
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
deleted file mode 100644
index 54c08c69f3a263b6d71a1b461baa393e62db0d10..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_camera.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-#include "base/sensor/sensor_camera.h"
-
-#include "base/pinhole_tools.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-
-namespace wolf
-{
-
-SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsCamera& _intrinsics) :
- SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model_raw, true), 1),
- img_width_(_intrinsics.width), //
- img_height_(_intrinsics.height), //
- distortion_(_intrinsics.distortion), //
- correction_(distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
- pinhole_model_raw_(_intrinsics.pinhole_model_raw), //
- pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
- using_raw_(true)
-{
- assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
- useRawImages();
- pinhole::computeCorrectionModel(getIntrinsic()->getState(), distortion_, correction_);
-}
-
-SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) :
- SensorCamera(_extrinsics, *_intrinsics_ptr)
-{
- //
-}
-
-SensorCamera::~SensorCamera()
-{
- //
-}
-
-Eigen::Matrix3s SensorCamera::setIntrinsicMatrix(Eigen::Vector4s _pinhole_model)
-{
- Eigen::Matrix3s K;
- K(0, 0) = _pinhole_model(2);
- K(0, 1) = 0;
- K(0, 2) = _pinhole_model(0);
- K(1, 0) = 0;
- K(1, 1) = _pinhole_model(3);
- K(1, 2) = _pinhole_model(1);
- K.row(2) << 0, 0, 1;
- return K;
-}
-
-// Define the factory method
-SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
- const Eigen::VectorXs& _extrinsics_pq, //
- const IntrinsicsBasePtr _intrinsics)
-{
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
- std::shared_ptr<IntrinsicsCamera> intrinsics_ptr = std::static_pointer_cast<IntrinsicsCamera>(_intrinsics);
- SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
- sen_ptr->setName(_unique_name);
-
- return sen_ptr;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf
-{
-WOLF_REGISTER_SENSOR("CAMERA", SensorCamera)
-} // namespace wolf
-
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
deleted file mode 100644
index dad9575a9135a4c20b38afb6ffac1492ed06a9c5..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_diff_drive.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-#include "base/sensor/sensor_diff_drive.h"
-#include "base/state_block.h"
-#include "base/capture/capture_motion.h"
-#include "base/eigen_assert.h"
-
-namespace wolf {
-
-SensorDiffDrive::SensorDiffDrive(const StateBlockPtr& _p_ptr,
- const StateBlockPtr& _o_ptr,
- const StateBlockPtr& _i_ptr,
- const IntrinsicsDiffDrivePtr& _intrinsics) :
- SensorBase("DIFF DRIVE", _p_ptr, _o_ptr, _i_ptr, 2, false, false),
- intrinsics_ptr_{_intrinsics}
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
- const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- Eigen::VectorSizeCheck<3>::check(_extrinsics_po);
-
- // cast intrinsics into derived type
- IntrinsicsDiffDrivePtr params = std::dynamic_pointer_cast<IntrinsicsDiffDrive>(_intrinsics);
-
- if (params == nullptr)
- {
- WOLF_ERROR("SensorDiffDrive::create expected IntrinsicsDiffDrive !");
- return nullptr;
- }
-
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
- StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, true);
-
- SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params);
-
- odo->setName(_unique_name);
-
- /// @todo make calibration optional at creation
- //if (calibrate)
- // odo->unfixIntrinsics();
-
- return odo;
-}
-
-/// @todo Further work to enforce wheel model
-
-//const IntrinsicsDiffDrive& SensorDiffDrive::getIntrinsics()
-//{
-//// if (intrinsic_ptr_)
-//// {
-//// const auto& intrinsics = intrinsic_ptr_->getVector();
-
-//// intrinsics_.left_radius_factor_ = intrinsics(0);
-//// intrinsics_.right_radius_factor_ = intrinsics(1);
-//// intrinsics_.separation_factor_ = intrinsics(2);
-//// }
-
-// return intrinsics_;
-//}
-
-//void SensorDiffDrive::initIntrisics()
-//{
-// assert(intrinsic_ptr_ == nullptr &&
-// "SensorDiffDrive::initIntrisicsPtr should only be called once at construction.");
-
-// Eigen::Vector3s state;
-// state << intrinsics_.left_radius_factor_,
-// intrinsics_.right_radius_factor_,
-// intrinsics_.separation_factor_;
-
-// assert(state(0) > 0 && "The left_radius_factor should be rather close to 1.");
-// assert(state(1) > 0 && "The right_radius_factor should be rather close to 1.");
-// assert(state(2) > 0 && "The separation_factor should be rather close to 1.");
-
-// intrinsic_ptr_ = new StateBlock(state);
-//}
-
-//void SensorDiffDrive::computeDataCov(const Eigen::Vector2s &_data, Eigen::Matrix2s &_data_cov)
-//{
-// const double dp_std_l = intrinsics_.left_gain_ * _data(0);
-// const double dp_std_r = intrinsics_.right_gain_ * _data(1);
-
-// const double dp_var_l = dp_std_l * dp_std_l;
-// const double dp_var_r = dp_std_r * dp_std_r;
-
-// /// Wheel resolution covariance, which is like a DC offset equal to half of
-// /// the resolution, which is the theoretical average error:
-// const double dp_std_avg = Scalar(0.5) * intrinsics_.left_resolution_;
-// const double dp_var_avg = dp_std_avg * dp_std_avg;
-
-// /// Set covariance matrix (diagonal):
-// _data_cov.diagonal() << dp_var_l + dp_var_avg,
-// dp_var_r + dp_var_avg;
-//}
-
-// This overload is probably not the best solution as it forces
-// me to call addCapture from a SensorDiffDrivePtr whereas
-// problem->installSensor() return a SensorBasePtr.
-//bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
-//{
-// if (intrinsics_.data_is_position_)
-// {
-// Eigen::Vector2s data = _capture_ptr->getData();
-
-// // dt is set to one as we are dealing with wheel position
-// data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
-// intrinsics_.separation_, 1);
-
-// _capture_ptr->setData(data);
-
-// Eigen::Matrix2s data_cov;
-// data_cov << 0.00001, 0, 0, 0.00001; // Todo
-
-// computeDataCov(data, data_cov);
-
-// _capture_ptr->setDataCovariance(data_cov);
-// }
-
-// /// @todo tofix
-// return false;//SensorBase::addCapture(_capture_ptr);
-//}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
-} // namespace wolf
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 09c619c78c1a1f271cae9da86ba19ccd4cb9746c..d91242cc5c9b976ba44b30dc050c7b566fb27f36 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -1,6 +1,6 @@
-#include "base/sensor/sensor_gnss.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf {
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
deleted file mode 100644
index a7a5677af497ccb2f925dce9ae993e160b860fd5..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_laser_2D.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/state_block.h"
-
-namespace wolf {
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8)
-{
- setDefaultScanParams();
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_({ _angle_min, _angle_max, _angle_step, _scan_time, _range_min, _range_max, _range_std_dev, _angle_std_dev })
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_(_params)
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_(_params.scan_params)
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) :
- SensorLaser2D(_p_ptr, _o_ptr, *_params)
-{
- //
-}
-
-SensorLaser2D::~SensorLaser2D()
-{
- //
-}
-
-void SensorLaser2D::setDefaultScanParams()
-{
- scan_params_.angle_min_ = M_PI_2;
- scan_params_.angle_max_ = -M_PI_2;
- scan_params_.angle_step_ = -M_PI/720;
- scan_params_.scan_time_ = 0.01;//not relevant
- scan_params_.range_min_ = 0.2;
- scan_params_.range_max_ = 100;
- scan_params_.range_std_dev_ = 0.01;
-
- setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
-}
-
-void SensorLaser2D::setScanParams(const laserscanutils::LaserScanParams & _params)
-{
- scan_params_ = _params;
-}
-
-const laserscanutils::LaserScanParams& SensorLaser2D::getScanParams() const
-{
- return scan_params_;
-}
-
-// Define the factory method
-SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
- // cast intrinsics into derived type
- IntrinsicsLaser2DPtr params = std::static_pointer_cast<IntrinsicsLaser2D>(_intrinsics);
- SensorLaser2DPtr sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params->scan_params);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory and the ParameterFactory
-#include "base/sensor/sensor_factory.h"
-//#include "intrinsics_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D)
-//const bool registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
-} // namespace wolf
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
deleted file mode 100644
index c698db52f69d668ca7bab3c522087cb168047d10..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_odom_2D.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-#include "base/sensor/sensor_odom_2D.h"
-#include "base/state_block.h"
-#include "base/state_angle.h"
-
-namespace wolf {
-
-SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics) :
- SensorBase("ODOM 2D", std::make_shared<StateBlock>(_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.");
- //
-}
-
-SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics) :
- SensorOdom2D(_extrinsics, *_intrinsics)
-{
- //
-}
-
-SensorOdom2D::~SensorOdom2D()
-{
- //
-}
-
-Scalar SensorOdom2D::getDispVarToDispNoiseFactor() const
-{
- return k_disp_to_disp_;
-}
-
-Scalar SensorOdom2D::getRotVarToRotNoiseFactor() const
-{
- return k_rot_to_rot_;
-}
-
-// Define the factory method
-SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-
- SensorOdom2DPtr odo;
- if (_intrinsics)
- {
- std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
- odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
- }
- else
- {
- IntrinsicsOdom2D params;
- params.k_disp_to_disp = 1;
- params.k_rot_to_rot = 1;
- odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
- }
- odo->setName(_unique_name);
- return odo;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D)
-} // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
deleted file mode 100644
index 0a5c2d7961b5d36e171e6b9d29fafdc30580a74e..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_odom_3D.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-/**
- * \file sensor_odom_3D.cpp
- *
- * Created on: Oct 7, 2016
- * \author: jsola
- */
-
-#include "base/sensor/sensor_odom_3D.h"
-
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-
-namespace wolf {
-
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) :
- SensorBase("ODOM 3D", std::make_shared<StateBlock>(_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),
- min_disp_var_(_intrinsics.min_disp_var),
- min_rot_var_(_intrinsics.min_rot_var)
-{
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3D.");
-
- noise_cov_ = (Eigen::Vector6s() << 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_
-}
-
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr _intrinsics) :
- SensorOdom3D(_extrinsics_pq, *_intrinsics)
-{
- //
-}
-
-SensorOdom3D::~SensorOdom3D()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
- // cast intrinsics into derived type
- IntrinsicsOdom3DPtr params = std::static_pointer_cast<IntrinsicsOdom3D>(_intrinsics);
-
- // Call constructor and finish
- SensorBasePtr odo = std::make_shared<SensorOdom3D>(_extrinsics_pq, params);
- odo->setName(_unique_name);
-
- return odo;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D)
-}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
deleted file mode 100644
index 3024401a1c70802c9cc96bbf8aeb4df85f618534..0000000000000000000000000000000000000000
--- a/src/solver/solver_manager.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-#include "base/solver/solver_manager.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
-#include "base/landmark/landmark_base.h"
-
-namespace wolf {
-
-SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
- wolf_problem_(_wolf_problem)
-{
- assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
-}
-
-void SolverManager::update()
-{
- // REMOVE CONSTRAINTS
- auto fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while ( fac_notification_it != wolf_problem_->getFactorNotificationMap().end() )
- {
- if (fac_notification_it->second == REMOVE)
- {
- removeFactor(fac_notification_it->first);
- fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
- }
- else
- fac_notification_it++;
- }
-
- // ADD/REMOVE STATE BLOCS
- auto sb_notification_it = wolf_problem_->getStateBlockNotificationMap().begin();
- while ( sb_notification_it != wolf_problem_->getStateBlockNotificationMap().end() )
- {
- StateBlockPtr state_ptr = sb_notification_it->first;
-
- if (sb_notification_it->second == ADD)
- {
- // only add if not added
- if (state_blocks_.find(state_ptr) == state_blocks_.end())
- {
- state_blocks_.emplace(state_ptr, state_ptr->getState());
- addStateBlock(state_ptr);
- // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
- state_ptr->resetStateUpdated();
- state_ptr->resetFixUpdated();
- state_ptr->resetLocalParamUpdated();
- }
- else
- {
- WOLF_DEBUG("Tried adding an already registered StateBlock.");
- }
- }
- else
- {
- // only remove if it exists
- if (state_blocks_.find(state_ptr)!=state_blocks_.end())
- {
- removeStateBlock(state_ptr);
- state_blocks_.erase(state_ptr);
- }
- else
- {
- WOLF_DEBUG("Tried to remove a StateBlock that was not added !");
- }
- }
- // next notification
- sb_notification_it = wolf_problem_->getStateBlockNotificationMap().erase(sb_notification_it);
- }
-
- // ADD CONSTRAINTS
- fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while (fac_notification_it != wolf_problem_->getFactorNotificationMap().end())
- {
- assert(wolf_problem_->getFactorNotificationMap().begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
-
- addFactor(wolf_problem_->getFactorNotificationMap().begin()->first);
- fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
- }
-
- // UPDATE STATE BLOCKS (state, fix or local parameterization)
- for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
- {
- if (state_blocks_.find(state_ptr)==state_blocks_.end())
- continue;
-
- assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
-
- // state update
- if (state_ptr->stateUpdated())
- {
- Eigen::VectorXs new_state = state_ptr->getState();
- // We assume the same size for the states in both WOLF and the solver.
- std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
- // reset flag
- state_ptr->resetStateUpdated();
- }
- // fix update
- if (state_ptr->fixUpdated())
- {
- updateStateBlockStatus(state_ptr);
- // reset flag
- state_ptr->resetFixUpdated();
- }
- // local parameterization update
- if (state_ptr->localParamUpdated())
- {
- updateStateBlockLocalParametrization(state_ptr);
- // reset flag
- state_ptr->resetLocalParamUpdated();
- }
- }
-
- //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
- //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
-}
-
-wolf::ProblemPtr SolverManager::getProblem()
-{
- return wolf_problem_;
-}
-
-std::string SolverManager::solve(const ReportVerbosity report_level)
-{
- // update problem
- update();
-
- std::string report = solveImpl(report_level);
-
- // update StateBlocks with optimized state value.
- /// @todo whatif someone has changed the state notification during opti ??
- /// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
-
- std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
- it_end = state_blocks_.end();
- for (; it != it_end; ++it)
- {
- // Avoid usuless copies
- if (!it->first->isFixed())
- it->first->setState(it->second, false); // false = do not raise the flag state_updated_
- }
-
- return report;
-}
-
-Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
-{
- auto it = state_blocks_.find(state_ptr);
-
- if (it == state_blocks_.end())
- throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
- return it->second;
-}
-
-Scalar* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
-{
- auto it = state_blocks_.find(state_ptr);
-
- if (it == state_blocks_.end())
- throw std::runtime_error("Tried to retrieve the memory block pointer of an unregistered StateBlock !");
-
- return it->second.data();
-}
-
-} // namespace wolf
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
deleted file mode 100644
index 2ba7af7081a64abec0149777e790c6fc355b6c49..0000000000000000000000000000000000000000
--- a/src/solver_suitesparse/solver_manager.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-#include "base/ceres_wrapper/ceres_manager.h"
-
-SolverManager::SolverManager()
-{
-
-}
-
-SolverManager::~SolverManager()
-{
- removeAllStateUnits();
-}
-
-void SolverManager::solve()
-{
-
-}
-
-//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
-//{
-//}
-
-void SolverManager::update(const WolfProblemPtr _problem_ptr)
-{
- // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
- if (_problem_ptr->isReallocated())
- {
- // todo: reallocate x
- }
- else
- {
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
- {
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
-
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
- }
- //std::cout << "state units updated!" << std::endl;
-
- // REMOVE STATE UNITS
- while (!_problem_ptr->getRemovedStateList().empty())
- {
- // TODO: remove state unit
- //_problem_ptr->getRemovedStateList().pop_front();
- }
- //std::cout << "state units removed!" << std::endl;
-
- // ADD CONSTRAINTS
- FactorBasePtrList fac_list;
- _problem_ptr->getTrajectory()->getFactorList(fac_list);
- //std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
- for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
- if ((*fac_it)->getPendingStatus() == ADD_PENDING)
- addFactor(*fac_it);
-
- //std::cout << "factors updated!" << std::endl;
- }
-}
-
-void SolverManager::addFactor(FactorBasePtr _corr_ptr)
-{
- //TODO MatrixXs J; Vector e;
- // getResidualsAndJacobian(_corr_ptr, J, e);
- // solverQR->addFactor(_corr_ptr, J, e);
-
-// factor_map_[_corr_ptr->id()] = blockIdx;
- _corr_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeFactor(const unsigned int& _corr_idx)
-{
- // TODO
-}
-
-void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
-{
- //std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- // TODO
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_1D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_VECTOR:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- if (_st_ptr->isFixed())
- updateStateUnitStatus(_st_ptr);
-
- _st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeAllStateUnits()
-{
- std::vector<double*> parameter_blocks;
-
- ceres_problem_->GetParameterBlocks(¶meter_blocks);
-
- for (unsigned int i = 0; i< parameter_blocks.size(); i++)
- ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
-}
-
-void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
-{
- // TODO
-
-// if (!_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
-// else if (_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
-// else
-// printf("\nERROR: Update state unit status with unknown status");
-//
-// _st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
-{
- //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
- //_corrPtr->print();
-
- switch (_corrPtr->getFactorType())
- {
- case FAC_GPS_FIX_2D:
- {
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorGPS2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2D_COMPLEX_ANGLE:
- {
- FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2D:
- {
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_CORNER_2D:
- {
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorCorner2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_IMU:
- {
- FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorIMU,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
-}
diff --git a/src/state_block.cpp b/src/state_block.cpp
deleted file mode 100644
index b4427c2aae025a794ae433bcb723011f1110f499..0000000000000000000000000000000000000000
--- a/src/state_block.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-#include "base/state_block.h"
-namespace wolf
-{
-
-void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify)
-{
- assert(_state.size() == state_.size());
- {
- std::lock_guard<std::mutex> lock(mut_state_);
- state_ = _state;
- state_size_ = state_.size();
- }
-
- // Flag
- if (_notify)
- state_updated_.store(true);
-}
-
-void StateBlock::setFixed(bool _fixed)
-{
- // Flag
- if (fixed_.load() != _fixed)
- fix_updated_.store(true);
-
- // set
- fixed_.store(_fixed);
-}
-
-//void StateBlock::addToProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->addStateBlock(shared_from_this());
-//}
-//
-//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->removeStateBlock(shared_from_this());
-//}
-
-}
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
deleted file mode 100644
index 55d25fc6ed7fc8d67d609b2d1f7d16d1455aef1b..0000000000000000000000000000000000000000
--- a/src/time_stamp.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-
-#include "base/time_stamp.h"
-
-namespace wolf {
-
-std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
-{
- os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream
- os << std::setfill(' ');
- return os;
-}
-
-TimeStamp::TimeStamp() :
- //time_stamp_(0)
- time_stamp_nano_(0)
-{
- setToNow();
-}
-
-TimeStamp::TimeStamp(const TimeStamp& _ts) :
- time_stamp_nano_(_ts.time_stamp_nano_)
-{
- //
-}
-
-TimeStamp::TimeStamp(const Scalar& _ts) :
- time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0)
-{
- //
-}
-
-TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) :
- time_stamp_nano_(_sec*NANOSECS+_nsec)
-{
- //
-}
-
-TimeStamp::~TimeStamp()
-{
- //nothing to do
-}
-
-void TimeStamp::setToNow()
-{
- timeval ts;
- gettimeofday(&ts, NULL);
- set(ts);
-}
-
-TimeStamp TimeStamp::operator +(const Scalar& dt) const
-{
- TimeStamp ts(*this);
- ts += dt;
- return ts;
-}
-
-TimeStamp TimeStamp::operator -(const Scalar& dt) const
-{
- TimeStamp ts(*this);
- ts -= dt;
- return ts;
-}
-
-inline void TimeStamp::operator -=(const Scalar& dt)
-{
- unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
- time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
-}
-
-void TimeStamp::print(std::ostream & ost) const
-{
- ost << *this;
-}
-
-} // namespace wolf
diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp
deleted file mode 100644
index 51b06c703e716e055f2fff03429da6873ad84b4f..0000000000000000000000000000000000000000
--- a/src/track_matrix.cpp
+++ /dev/null
@@ -1,261 +0,0 @@
-/**
- * \file track_matrix.cpp
- *
- * Created on: Mar 24, 2018
- * \author: jsola
- */
-
-#include "base/track_matrix.h"
-
-namespace wolf
-{
-
-size_t TrackMatrix::track_id_count_ = 0;
-
-TrackMatrix::TrackMatrix()
-{
- //
-}
-
-TrackMatrix::~TrackMatrix()
-{
- //
-}
-
-map<size_t, Track > TrackMatrix::tracks() const
-{
- return tracks_;
-}
-
-Track TrackMatrix::track(size_t _track_id)
-{
- if (tracks_.count(_track_id) > 0)
- return tracks_.at(_track_id);
- else
- return Track();
-}
-
-Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
-{
- if (_capture && snapshots_.count(_capture->id()) > 0)
- return snapshots_.at(_capture->id());
- else
- return Snapshot();
-}
-
-void TrackMatrix::newTrack(CaptureBasePtr _cap, FeatureBasePtr _ftr)
-{
- track_id_count_ ++;
- add(track_id_count_, _cap, _ftr);
-}
-
-void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr)
-{
- assert( (_track_id > 0) && (_track_id <= track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
- assert( (_cap != nullptr) && "Capture is null ptr." );
-
- _ftr->setTrackId(_track_id);
- if (_cap != _ftr->getCapture())
- _ftr->setCapture(_cap);
-
- tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
- snapshots_[_cap->id()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
-}
-
-void TrackMatrix::remove(size_t _track_id)
-{
- // Remove track features from all Snapshots
- if (tracks_.count(_track_id))
- {
- for (auto const& pair_time_ftr : tracks_.at(_track_id))
- {
- SizeStd cap_id = pair_time_ftr.second->getCapture()->id();
- snapshots_.at(cap_id).erase(_track_id);
- if (snapshots_.at(cap_id).empty())
- snapshots_.erase(cap_id);
- }
-
- // Remove track
- tracks_.erase(_track_id);
- }
-}
-
-void TrackMatrix::remove(CaptureBasePtr _cap)
-{
- // remove snapshot features from all tracks
- if (snapshots_.count(_cap->id()))
- {
- TimeStamp ts = _cap->getTimeStamp();
- for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
- {
- SizeStd trk_id = pair_trkid_ftr.first;
- tracks_.at(trk_id).erase(ts);
- if (tracks_.at(trk_id).empty())
- tracks_.erase(trk_id);
- }
-
- // remove snapshot
- snapshots_.erase(_cap->id());
- }
-}
-
-void TrackMatrix::remove(FeatureBasePtr _ftr)
-{
- // assumes _ftr->getCapture() and _ftr->trackId() are valid
- if (_ftr)
- {
- if (auto cap = _ftr->getCapture())
- {
- tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
- if (tracks_.at(_ftr->trackId()).empty())
- tracks_.erase(_ftr->trackId());
-
- snapshots_.at(cap->id()) .erase(_ftr->trackId());
- if (snapshots_.at(cap->id()).empty())
- snapshots_.erase(cap->id());
- }
- }
-}
-
-size_t TrackMatrix::numTracks()
-{
- return tracks_.size();
-}
-
-size_t TrackMatrix::trackSize(size_t _track_id)
-{
- return track(_track_id).size();
-}
-
-FeatureBasePtr TrackMatrix::firstFeature(size_t _track_id)
-{
- if (tracks_.count(_track_id) > 0)
- return tracks_.at(_track_id).begin()->second;
- else
- return nullptr;
-}
-
-FeatureBasePtr TrackMatrix::lastFeature(size_t _track_id)
-{
- if (tracks_.count(_track_id) > 0)
- return tracks_.at(_track_id).rbegin()->second;
- else
- return nullptr;
-}
-
-vector<FeatureBasePtr> TrackMatrix::trackAsVector(size_t _track_id)
-{
- vector<FeatureBasePtr> vec;
- if (tracks_.count(_track_id))
- {
- vec.reserve(trackSize(_track_id));
- for (auto const& pair_time_ftr : tracks_.at(_track_id))
- vec.push_back(pair_time_ftr.second);
- }
- return vec;
-}
-
-std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
-{
- std::list<FeatureBasePtr> lst;
- if (snapshots_.count(_cap->id()))
- for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
- lst.push_back(pair_trkid_ftr.second);
- return lst;
-}
-
-TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
-{
- TrackMatches pairs;
-
- if (_cap_1 == _cap_2) return pairs;
-
- Snapshot s_1 = snapshot(_cap_1);
- Snapshot s_2 = snapshot(_cap_2);
- Snapshot s_short = s_1;
- Snapshot s_long = s_2;
- if (s_1.size() > s_2.size())
- {
- s_long = s_1;
- s_short = s_2;
- }
-
- for (auto const & pair_trkid_ftr : s_short)
- {
- SizeStd trk_id = pair_trkid_ftr.first;
- if (s_long.count(trk_id))
- pairs[trk_id] = pair<FeatureBasePtr, FeatureBasePtr>(s_1.at(trk_id), s_2.at(trk_id));
- }
-
- return pairs;
-}
-
-FeatureBasePtr TrackMatrix::feature(size_t _track_id, CaptureBasePtr _cap)
-{
- if (snapshot(_cap).count(_track_id))
- return snapshot(_cap).at(_track_id);
- else
- return nullptr;
-}
-
-FeatureBasePtr TrackMatrix::feature(size_t _track_id, const TimeStamp& _ts)
-{
- Track trk = track(_track_id);
-
- auto ftr_it = trk.lower_bound(_ts);
-
- if (ftr_it == trk.end()) return nullptr;
-
- FeatureBasePtr ftr = ftr_it->second;
- return ftr;
-}
-
-CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
-{
- return firstFeature(_track_id)->getCapture();
-}
-
-Track TrackMatrix::trackAtKeyframes(size_t _track_id)
-{
- if (tracks_kf_.count(_track_id) > 0)
- return tracks_kf_.at(_track_id);
- else
- return Track();
-}
-
-bool TrackMatrix::markKeyframe(CaptureBasePtr _capture)
-{
- if (_capture->getFrame() && _capture->getFrame()->isKey())
- {
- auto snap = snapshot(_capture);
-
- if (snap.empty())
- return false;
-
- for (auto pair_trkid_ftr : snap)
- {
- auto track_id = pair_trkid_ftr.first;
- auto ftr = pair_trkid_ftr.second;
- auto ts = _capture->getFrame()->getTimeStamp();
- tracks_kf_[track_id][ts] = ftr;
- }
- return true;
- }
- return false;
-}
-
-bool TrackMatrix::unmarkKeyframe(FrameBasePtr _keyframe)
-{
- bool removed = false;
- auto ts = _keyframe->getTimeStamp();
- for (auto pair_id_trk : tracks_kf_)
- {
- if (pair_id_trk.second.erase(ts))
- {
- removed = true;
- }
- }
- return removed;
-}
-
-}
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
deleted file mode 100644
index 5820b99ba5df29f281ee2e4e61c82368a93d3561..0000000000000000000000000000000000000000
--- a/src/trajectory_base.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-#include "base/trajectory_base.h"
-#include "base/frame_base.h"
-
-namespace wolf {
-
-TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
- NodeBase("TRAJECTORY", "Base"),
- frame_structure_(_frame_structure),
- last_key_frame_ptr_(nullptr)
-{
-// WOLF_DEBUG("constructed T");
-}
-
-TrajectoryBase::~TrajectoryBase()
-{
- //
-}
-
-FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
-{
- // link up
- _frame_ptr->setTrajectory(shared_from_this());
- _frame_ptr->setProblem(getProblem());
-
- if (_frame_ptr->isKey())
- {
- _frame_ptr->registerNewStateBlocks();
- if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp())
- last_key_frame_ptr_ = _frame_ptr;
-
- frame_list_.insert(computeFrameOrder(_frame_ptr), _frame_ptr);
- }
- else
- {
- frame_list_.push_back(_frame_ptr);
- }
-
- return _frame_ptr;
-}
-
-void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
-{
- for(auto fr_ptr : getFrameList())
- fr_ptr->getFactorList(_fac_list);
-}
-
-void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
-{
- moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
- // last_key_frame_ptr_ = findLastKeyFrame(); // done in moveFrame() just above
-}
-
-void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
-{
- if (*_place != _frm_ptr)
- {
- frame_list_.remove(_frm_ptr);
- frame_list_.insert(_place, _frm_ptr);
- last_key_frame_ptr_ = findLastKeyFrame();
- }
-}
-
-FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
-{
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
- if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
- return frm_rit.base();
- return getFrameList().begin();
-}
-
-FrameBasePtr TrajectoryBase::findLastKeyFrame()
-{
- // NOTE: Assumes keyframes are sorted by timestamp
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
- if ((*frm_rit)->isKey())
- return (*frm_rit);
-
- return nullptr;
-}
-
-FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
-{
- FrameBasePtr closest_kf = nullptr;
- Scalar min_dt = 1e9;
-
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
- if ((*frm_rit)->isKey())
- {
- Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
- if (dt < min_dt)
- {
- min_dt = dt;
- closest_kf = *frm_rit;
- }
- else
- break;
- }
- return closest_kf;
-}
-
-} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 56ff30a51d369af9e2d8a00bfdc3141788b0bc36..923632d408e92c88c5ee7234287a34753b1b7251 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -2,25 +2,6 @@
add_subdirectory(gtest)
-## Added these two include_dirs: ######################
-#
-#CMAKE modules
-#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-#MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-#
-# Include Eigen
-#FIND_PACKAGE(Eigen 3 REQUIRED)
-#INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-#
-# Include Ceres
-#FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-#IF(Ceres_FOUND)
-# INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-#ENDIF(Ceres_FOUND)
-#
-## and now gtest_motion_2d works ######################
-
-
# Include gtest directory.
include_directories(${GTEST_INCLUDE_DIRS})
@@ -32,187 +13,10 @@ target_link_libraries(gtest_example ${PROJECT_NAME}) #
# #
###########################################################
-
-################# ADD YOUR TESTS BELOW ####################
-# #
-# ==== IN ALPHABETICAL ORDER! ==== #
-# #
-
-# ------- First Core classes ----------
-
-# CaptureBase class test
-wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-target_link_libraries(gtest_capture_base ${PROJECT_NAME})
-
-# CaptureBase class test
-#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
-#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME})
-
-# FactorAutodiff class test
-wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
-target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
-
-# FeatureBase classes test
-wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
-
-# FeatureBase classes test
-wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-target_link_libraries(gtest_feature_base ${PROJECT_NAME})
-
-# FrameBase classes test
-wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-target_link_libraries(gtest_frame_base ${PROJECT_NAME})
-
-# IMU tools test
-wolf_add_gtest(gtest_IMU_tools gtest_IMU_tools.cpp)
-# target_link_libraries(gtest_IMU_tools ${PROJECT_NAME}) // WOLF library not needed
-
-# LocalParametrizationXxx classes test
-wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-target_link_libraries(gtest_local_param ${PROJECT_NAME})
-
-# MotionBuffer class test
-wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
-
-# PackKFBuffer
-wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
-target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME})
-
-# Problem class test
-wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PROJECT_NAME})
-
-# ProcessorBase class test
-wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PROJECT_NAME})
-
-# ProcessorMotion class test
-wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
-
-# Rotation test
-wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
-
-# SE3 test
-wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
-
-# SensorBase test
-wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
-target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
-
-# shared_from_this test
-wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
-
-# SolverManager test
-wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
-target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
-
-# TimeStamp class test
-wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
-target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
-
-# TrackMatrix class test
-wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
-target_link_libraries(gtest_track_matrix ${PROJECT_NAME})
-
-# TrajectoryBase class test
-wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
-target_link_libraries(gtest_trajectory ${PROJECT_NAME})
-
-# ------- Now Derived classes ----------
-
-IF (Ceres_FOUND)
-# CeresManager test
-wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp)
-target_link_libraries(gtest_ceres_manager ${PROJECT_NAME})
-ENDIF(Ceres_FOUND)
-
-# FactorAbs(P/O/V) classes test
-wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
-target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
-
-# FactorAutodiffDistance3D test
-wolf_add_gtest(gtest_factor_autodiff_distance_3D gtest_factor_autodiff_distance_3D.cpp)
-target_link_libraries(gtest_factor_autodiff_distance_3D ${PROJECT_NAME})
-
# FactorGnssFix2D test
wolf_add_gtest(gtest_factor_gnss_fix_2D gtest_factor_gnss_fix_2D.cpp)
-target_link_libraries(gtest_factor_gnss_fix_2D ${PROJECT_NAME})
+target_link_libraries(gtest_factor_gnss_fix_2D ${PROJECT_NAME} ${wolf_LIBRARY})
# FactorGnssSingleDiff2D test
wolf_add_gtest(gtest_factor_gnss_single_diff_2D gtest_factor_gnss_single_diff_2D.cpp)
-target_link_libraries(gtest_factor_gnss_single_diff_2D ${PROJECT_NAME})
-
-# FactorOdom3D class test
-wolf_add_gtest(gtest_factor_odom_3D gtest_factor_odom_3D.cpp)
-target_link_libraries(gtest_factor_odom_3D ${PROJECT_NAME})
-
-# FactorPose2D class test
-wolf_add_gtest(gtest_factor_pose_2D gtest_factor_pose_2D.cpp)
-target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
-
-# FactorPose3D class test
-wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
-target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
-
-# FeatureIMU test
-wolf_add_gtest(gtest_feature_IMU gtest_feature_IMU.cpp)
-target_link_libraries(gtest_feature_IMU ${PROJECT_NAME})
-
-# IMUTest (Bias + estimation)
-wolf_add_gtest(gtest_IMU gtest_IMU.cpp)
-target_link_libraries(gtest_IMU ${PROJECT_NAME})
-
-# LandmarkPolyline test
-wolf_add_gtest(gtest_landmark_polyline gtest_landmark_polyline.cpp)
-target_link_libraries(gtest_landmark_polyline ${PROJECT_NAME})
-
-# MakePosDef function test
-wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
-
-# Polyline 2D test
-wolf_add_gtest(gtest_polyline_2D gtest_polyline_2D.cpp)
-target_link_libraries(gtest_polyline_2D ${PROJECT_NAME})
-
-# Parameter prior test
-wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PROJECT_NAME})
-
-
-# Pinhole test
-wolf_add_gtest(gtest_pinhole gtest_pinhole.cpp)
-target_link_libraries(gtest_pinhole ${PROJECT_NAME})
-
-# ProcessorFrameNearestNeighborFilter class test
-wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
-target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
-
-# ProcessorIMU class test
-wolf_add_gtest(gtest_processor_IMU gtest_processor_IMU.cpp)
-target_link_libraries(gtest_processor_IMU ${PROJECT_NAME})
-
-# ProcessorIMUJacobians test
-wolf_add_gtest(gtest_processor_IMU_jacobians gtest_processor_IMU_jacobians.cpp)
-target_link_libraries(gtest_processor_IMU_jacobians ${PROJECT_NAME})
-
-# ProcessorMotion in 2D
-wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
-target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
-
-# ProcessorOdom3D class test
-wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp)
-target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
-
-# SensorBase test
-wolf_add_gtest(gtest_sensor_camera gtest_sensor_camera.cpp)
-target_link_libraries(gtest_sensor_camera ${PROJECT_NAME})
-
-
-# ------- Now Core classes Serialization ----------
-
-add_subdirectory(serialization)
-
+target_link_libraries(gtest_factor_gnss_single_diff_2D ${PROJECT_NAME} ${wolf_LIBRARY})
\ No newline at end of file
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
deleted file mode 100644
index 7487eb2fe49ada12c03120ce83810abf3d0d8943..0000000000000000000000000000000000000000
--- a/test/gtest_IMU.cpp
+++ /dev/null
@@ -1,1531 +0,0 @@
-/*
- * gtest_IMU.cpp
- *
- * Created on: Nov 14, 2017
- * Author: jsola
- */
-
-//Wolf
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/sensor/sensor_odom_3D.h"
-#include "base/processor/processor_odom_3D.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-// make my life easier
-using namespace Eigen;
-using namespace wolf;
-using std::shared_ptr;
-using std::make_shared;
-using std::static_pointer_cast;
-using std::string;
-
-class Process_Factor_IMU : public testing::Test
-{
- public:
- // Wolf objects
- ProblemPtr problem;
- CeresManagerPtr ceres_manager;
- SensorIMUPtr sensor_imu;
- ProcessorIMUPtr processor_imu;
- CaptureIMUPtr capture_imu;
- FrameBasePtr KF_0, KF_1; // keyframes
- CaptureBasePtr C_0 , C_1; // base captures
- CaptureMotionPtr CM_0, CM_1; // motion captures
-
- // time
- TimeStamp t0, t;
- Scalar dt, DT;
- int num_integrations;
-
- // initial state
- VectorXs x0; // initial state
- Vector3s p0, v0; // initial pos and vel
- Quaternions q0, q; // initial and current orientations
- Matrix<Scalar,9,9> P0; // initial state covariance
-
- // bias
- Vector6s bias_real, bias_preint, bias_null; // real, pre-int and zero biases.
- Vector6s bias_0, bias_1; // optimized bias at KF's 0 and 1
-
- // input
- Matrix<Scalar, 6, Dynamic> motion; // Motion in IMU frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
- Matrix<Scalar, 3, Dynamic> a, w; // True acc and angvel in IMU frame. Each column is a motion step. Used to create motion with `motion << a,w;`
- Vector6s data; // IMU data. It's the motion with gravity and bias. See imu::motion2data().
-
- // Deltas and states (exact, integrated, corrected, solved, etc)
- VectorXs D_exact, x1_exact; // exact or ground truth
- VectorXs D_preint_imu, x1_preint_imu; // preintegrated with imu_tools
- VectorXs D_corrected_imu, x1_corrected_imu; // corrected with imu_tools
- VectorXs D_preint, x1_preint; // preintegrated with processor_imu
- VectorXs D_corrected, x1_corrected; // corrected with processor_imu
- VectorXs D_optim, x1_optim; // optimized using factor_imu
- VectorXs D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias
- VectorXs x0_optim; // optimized using factor_imu
-
- // Trajectory buffer of correction Jacobians
- std::vector<MatrixXs> Buf_Jac_preint_prc;
-
- // Trajectory matrices
- MatrixXs Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
- MatrixXs Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
-
- // Delta correction Jacobian and step
- Matrix<Scalar,9,6> J_D_bias; // Jacobian of pre-integrated delta w
- Vector9s step;
-
- // Flags for fixing/unfixing state blocks
- bool p0_fixed, q0_fixed, v0_fixed;
- bool p1_fixed, q1_fixed, v1_fixed;
-
- virtual void SetUp( )
- {
- string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================== SETTING PROBLEM
- problem = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_manager = make_shared<CeresManager>(problem, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sensor = problem->installSensor ("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_no_vote.yaml");
- sensor_imu = static_pointer_cast<SensorIMU> (sensor);
- processor_imu = static_pointer_cast<ProcessorIMU>(processor);
-
- dt = 0.01;
- processor_imu->setTimeTolerance(dt/2);
-
- // Some initializations
- bias_null .setZero();
- x0 .resize(10);
- D_preint .resize(10);
- D_corrected .resize(10);
- x1_optim .resize(10);
- x1_optim_imu.resize(10);
-
- }
-
-
- /* Integrate one step of acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * q: current orientation
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Input/output
- * Delta: the preintegrated delta
- * J_D_b_ptr: a pointer to the Jacobian of Delta wrt bias. Defaults to nullptr.
- */
- static void integrateOneStep(const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Quaternions& q_real, VectorXs& Delta, Matrix<Scalar, 9, 6>* J_D_b_ptr = nullptr)
- {
- VectorXs delta(10), data(6);
- VectorXs Delta_plus(10);
- Matrix<Scalar, 9, 6> J_d_d, J_D_b, J_d_b;
- Matrix<Scalar, 9, 9> J_D_D, J_D_d;
-
- data = imu::motion2data(motion, q_real, bias_real);
- q_real = q_real*exp_q(motion.tail(3)*dt);
- Vector6s body = data - bias_preint;
- if (J_D_b_ptr == nullptr)
- {
- delta = imu::body2delta(body, dt);
- Delta_plus = imu::compose(Delta, delta, dt);
- }
- else
- {
- imu::body2delta(body, dt, delta, J_d_d);
- imu::compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
- J_D_b = *J_D_b_ptr;
- J_d_b = - J_d_d;
- J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
- *J_D_b_ptr = J_D_b;
- }
- Delta = Delta_plus;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientation
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * return: the preintegrated delta
- */
- static VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
- {
- VectorXs Delta(10);
- Delta = imu::identity();
- Quaternions q(q0);
- for (int n = 0; n < N; n++)
- {
- integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta);
- }
- return Delta;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientation
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
- static VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
- {
- VectorXs Delta(10);
- Quaternions q;
-
- Delta = imu::identity();
- J_D_b .setZero();
- q = q0;
- for (int n = 0; n < N; n++)
- {
- integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta, &J_D_b);
- }
- return Delta;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * q0: initial orientation
- * motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
- static VectorXs integrateDelta(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
- {
- VectorXs Delta(10);
- Quaternions q;
-
- Delta = imu::identity();
- J_D_b .setZero();
- q = q0;
- for (int n = 0; n < motion.cols(); n++)
- {
- integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
- }
- return Delta;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * q0: initial orientation
- * motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
- static MotionBuffer integrateDeltaTrajectory(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
- {
- MotionBuffer trajectory(6, 10, 9, 6);
- VectorXs Delta(10);
- MatrixXs M9(9,9), M6(6,6), J9(9,9), J96(9,6), V10(10,1), V6(6,1);
- Quaternions q;
-
- Delta = imu::identity();
- J_D_b .setZero();
- q = q0;
- TimeStamp t(0);
- trajectory.get().emplace_back(t, Vector6s::Zero(), M6, VectorXs::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXs::Zero(9,6));
- for (int n = 0; n < motion.cols(); n++)
- {
- t += dt;
- integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
- trajectory.get().emplace_back(t, motion.col(n), M6, V10, M9, Delta, M9, J9, J9, J_D_b);
- }
- return trajectory;
- }
-
- MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected)
- {
- Vector6s motion_now;
- data = imu::motion2data(motion.col(0), q0, bias_real);
- capture_imu = make_shared<CaptureIMU>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
- q = q0;
- t = t0;
- for (int i= 0; i < N; i++)
- {
- t += dt;
- motion_now = motion.cols() == 1
- ? motion
- : motion.col(i);
- data = imu::motion2data(motion_now, q, bias_real);
- w = motion_now.tail<3>();
- q = q * exp_q(w*dt);
-
- capture_imu->setTimeStamp(t);
- capture_imu->setData(data);
-
- sensor_imu->process(capture_imu);
-
- D_preint = processor_imu->getLast()->getDeltaPreint();
- D_corrected = processor_imu->getLast()->getDeltaCorrected(bias_real);
- }
- return processor_imu->getBuffer();
- }
-
- // Initial configuration of variables
- bool configureAll()
- {
- // initial state
- q0 .normalize();
- x0 << p0, q0.coeffs(), v0;
- P0 .setIdentity() * 0.01;
-
- // motion
- if (motion.cols() == 0)
- {
- motion.resize(6,a.cols());
- motion << a, w;
- }
- else
- {
- // if motion has any column at all, then it is already initialized in TEST_F(...) and we do nothing.
- }
- if (motion.cols() != 1)
- {
- // if motion has more than 1 col, make num_integrations consistent with nbr of cols, just for consistency
- num_integrations = motion.cols();
- }
-
- // total run time
- DT = num_integrations * dt;
-
- // wolf objects
- KF_0 = problem->setPrior(x0, P0, t0, dt/2);
- C_0 = processor_imu->getOrigin();
-
- processor_imu->getLast()->setCalibrationPreint(bias_preint);
-
- return true;
- }
-
- // Integrate using all methods
- virtual void integrateAll()
- {
- // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
- if (motion.cols() == 1)
- D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
- else
- D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
- x1_exact = imu::composeOverState(x0, D_exact, DT );
-
- // ===================================== INTEGRATE USING IMU_TOOLS
- // pre-integrate
- if (motion.cols() == 1)
- D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias);
- else
- D_preint_imu = integrateDelta(q0, motion, bias_real, bias_preint, dt, J_D_bias);
-
- // correct perturbated
- step = J_D_bias * (bias_real - bias_preint);
- D_corrected_imu = imu::plus(D_preint_imu, step);
-
- // compose states
- x1_preint_imu = imu::composeOverState(x0, D_preint_imu , DT );
- x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
-
- // ===================================== INTEGRATE USING PROCESSOR_IMU
-
- integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
-
- // compose states
- x1_preint = imu::composeOverState(x0, D_preint , DT );
- x1_corrected = imu::composeOverState(x0, D_corrected , DT );
- }
-
- // Integrate Trajectories all methods
- virtual void integrateAllTrajectories()
- {
- SizeEigen cols = motion.cols() + 1;
- Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
- Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
-
- // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
- MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
-
- // Build exact trajectories
- int col = 0;
- Scalar Dt = 0;
- for (auto m : Buf_exact.get())
- {
- Trj_D_exact.col(col) = m.delta_integr_;
- Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, Dt );
- Dt += dt;
- col ++;
- }
-
- // set
- D_exact = Trj_D_exact.col(cols-1);
- x1_exact = Trj_x_exact.col(cols-1);
-
- // ===================================== INTEGRATE USING IMU_TOOLS
- // pre-integrate
- MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);
-
- // Build trajectories preintegrated and corrected with imu_tools
- col = 0;
- Dt = 0;
- for (auto m : Buf_preint_imu.get())
- {
- // preint
- Trj_D_preint_imu.col(col) = m.delta_integr_;
- Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt );
-
- // corrected
- VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
- Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ;
- Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt );
- Dt += dt;
- col ++;
- }
-
- D_preint_imu = Trj_D_preint_imu.col(cols-1);
-
- // correct perturbated
- step = J_D_bias * (bias_real - bias_preint);
- D_corrected_imu = imu::plus(D_preint_imu, step);
-
- // compose states
- x1_preint_imu = imu::composeOverState(x0, D_preint_imu , DT );
- x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
-
- // ===================================== INTEGRATE USING PROCESSOR_IMU
-
- MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
-
- // Build trajectories preintegrated and corrected with processorIMU
- Dt = 0;
- col = 0;
- Buf_Jac_preint_prc.clear();
- for (auto m : Buf_D_preint_prc.get())
- {
- // preint
- Trj_D_preint_prc.col(col) = m.delta_integr_;
- Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt );
-
- // corrected
- VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
- Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
- Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt );
-
- Buf_Jac_preint_prc.push_back(m.jacobian_calib_);
- Dt += dt;
- col ++;
- }
-
- // compose states
- x1_preint = imu::composeOverState(x0, D_preint , DT );
- x1_corrected = imu::composeOverState(x0, D_corrected , DT );
- }
-
- // Set state_blocks status
- void setFixedBlocks()
- {
- // this sets each state block status fixed / unfixed
- KF_0->getP()->setFixed(p0_fixed);
- KF_0->getO()->setFixed(q0_fixed);
- KF_0->getV()->setFixed(v0_fixed);
- KF_1->getP()->setFixed(p1_fixed);
- KF_1->getO()->setFixed(q1_fixed);
- KF_1->getV()->setFixed(v1_fixed);
- }
-
- void setKfStates()
- {
- // This perturbs states to estimate around the exact value, then assigns to the keyframe
- // Perturbations are applied only if the state block is unfixed
-
- VectorXs x_pert(10);
-
- // KF 0
- x_pert = x0;
- if (!p0_fixed)
- x_pert.head(3) += Vector3s::Random() * 0.01;
- if (!q0_fixed)
- x_pert.segment(3,4) = (Quaternions(x_pert.data() + 3) * exp_q(Vector3s::Random() * 0.01)).coeffs().normalized();
- if (!v0_fixed)
- x_pert.tail(3) += Vector3s::Random() * 0.01;
- KF_0->setState(x_pert);
-
- // KF 1
- x_pert = x1_exact;
- if (!p1_fixed)
- x_pert.head(3) += Vector3s::Random() * 0.01;
- if (!q1_fixed)
- x_pert.segment(3,4) = (Quaternions(x_pert.data() + 3) * exp_q(Vector3s::Random() * 0.01)).coeffs().normalized();
- if (!v1_fixed)
- x_pert.tail(3) += Vector3s::Random() * 0.01;
- KF_1->setState(x_pert);
- }
-
- virtual void buildProblem()
- {
- // ===================================== SET KF in Wolf tree
- FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t);
-
- // ===================================== IMU CALLBACK
- problem->keyFrameCallback(KF, nullptr, dt/2);
-
- // Process IMU for the callback to take effect
- data = Vector6s::Zero();
- capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
- processor_imu->process(capture_imu);
-
- KF_1 = problem->getLastKeyFrame();
- C_1 = KF_1->getCaptureList().front(); // front is IMU
- CM_1 = static_pointer_cast<CaptureMotion>(C_1);
-
- // ===================================== SET BOUNDARY CONDITIONS
- setFixedBlocks();
- setKfStates();
- }
-
- string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
- {
- string report = ceres_manager->solve(verbose);
-
- bias_0 = C_0->getCalibration();
- bias_1 = C_1->getCalibration();
-
- // ===================================== GET INTEGRATED STATE WITH SOLVED BIAS
- // with processor
- x0_optim = KF_0->getState();
- D_optim = CM_1->getDeltaCorrected(bias_0);
- x1_optim = KF_1->getState();
-
- // with imu_tools
- step = J_D_bias * (bias_0 - bias_preint);
- D_optim_imu = imu::plus(D_preint, step);
- x1_optim_imu = imu::composeOverState(x0_optim, D_optim_imu, DT);
-
- return report;
- }
-
- string runAll(SolverManager::ReportVerbosity verbose)
- {
- configureAll();
- integrateAll();
- buildProblem();
- string report = solveProblem(verbose);
- return report;
- }
-
- void printAll(std::string report = "")
- {
- WOLF_TRACE(report);
-
- WOLF_TRACE("D_exact : ", D_exact .transpose() ); // exact delta integrated, with absolutely no bias
- WOLF_TRACE("D_preint : ", D_preint .transpose() ); // pre-integrated delta using processor
- WOLF_TRACE("D_preint_imu : ", D_preint_imu .transpose() ); // pre-integrated delta using imu_tools
- WOLF_TRACE("D_correct : ", D_corrected .transpose() ); // corrected delta using processor
- WOLF_TRACE("D_correct_imu : ", D_corrected_imu .transpose() ); // corrected delta using imu_tools
- WOLF_TRACE("D_optim : ", D_optim .transpose() ); // corrected delta using optimum bias after solving using processor
- WOLF_TRACE("D_optim_imu : ", D_optim_imu .transpose() ); // corrected delta using optimum bias after solving using imu_tools
-
- WOLF_TRACE("bias real : ", bias_real .transpose() ); // real bias
- WOLF_TRACE("bias preint : ", bias_preint .transpose() ); // bias used during pre-integration
- WOLF_TRACE("bias optim 0 : ", bias_0 .transpose() ); // solved bias at KF_0
- WOLF_TRACE("bias optim 1 : ", bias_1 .transpose() ); // solved bias at KF_1
-
- // states at the end of integration, i.e., at KF_1
- WOLF_TRACE("X1_exact : ", x1_exact .transpose() ); // exact state
- WOLF_TRACE("X1_preint : ", x1_preint .transpose() ); // state using delta preintegrated by processor
- WOLF_TRACE("X1_preint_imu : ", x1_preint_imu .transpose() ); // state using delta preintegrated by imu_tools
- WOLF_TRACE("X1_correct : ", x1_corrected .transpose() ); // corrected state vy processor
- WOLF_TRACE("X1_correct_imu: ", x1_corrected_imu .transpose() ); // corrected state by imu_tools
- WOLF_TRACE("X1_optim : ", x1_optim .transpose() ); // optimal state after solving using processor
- WOLF_TRACE("X1_optim_imu : ", x1_optim_imu .transpose() ); // optimal state after solving using imu_tools
- WOLF_TRACE("err1_optim : ", (x1_optim-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
- WOLF_TRACE("err1_optim_imu: ", (x1_optim_imu-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
- WOLF_TRACE("X0_optim : ", x0_optim .transpose() ); // optimal state after solving using processor
- }
-
- virtual void assertAll()
- {
- // check delta and state integrals
- ASSERT_MATRIX_APPROX(D_preint , D_preint_imu , 1e-8 );
- ASSERT_MATRIX_APPROX(D_corrected , D_corrected_imu , 1e-8 );
- ASSERT_MATRIX_APPROX(D_corrected_imu , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(D_corrected , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_corrected_imu , x1_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_corrected , x1_exact , 1e-5 );
-
- // check optimal solutions
- ASSERT_MATRIX_APPROX(x0_optim , x0 , 1e-5 );
- ASSERT_NEAR(x0_optim.segment(3,4).norm(), 1.0 , 1e-8 );
- ASSERT_MATRIX_APPROX(bias_0 , bias_real , 1e-4 );
- ASSERT_MATRIX_APPROX(bias_1 , bias_real , 1e-4 );
- ASSERT_MATRIX_APPROX(D_optim , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_optim , x1_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(D_optim_imu , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_optim_imu , x1_exact , 1e-5 );
- ASSERT_NEAR(x1_optim.segment(3,4).norm(), 1.0 , 1e-8 );
- }
-
-};
-
-class Process_Factor_IMU_ODO : public Process_Factor_IMU
-{
- public:
- // Wolf objects
- SensorOdom3DPtr sensor_odo;
- ProcessorOdom3DPtr processor_odo;
- CaptureOdom3DPtr capture_odo;
-
- virtual void SetUp( ) override
- {
-
- // ===================================== IMU
- Process_Factor_IMU::SetUp();
-
- // ===================================== ODO
- string wolf_root = _WOLF_ROOT_DIR;
-
- SensorBasePtr sensor = problem->installSensor ("ODOM 3D", "Odometer", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml" );
- ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/src/examples/processor_odom_3D.yaml");
- sensor_odo = static_pointer_cast<SensorOdom3D>(sensor);
-
- processor_odo = static_pointer_cast<ProcessorOdom3D>(processor);
-
- processor_odo->setTimeTolerance(dt/2);
-
- // prevent this processor from voting by setting high thresholds :
- processor_odo->setAngleTurned(999);
- processor_odo->setDistTraveled(999);
- processor_odo->setMaxBuffLength(999);
- processor_odo->setMaxTimeSpan(999);
- }
-
- virtual void integrateAll() override
- {
- // ===================================== IMU
- Process_Factor_IMU::integrateAll();
-
- // ===================================== ODO
- Vector6s data;
- Vector3s p1 = x1_exact.head(3);
- Quaternions q1 (x1_exact.data() + 3);
- Vector3s dp = q0.conjugate() * (p1 - p0);
- Vector3s dth = log_q( q0.conjugate() * q1 );
- data << dp, dth;
-
- CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
-
- sensor_odo->process(capture_odo);
- }
-
- virtual void integrateAllTrajectories() override
- {
- // ===================================== IMU
- Process_Factor_IMU::integrateAllTrajectories();
-
- // ===================================== ODO
- Vector6s data;
- Vector3s p1 = x1_exact.head(3);
- Quaternions q1 (x1_exact.data() + 3);
- Vector3s dp = q0.conjugate() * (p1 - p0);
- Vector3s dth = log_q( q0.conjugate() * q1 );
- data << dp, dth;
-
- CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
-
- sensor_odo->process(capture_odo);
- }
-
- virtual void buildProblem() override
- {
- // ===================================== IMU
- Process_Factor_IMU::buildProblem();
-
- // ===================================== ODO
- // Process ODO for the callback to take effect
- data = Vector6s::Zero();
- capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
- processor_odo->process(capture_odo);
-
- }
-
-};
-
-TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001, .003, -.002, .001).finished());
- sensor_imu->getIntrinsic()->setState(initial_bias);
- configureAll();
- integrateAll();
- buildProblem();
- //Since we did not solve, hence bias estimates did not change yet, both capture should have the same calibration
- ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), initial_bias, 1e-8 );
- ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 );
-}
-
-TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = false;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 3,2,1;
- q0.coeffs() << 0.5,0.5,0.5,.5;
- v0 << 1,2,3;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = false;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 25;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random();
- w = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- configureAll();
- integrateAllTrajectories();
- buildProblem();
- string report = solveProblem(SolverManager::ReportVerbosity::BRIEF);
-
- assertAll();
-
- // Build optimal trajectory
- MatrixXs Trj_x_optim(10,Trj_D_preint_prc.cols());
- Scalar Dt = 0;
- SizeEigen i = 0;
- for (auto J_D_b : Buf_Jac_preint_prc)
- {
- VectorXs step = J_D_b * (bias_0 - bias_preint);
- VectorXs D_opt = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
- Trj_x_optim.col(i) = imu::composeOverState(x0_optim, D_opt, Dt);
- Dt += dt;
- i ++;
- }
-
- // Get optimal trajectory via getState()
- i = 0;
- t = 0;
- MatrixXs Trj_x_optim_prc(10,Trj_D_preint_prc.cols());
- for (int i = 0; i < Trj_x_optim_prc.cols(); i++)
- {
- Trj_x_optim_prc.col(i) = problem->getState(t);
- t += dt;
- }
-
- // printAll(report);
-
-// WOLF_INFO("------------------------------------");
-// WOLF_INFO("Exact x0 \n", x0 .transpose());
-// WOLF_INFO("Optim x0 \n", x0_optim .transpose());
-// WOLF_INFO("Optim x \n", Trj_x_optim_prc.transpose());
-// WOLF_INFO("Optim x1 \n", x1_optim .transpose());
-// WOLF_INFO("Exact x1 \n", x1_exact .transpose());
-// WOLF_INFO("------------------------------------");
-
- // The Mother of Asserts !!!
- ASSERT_MATRIX_APPROX(Trj_x_optim, Trj_x_optim_prc, 1e-6);
-
- // First and last trj states match known extrema
- ASSERT_MATRIX_APPROX(Trj_x_optim_prc.leftCols (1), x0, 1e-6);
- ASSERT_MATRIX_APPROX(Trj_x_optim_prc.rightCols(1), x1_exact, 1e-6);
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
-
- return RUN_ALL_TESTS();
-}
-
-/* Some notes :
- *
- * - Process_Factor_IMU_ODO.MotionConstant_PQv_b__PQv_b :
- * this test will not work + jacobian is rank deficient because of estimating both initial
- * and final velocities.
- * IMU data integration is done with correct biases (so this is the case of a calibrated IMU). Before solving the problem, we perturbate the initial bias.
- * We solve the problem by fixing all states excepted V1 and V2. while creating the factors, both velocities are corrected using the difference between the actual
- * bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the
- * difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this
- * solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
- *
- * - Bias evaluation with a precision of 1e-4 :
- * The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated IMU
- * Because of cross relations between acc and gyro biases (respectively a_b and w_b) it is difficult to expect a better estimation.
- * A small change in a_b can be cancelled by a small variation in w_b. in other words : there are local minima.
- * In addition, for Process_Factor_IMU tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
- * Errors tend to be distributed in different estimated variable when we get into a local minima (to minimize residuals in a least square sense).
- */
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
deleted file mode 100644
index e41007154f27c8c6de6845db934215df22d7c382..0000000000000000000000000000000000000000
--- a/test/gtest_IMU_tools.cpp
+++ /dev/null
@@ -1,629 +0,0 @@
-/*
- * gtest_imu_tools.cpp
- *
- * Created on: Jul 29, 2017
- * Author: jsola
- */
-
-#include "base/IMU_tools.h"
-#include "utils_gtest.h"
-
-using namespace Eigen;
-using namespace wolf;
-using namespace imu;
-
-TEST(IMU_tools, identity)
-{
- VectorXs id_true;
- id_true.setZero(10);
- id_true(6) = 1.0;
-
- VectorXs id = identity<Scalar>();
- ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
-}
-
-TEST(IMU_tools, identity_split)
-{
- VectorXs p(3), qv(4), v(3);
- Quaternions q;
-
- identity(p,q,v);
- ASSERT_MATRIX_APPROX(p, Vector3s::Zero(), 1e-10);
- ASSERT_QUATERNION_APPROX(q, Quaternions::Identity(), 1e-10);
- ASSERT_MATRIX_APPROX(v, Vector3s::Zero(), 1e-10);
-
- identity(p,qv,v);
- ASSERT_MATRIX_APPROX(p , Vector3s::Zero(), 1e-10);
- ASSERT_MATRIX_APPROX(qv, (Vector4s()<<0,0,0,1).finished(), 1e-10);
- ASSERT_MATRIX_APPROX(v , Vector3s::Zero(), 1e-10);
-}
-
-TEST(IMU_tools, inverse)
-{
- VectorXs d(10), id(10), iid(10), iiid(10), I(10);
- Vector4s qv;
- Scalar dt = 0.1;
-
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d << 0, 1, 2, qv, 7, 8, 9;
-
- id = inverse(d, dt);
-
- compose(id, d, dt, I);
- ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
- compose(d, id, -dt, I); // Observe -dt is reversed !!
- ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
-
- inverse(id, -dt, iid); // Observe -dt is reversed !!
- ASSERT_MATRIX_APPROX( d, iid, 1e-10);
- iiid = inverse(iid, dt);
- ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
-}
-
-TEST(IMU_tools, compose_between)
-{
- VectorXs dx1(10), dx2(10), dx3(10);
- Matrix<Scalar, 10, 1> d1, d2, d3;
- Vector4s qv;
- Scalar dt = 0.1;
-
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- dx1 << 0, 1, 2, qv, 7, 8, 9;
- d1 = dx1;
- qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
- dx2 << 9, 8, 7, qv, 2, 1, 0;
- d2 = dx2;
-
- // combinations of templates for sum()
- compose(dx1, dx2, dt, dx3);
- compose(d1, d2, dt, d3);
- ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
-
- compose(d1, dx2, dt, dx3);
- d3 = compose(dx1, d2, dt);
- ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
-
- // minus(d1, d3) should recover delta_2
- VectorXs diffx(10);
- Matrix<Scalar,10,1> diff;
- between(d1, d3, dt, diff);
- ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
-
- // minus(d3, d1, -dt) should recover inverse(d2, dt)
- diff = between(d3, d1, -dt);
- ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10);
-}
-
-TEST(IMU_tools, compose_between_with_state)
-{
- VectorXs x(10), d(10), x2(10), x3(10), d2(10), d3(10);
- Vector4s qv;
- Scalar dt = 0.1;
-
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- x << 0, 1, 2, qv, 7, 8, 9;
- qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
- d << 9, 8, 7, qv, 2, 1, 0;
-
- composeOverState(x, d, dt, x2);
- x3 = composeOverState(x, d, dt);
- ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
-
- // betweenStates(x, x2) should recover d
- betweenStates(x, x2, dt, d2);
- d3 = betweenStates(x, x2, dt);
- ASSERT_MATRIX_APPROX(d2, d, 1e-10);
- ASSERT_MATRIX_APPROX(d3, d, 1e-10);
- ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10);
-
- // x + (x2 - x) = x2
- ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt), dt), x2, 1e-10);
-
- // (x + d) - x = d
- ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
-}
-
-TEST(IMU_tools, lift_retract)
-{
- VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
-
- // transform to delta
- VectorXs delta = exp_IMU(d_min);
-
- // expected delta
- Vector3s dp = d_min.head(3);
- Quaternions dq = v2q(d_min.segment(3,3));
- Vector3s dv = d_min.tail(3);
- VectorXs delta_true(10); delta_true << dp, dq.coeffs(), dv;
- ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
-
- // transform to a new tangent -- should be the original tangent
- VectorXs d_from_delta = log_IMU(delta);
- ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
-
- // transform to a new delta -- should be the previous delta
- VectorXs delta_from_d = exp_IMU(d_from_delta);
- ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
-}
-
-TEST(IMU_tools, plus)
-{
- VectorXs d1(10), d2(10), d3(10);
- VectorXs err(9);
- Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv, 7, 8, 9;
- err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09;
-
- d3.head(3) = d1.head(3) + err.head(3);
- d3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
- d3.tail(3) = d1.tail(3) + err.tail(3);
-
- plus(d1, err, d2);
- ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXs::Zero(9), 1e-10);
-}
-
-TEST(IMU_tools, diff)
-{
- VectorXs d1(10), d2(10);
- Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv, 7, 8, 9;
- d2 = d1;
-
- VectorXs err(9);
- diff(d1, d2, err);
- ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
- ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 1e-10);
-
- VectorXs d3(10);
- d3.setRandom(); d3.segment(3,4).normalize();
- err.head(3) = d3.head(3) - d1.head(3);
- err.segment(3,3) = log_q(Quaternions(d1.data()+3).conjugate()*Quaternions(d3.data()+3));
- err.tail(3) = d3.tail(3) - d1.tail(3);
-
- ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
-
-}
-
-TEST(IMU_tools, compose_jacobians)
-{
- VectorXs d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
- VectorXs t1(9), t2(9); // tangents
- Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
- Vector4s qv1, qv2;
- Scalar dt = 0.1;
- Scalar dx = 1e-6;
- qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv1, 7, 8, 9;
- qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
- d2 << 9, 8, 7, qv2, 2, 1, 0;
-
- // analytical jacobians
- compose(d1, d2, dt, d3, J1_a, J2_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<9; i++)
- {
- t1 . setZero();
- t1(i) = dx;
-
- // Jac wrt first delta
- d1_pert = plus(d1, t1); // (d1 + t1)
- d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2
- t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 }
- J1_n.col(i) = t2/dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx
-
- // Jac wrt second delta
- d2_pert = plus(d2, t1); // (d2 + t1)
- d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1)
- t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 }
- J2_n.col(i) = t2/dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
- ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-}
-
-TEST(IMU_tools, diff_jacobians)
-{
- VectorXs d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
- VectorXs t1(9), t2(9); // tangents
- Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
- Vector4s qv1, qv2;
- Scalar dx = 1e-6;
- qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv1, 7, 8, 9;
- qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
- d2 << 9, 8, 7, qv2, 2, 1, 0;
-
- // analytical jacobians
- diff(d1, d2, d3, J1_a, J2_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<9; i++)
- {
- t1 . setZero();
- t1(i) = dx;
-
- // Jac wrt first delta
- d1_pert = plus(d1, t1); // (d1 + t1)
- d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
- t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 }
- J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
-
- // Jac wrt second delta
- d2_pert = plus(d2, t1); // (d2 + t1)
- d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
- t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 }
- J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
- ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-}
-
-TEST(IMU_tools, body2delta_jacobians)
-{
- VectorXs delta(10), delta_pert(10); // deltas
- VectorXs body(6), pert(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
- Vector4s qv;;
- Scalar dt = 0.1;
- Scalar dx = 1e-6;
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- delta << 0, 1, 2, qv, 7, 8, 9;
- body << 1, 2, 3, 3, 2, 1;
-
- // analytical jacobians
- body2delta(body, dt, delta, J_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<6; i++)
- {
- pert . setZero();
- pert(i) = dx;
-
- // Jac wrt first delta
- delta_pert = body2delta(body + pert, dt); // delta(body + pert)
- tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
- J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
-}
-
-TEST(motion2data, zero)
-{
- Vector6s motion;
- Vector6s bias;
- Quaternions q;
-
- motion .setZero();
- bias .setZero();
- q .setIdentity();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true << -gravity(), Vector3s::Zero();
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, motion)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- g_extended << gravity() , Vector3s::Zero();
-
- motion << 1,2,3, 4,5,6;
- bias .setZero();
- q .setIdentity();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true = motion - g_extended;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, bias)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- g_extended << gravity() , Vector3s::Zero();
-
- motion .setZero();
- bias << 1,2,3, 4,5,6;
- q .setIdentity();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true = bias - g_extended;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, orientation)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- g_extended << gravity() , Vector3s::Zero();
-
- motion .setZero();
- bias .setZero();
- q = v2q(Vector3s(M_PI/2, 0, 0)); // turn 90 deg in X axis
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true << 0,-gravity()(2),0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, AllRandom)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- motion .setRandom();
- bias .setRandom();
- q.coeffs() .setRandom().normalize();
-
- g_extended << q.conjugate()*gravity() , Vector3s::Zero();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true = motion + bias - g_extended;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-/* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientaiton
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * return: the preintegrated delta
- */
-VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
-{
- VectorXs data(6);
- VectorXs body(6);
- VectorXs delta(10);
- VectorXs Delta(10), Delta_plus(10);
- Delta << 0,0,0, 0,0,0,1, 0,0,0;
- Quaternions q(q0);
- for (int n = 0; n<N; n++)
- {
- data = motion2data(motion, q, bias_real);
- q = q*exp_q(motion.tail(3)*dt);
- body = data - bias_preint;
- delta = body2delta(body, dt);
- Delta_plus = compose(Delta, delta, dt);
- Delta = Delta_plus;
- }
- return Delta;
-}
-
-/* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientaiton
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
-VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
-{
- VectorXs data(6);
- VectorXs body(6);
- VectorXs delta(10);
- Matrix<Scalar, 9, 6> J_d_d, J_d_b;
- Matrix<Scalar, 9, 9> J_D_D, J_D_d;
- VectorXs Delta(10), Delta_plus(10);
- Quaternions q;
-
- Delta << 0,0,0, 0,0,0,1, 0,0,0;
- J_D_b.setZero();
- q = q0;
- for (int n = 0; n<N; n++)
- {
- // Simulate data
- data = motion2data(motion, q, bias_real);
- q = q*exp_q(motion.tail(3)*dt);
- // Motion::integrateOneStep()
- { // IMU::computeCurrentDelta
- body = data - bias_preint;
- body2delta(body, dt, delta, J_d_d);
- J_d_b = - J_d_d;
- }
- { // IMU::deltaPlusDelta
- compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
- }
- // Motion:: jac calib
- J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
- // Motion:: buffer
- Delta = Delta_plus;
- }
- return Delta;
-}
-
-TEST(IMU_tools, integral_jacobian_bias)
-{
- VectorXs Delta(10), Delta_pert(10); // deltas
- VectorXs bias_real(6), pert(6), bias_pert(6), bias_preint(6);
- VectorXs body(6), data(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
- Scalar dt = 0.001;
- Scalar dx = 1e-4;
- Quaternions q0(3, 4, 5, 6); q0.normalize();
- motion << .1, .2, .3, .3, .2, .1;
- bias_real << .002, .004, .001, .003, .002, .001;
- bias_preint << .004, .005, .006, .001, .002, .003;
-
- int N = 500; // # steps of integration
-
- // Analytical Jacobians
- Delta = integrateDelta(N, q0, motion, bias_real, bias_preint, dt, J_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<6; i++)
- {
- pert . setZero();
- pert(i) = dx;
-
- bias_pert = bias_preint + pert;
- Delta_pert = integrateDelta(N, q0, motion, bias_real, bias_pert, dt);
- tang = diff(Delta, Delta_pert); // Delta(bias + pert) -- Delta(bias)
- J_n.col(i) = tang/dx; // ( Delta(bias + pert) -- Delta(bias) ) / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
-}
-
-TEST(IMU_tools, delta_correction)
-{
- VectorXs Delta(10), Delta_preint(10), Delta_corr; // deltas
- VectorXs bias(6), pert(6), bias_preint(6);
- VectorXs body(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
- Vector4s qv;;
- Scalar dt = 0.001;
- Quaternions q0(3, 4, 5, 6); q0.normalize();
- motion << 1, 2, 3, 3, 2, 1; motion *= .1;
-
- int N = 1000; // # steps of integration
-
- // preintegration with correct bias
- bias << .004, .005, .006, .001, .002, .003;
- Delta = integrateDelta(N, q0, motion, bias, bias, dt);
-
- // preintegration with wrong bias
- pert << .002, -.001, .003, -.0003, .0002, -.0001;
- bias_preint = bias + pert;
- Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
-
- // correct perturbated
- Vector9s step = J_b*(bias-bias_preint);
- Delta_corr = plus(Delta_preint, step);
-
- // Corrected delta should match real delta
- ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5);
-
- // diff between real and corrected should be zero
- ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9s::Zero(), 1e-5);
-
- // diff between preint and corrected deltas should be the jacobian-computed step
- ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
-}
-
-TEST(IMU_tools, full_delta_residual)
-{
- VectorXs x1(10), x2(10);
- VectorXs Delta(10), Delta_preint(10), Delta_corr(10);
- VectorXs Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
- VectorXs bias(6), pert(6), bias_preint(6), bias_null(6);
- VectorXs body(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
- Scalar dt = 0.001;
- Quaternions q0; q0.setIdentity();
-
- x1 << 0,0,0, 0,0,0,1, 0,0,0;
- motion << .3, .2, .1, .1, .2, .3; // acc and gyro
-// motion << .3, .2, .1, .0, .0, .0; // only acc
-// motion << .0, .0, .0, .1, .2, .3; // only gyro
- bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
- bias_null << 0, 0, 0, 0, 0, 0;
-// bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003;
- bias_preint = bias_null;
-
- int N = 1000; // # steps of integration
-
- // preintegration with no bias
- Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt);
-
- // final state
- x2 = composeOverState(x1, Delta_real, 1000*dt);
-
- // preintegration with the correct bias
- Delta = integrateDelta(N, q0, motion, bias, bias, dt);
-
- ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4);
-
- // preintegration with wrong bias
- Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
-
- // compute correction step
- Vector9s step = J_b*(bias-bias_preint);
-
- // correct preintegrated delta
- Delta_corr = plus(Delta_preint, step);
-
- // Corrected delta should match real delta
- ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
-
- // diff between real and corrected should be zero
- ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9s::Zero(), 1e-3);
-
- // expected delta
- Delta_exp = betweenStates(x1, x2, N*dt);
-
- ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
-
- // compute diff between expected and corrected
-// Matrix<Scalar, 9, 9> J_err_exp, J_err_corr;
- diff(Delta_exp, Delta_corr, Delta_err); //, J_err_exp, J_err_corr);
-
- ASSERT_MATRIX_APPROX(Delta_err, Vector9s::Zero(), 1e-3);
-
- // compute error between expected and preintegrated
- Delta_err = diff(Delta_preint, Delta_exp);
-
- // diff between preint and corrected deltas should be the jacobian-computed step
- ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3);
- /* This implies:
- * - Since D_corr is expected to be similar to D_exp
- * step ~ diff(Delta_preint, Delta_exp)
- * - the residual can be computed with a regular '-' :
- * residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint)
- */
-
-// WOLF_TRACE("Delta real: ", Delta_real.transpose());
-// WOLF_TRACE("x2: ", x2.transpose());
-// WOLF_TRACE("Delta: ", Delta.transpose());
-// WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
-// WOLF_TRACE("Jac bias: \n", J_b);
-// WOLF_TRACE("Delta step: ", step.transpose());
-// WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
-// WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
-// WOLF_TRACE("Delta err: ", Delta_err.transpose());
-// WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
deleted file mode 100644
index d6e3b9b1c2a2154677f4a2a77094b4bac93146f0..0000000000000000000000000000000000000000
--- a/test/gtest_SE3.cpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/**
- * \file gtest_SE3.cpp
- *
- * Created on: Feb 2, 2019
- * \author: jsola
- */
-
-
-#include "base/SE3.h"
-#include "utils_gtest.h"
-
-
-
-using namespace Eigen;
-using namespace wolf;
-using namespace three_D;
-
-TEST(SE3, exp_0)
-{
- Vector6s tau = Vector6s::Zero();
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
-
- ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
-}
-
-TEST(SE3, log_I)
-{
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
- Vector6s tau = Vector6s::Zero();
-
- ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
-}
-
-TEST(SE3, exp_log)
-{
- Vector6s tau = Vector6s::Random() / 5.0;
- Vector7s pose = exp_SE3(tau);
-
- ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
- ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
-}
-
-TEST(SE3, exp_of_multiple)
-{
- Vector6s tau, tau2, tau3;
- Vector7s pose, pose2, pose3;
- tau = Vector6s::Random() / 5;
- pose << exp_SE3(tau);
- tau2 = 2*tau;
- tau3 = 3*tau;
- pose2 = compose(pose, pose);
- pose3 = compose(pose2, pose);
-
- // 1
- ASSERT_MATRIX_APPROX(exp_SE3(tau), pose, 1e-8);
-
- // 2
- ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8);
-
- // 3
- ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8);
-}
-
-TEST(SE3, log_of_power)
-{
- Vector6s tau, tau2, tau3;
- Vector7s pose, pose2, pose3;
- tau = Vector6s::Random() / 5;
- pose << exp_SE3(tau);
- tau2 = 2*tau;
- tau3 = 3*tau;
- pose2 = compose(pose, pose);
- pose3 = compose(pose2, pose);
-
- // 1
- ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
-
- // 2
- ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8);
-
- // 3
- ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8);
-}
-
-TEST(SE3, interpolate_I_xyz)
-{
- Vector7s p1, p2, p_pre;
-
- p1 << 0,0,0, 0,0,0,1;
- p2 << 1,2,3, 0,0,0,1;
- Scalar t = 0.2;
-
- interpolate(p1, p2, t, p_pre);
-
- Vector7s p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, interpolate_xyz_xyz)
-{
- Vector7s p1, p2, p_pre;
-
- p1 << 1,2,3, 0,0,0,1;
- p2 << 2,4,6, 0,0,0,1;
- Scalar t = 0.2;
-
- interpolate(p1, p2, t, p_pre);
-
- Vector7s p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, interpolate_I_qx)
-{
- Vector7s p1, p2, p_pre;
-
- p1 << 0,0,0, 0,0,0,1;
- p2 << 0,0,0, 1,0,0,0;
- Scalar t = 0.5;
-
- interpolate(p1, p2, t, p_pre);
-
- Vector7s p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, interpolate_I_qy)
-{
- Vector7s p1, p2, p_pre;
-
- p1 << 0,0,0, 0,0,0,1;
- p2 << 0,0,0, 0,1,0,0;
- Scalar t = 0.5;
-
- interpolate(p1, p2, t, p_pre);
-
- Vector7s p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, interpolate_I_qz)
-{
- Vector7s p1, p2, p_pre;
-
- p1 << 0,0,0, 0,0,0,1;
- p2 << 0,0,0, 0,0,1,0;
- Scalar t = 0.5;
-
- interpolate(p1, p2, t, p_pre);
-
- Vector7s p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, interpolate_I_qxyz)
-{
- Vector7s p1, p2, dp, p_pre, p_gt;
- Vector3s da;
-
- da.setRandom(); da /= 5;
-
- p1 << 0,0,0, 0,0,0,1;
- dp << 0,0,0, exp_q(da).coeffs();
- p_gt = compose(p1, dp);
- p2 = compose(p_gt, dp);
- Scalar t = 0.5;
-
- interpolate(p1, p2, t, p_pre);
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, interpolate_half)
-{
- Vector7s p1, p2, dp, p_pre, p_gt;
- Vector6s da;
-
- p1.setRandom(); p1.tail(4).normalize();
-
- da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
-
- // compose double, then interpolate 1/2
-
- p_gt = compose(p1, dp);
- p2 = compose(p_gt, dp);
-
- Scalar t = 0.5;
- interpolate(p1, p2, t, p_pre);
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, interpolate_third)
-{
- Vector7s p1, p2, dp, dp2, dp3, p_pre, p_gt;
- Vector6s da;
-
- p1.setRandom(); p1.tail(4).normalize();
-
- da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
- dp2 = compose(dp, dp);
- dp3 = compose(dp2, dp);
-
- // compose triple, then interpolate 1/3
-
- p_gt = compose(p1, dp);
- p2 = compose(p1, dp3);
-
- Scalar t = 1.0/3.0;
- interpolate(p1, p2, t, p_pre);
-
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
-}
-
-TEST(SE3, toSE3_I)
-{
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
- Matrix4s R;
- toSE3(pose, R);
-
- ASSERT_MATRIX_APPROX(R, Matrix4s::Identity(), 1e-8);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
deleted file mode 100644
index 71217c8e6df3dc2cd0a9ce5bc2e73c97ec19c72a..0000000000000000000000000000000000000000
--- a/test/gtest_capture_base.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/*
- * gtest_capture_base.cpp
- *
- * Created on: Apr 11, 2017
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "base/capture/capture_base.h"
-#include "base/state_angle.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(CaptureBase, ConstructorNoSensor)
-{
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
-
- ASSERT_EQ(C->getTimeStamp(), 1.2);
- ASSERT_FALSE(C->getFrame());
- ASSERT_FALSE(C->getProblem());
- ASSERT_FALSE(C->getSensor());
-}
-
-TEST(CaptureBase, ConstructorWithSensor)
-{
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
- ASSERT_EQ(C->getTimeStamp(), 1.5);
- ASSERT_FALSE(C->getFrame());
- ASSERT_FALSE(C->getProblem());
- ASSERT_TRUE(C->getSensor());
- ASSERT_FALSE(C->getSensorP());
- ASSERT_FALSE(C->getSensorO());
-}
-
-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));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", p, o, i, 2));
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
-
- // query sensor blocks
- ASSERT_EQ(S->getP(), p);
- ASSERT_EQ(S->getO(), o);
- ASSERT_EQ(S->getIntrinsic(), i);
-
- // query capture blocks
- ASSERT_EQ(C->getSensorP(), p);
- ASSERT_EQ(C->getSensorO(), o);
- ASSERT_EQ(C->getSensorIntrinsic(), i);
-}
-
-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", nullptr, nullptr, nullptr, 2, true, true)); // last 2 '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
- // ASSERT_EQ(S->getP(), p);
- // ASSERT_EQ(S->getO(), o);
- // ASSERT_EQ(S->getIntrinsic(), i);
-
- // query capture blocks
- ASSERT_EQ(C->getSensorP(), p);
- ASSERT_EQ(C->getSensorO(), o);
- ASSERT_EQ(C->getSensorIntrinsic(), i);
-}
-
-TEST(CaptureBase, addFeature)
-{
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
- ASSERT_FALSE(C->getFeatureList().empty());
- ASSERT_EQ(C->getFeatureList().front(), f);
-}
-
-TEST(CaptureBase, addFeatureList)
-{
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
- FeatureBasePtr f_first = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
-
- // make a list to add
- std::list<FeatureBasePtr> fl;
- for (int i = 0; i<3; i++)
- {
- fl.push_back(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
- }
- FeatureBasePtr f_last = fl.back();
-
- C->addFeatureList((fl));
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 4);
- ASSERT_EQ(fl.size(), (unsigned int) 0); // features have been moved, not copied
- ASSERT_EQ(C->getFeatureList().front(), f_first);
- ASSERT_EQ(C->getFeatureList().back(), f_last);
-}
-
-TEST(CaptureBase, process)
-{
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
- ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
- C->setSensor(S);
- ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
deleted file mode 100644
index cbe91a6fdf1c276f55bf50e9c32451f0618feb5b..0000000000000000000000000000000000000000
--- a/test/gtest_ceres_manager.cpp
+++ /dev/null
@@ -1,637 +0,0 @@
-/*
- * gtest_ceres_manager.cpp
- *
- * Created on: Jun, 2018
- * Author: jvallve
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/problem.h"
-#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
-#include "base/capture/capture_void.h"
-#include "base/factor/factor_pose_2D.h"
-#include "base/factor/factor_quaternion_absolute.h"
-#include "base/solver/solver_manager.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/local_parametrization_angle.h"
-#include "base/local_parametrization_quaternion.h"
-
-#include "ceres/ceres.h"
-
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-WOLF_PTR_TYPEDEFS(CeresManagerWrapper);
-class CeresManagerWrapper : public CeresManager
-{
- public:
- CeresManagerWrapper(const ProblemPtr& wolf_problem) :
- CeresManager(wolf_problem)
- {
- };
-
- bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st)
- {
- return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st)
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st)
- {
- return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- int numStateBlocks()
- {
- return ceres_problem_->NumParameterBlocks();
- };
-
- int numFactors()
- {
- return ceres_problem_->NumResidualBlocks();
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
- {
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
- state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
- ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
- };
-
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end();
- };
-
-};
-
-TEST(CeresManager, Create)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // check double ointers to branches
- ASSERT_EQ(P, ceres_manager_ptr->getProblem());
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, DoubleAddStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // add stateblock again
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, UpdateStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock unfixed
- ASSERT_FALSE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
-
- // Fix frame
- sb_ptr->fix();
-
- // update solver manager
- ceres_manager_ptr->update();
-
- // check stateblock fixed
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddUpdateStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // Fix state block
- sb_ptr->fix();
-
- // update solver manager
- ceres_manager_ptr->update();
-
- // check stateblock fixed
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, RemoveStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock
- ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddRemoveStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check no stateblocks
- ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, RemoveUpdateStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, DoubleRemoveStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver manager
- ceres_manager_ptr->update();
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- // update solver
- ceres_manager_ptr->update();
-
- // check factor
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, DoubleAddFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- // add factor again
- P->addFactor(c);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check factor
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, RemoveFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- // update solver
- ceres_manager_ptr->update();
-
- // remove factor
- P->removeFactor(c);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check factor
- ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddRemoveFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
- // remove factor
- P->removeFactor(c);
-
- ASSERT_TRUE(P->getFactorNotificationMap().empty());
-
- // update solver
- ceres_manager_ptr->update();
-
- // check factor
- ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, DoubleRemoveFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- // update solver
- ceres_manager_ptr->update();
-
- // remove factor
- P->removeFactor(c);
-
- // update solver
- ceres_manager_ptr->update();
-
- // remove factor
- P->removeFactor(c);
-
- ASSERT_DEATH({
- // update solver
- ceres_manager_ptr->update();},"");
-
- // check factor
- ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddStateBlockLocalParam)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector1s state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, RemoveLocalParam)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector1s state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // Remove local param
- sb_ptr->removeLocalParametrization();
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock
- ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddLocalParam)
-{
- ProblemPtr P = Problem::create("PO 2D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- Vector1s state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock
- ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
-
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check stateblock
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, FactorsRemoveLocalParam)
-{
- ProblemPtr P = Problem::create("PO 3D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
-
- // update solver
- ceres_manager_ptr->update();
-
- // check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
-
- // check factor
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
- // remove local param
- F->getO()->removeLocalParametrization();
-
- // update solver
- ceres_manager_ptr->update();
-
- // check local param
- ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
-
- // check factor
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, FactorsUpdateLocalParam)
-{
- ProblemPtr P = Problem::create("PO 3D");
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
-
- // update solver
- ceres_manager_ptr->update();
-
- // check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
-
- // check factor
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
- // remove local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
- F->getO()->setLocalParametrization(local_param_ptr);
-
- // update solver
- ceres_manager_ptr->update();
-
- // check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),local_param_ptr));
-
- // check factor
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
deleted file mode 100644
index 649ba85e9b5477d38498b3f80678eaf25e87697b..0000000000000000000000000000000000000000
--- a/test/gtest_eigen_predicates.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-#include "utils_gtest.h"
-
-#include "base/eigen_predicates.h"
-
-TEST(TestEigenPredicates, TestEigenDynPredZero)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::MatrixXs::Zero(4,4);
- B = Eigen::MatrixXs::Random(4,4);
- C = Eigen::MatrixXs::Ones(4,4) * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredZero)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::Matrix4s::Zero();
- B = Eigen::Matrix4s::Random();
- C = Eigen::Matrix4s::Ones() * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
-{
- Eigen::MatrixXs A, B, C;
-
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
-{
- Eigen::Quaternions A, B, C;
-
- /// @todo which version of Eigen provides this ?
-// A = Eigen::Quaternions::UnitRandom();
-
- A.coeffs() = Eigen::Vector4s::Random().normalized();
- B.coeffs() = Eigen::Vector4s::Random().normalized();
- C = A;
-
- EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
-{
- Eigen::Quaternions A, B;
-
- A = Eigen::Quaternions::Identity();
- B.coeffs() = Eigen::Vector4s::Random().normalized();
-
- EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsApprox)
-{
- wolf::Scalar a = M_PI;
- wolf::Scalar b = -M_PI;
- wolf::Scalar c = 0;
-
- EXPECT_TRUE(wolf::pred_angle_is_approx(a, b, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_is_approx(a, c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsZero)
-{
- wolf::Scalar a = 0;
- wolf::Scalar b = M_PI;
- wolf::Scalar c = 2.*M_PI;
-
- EXPECT_TRUE(wolf::pred_angle_zero(a, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_zero(b, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_angle_zero(c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
deleted file mode 100644
index 6c8990c291af5bb43dc9e042f9ca60dc84854e75..0000000000000000000000000000000000000000
--- a/test/gtest_factor_IMU.cpp
+++ /dev/null
@@ -1,2840 +0,0 @@
-/**
- * \file gtest_factor_imu.cpp
- *
- * Created on: Jan 01, 2017
- * \author: Dinesh Atchuthan
- */
-
-//Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/factor/factor_pose_3D.h"
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/processor/processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include <iostream>
-#include <fstream>
-
-//#define GET_RESIDUALS
-
-using namespace Eigen;
-using namespace std;
-using namespace wolf;
-
-/*
- * This test is designed to test IMU biases in a particular case : perfect IMU, not moving.
- * var(b1,b2,p2,v2,q2), inv(p1,q1,v1); fac1: imu+(b1=b2)
- * So there is no odometry data.
- * IMU data file should first contain initial conditions, then the time_step between each new imu and the last one (in seconds) data,
- * and finally the last stateafter integration and the last timestamp, Then it should contain all IMU data and related timestamps
- */
-
-class FactorIMU_biasTest_Static_NullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr KF0;
- FrameBasePtr KF1;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
-
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- origin_bias << 0,0,0, 0,0,0;
- expected_final_state = x_origin; //null bias + static
-
- //set origin of the problem
- KF0 = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
-
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
-
- }
-
- KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- KF0->unfix();
- KF1->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr KF0;
- FrameBasePtr KF1;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
- origin_bias << 0.02, 0.05, 0.1, 0,0,0;
-
- expected_final_state = x_origin; //null bias + static
-
- //set origin of the problem
- KF0 = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
- data_imu = data_imu + origin_bias;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- KF0->unfix();
- KF1->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
-// ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-// ceres_options.max_line_search_step_contraction = 1e-3;
-// ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
- origin_bias << 0, 0, 0, 0.0002, 0.0005, 0.001;
-
- expected_final_state = x_origin; //null bias + static,
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
- data_imu = data_imu + origin_bias;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
- origin_bias << 0.002, 0.005, 0.1, 0.07,-0.035,-0.1;
- origin_bias *= .01;
-
- expected_final_state = x_origin; //null bias + static
-
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
- data_imu = data_imu + origin_bias;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero()); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
-
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- Eigen::Vector6s data_imu;
- data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction
- expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
-
- origin_bias<< 0,0,0,0,0,0;
-
- expected_final_state = x_origin;
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
-
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- Eigen::Vector6s data_imu;
- origin_bias = Eigen::Vector6s::Random() * 0.001;
- data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction
- data_imu = data_imu + origin_bias;
- expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- Eigen::Vector6s data_imu, data_imu_initial;
- origin_bias = Eigen::Vector6s::Random() * 0.001;
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
- data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only
- data_imu_initial = data_imu;
-
- // Expected state after one second integration
- Eigen::Quaternions quat_comp(Eigen::Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data_imu.tail(3)*1);
- expected_final_state << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.1s = 5 deg => 5 * M_PI/180
-
- data_imu = data_imu + origin_bias; // bias measurements
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- //gravity measure depends on current IMU orientation + bias
- //use data_imu_initial to measure gravity from real orientation of IMU then add biases
- data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setData(data_imu);
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 10,-3,4;
- t.set(0);
-
- Eigen::Vector6s data_imu, data_imu_initial;
- origin_bias = Eigen::Vector6s::Random();
- origin_bias << 0,0,0, 0,0,0;
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
- data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only
- data_imu_initial = data_imu;
-
- // Expected state after one second integration
- Eigen::Quaternions quat_comp(Eigen::Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data_imu.tail(3)*1);
-
- // rotated at 5 deg/s for 1s = 5 deg => 5 * M_PI/180
- // no other acceleration : we should still be moving at initial velocity
- // position = V*dt, dt = 1s
- expected_final_state << 10,-3,4, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 10,-3,4;
-
- data_imu = data_imu + origin_bias; // bias measurements
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- //gravity measure depends on current IMU orientation + bias
- //use data_imu_initial to measure gravity from real orientation of IMU then add biases
- data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setData(data_imu);
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-// var(b1,b2), inv(p1,q1,v1,p2,q2,v2); fac1: imu(p,q,v)+(b1=b2)
-
-class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
- t.set(0);
- Eigen::Quaternions quat_current(Eigen::Quaternions::Identity());
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s
-
- Scalar dt(0.001);
- TimeStamp ts(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- while( ts.get() < 1 )
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts += dt;
-
- rateOfTurn << .1, .2, .3; //to have rate of turn > 0 deg/s
- data_imu.head(3) = quat_current.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
- data_imu.tail(3) = rateOfTurn;
-
- //compute current orientaton taking this measure into account
- quat_current = quat_current * wolf::v2q(rateOfTurn*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- }
-
- expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- ProblemPtr problem;
- CeresManagerPtr ceres_manager;
- SensorBasePtr sensor;
- SensorIMUPtr sensor_imu;
- SensorOdom3DPtr sensor_odo;
- ProcessorBasePtr processor;
- ProcessorIMUPtr processor_imu;
- ProcessorOdom3DPtr processor_odo;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- CaptureIMUPtr capture_imu;
- CaptureOdom3DPtr capture_odo;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- problem = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- sensor = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- sensor_imu = std::static_pointer_cast<SensorIMU>(sensor);
- processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
-
- // SENSOR + PROCESSOR ODOM 3D
- sensor = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
- sensor_odo = std::static_pointer_cast<SensorOdom3D>(sensor);
-
- sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
- sensor_odo->setNoiseStd((sensor_odo->getNoiseStd()/10.0).eval());
- WOLF_TRACE("IMU cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
- WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose());
-
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.0099;
- prc_odom3D_params->max_buff_length = 1000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000;
- prc_odom3D_params->angle_turned = 1000;
-
- processor = problem->installProcessor("ODOM 3D", "odom", sensor_odo, prc_odom3D_params);
- processor_odo = std::static_pointer_cast<ProcessorOdom3D>(processor);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
-// origin_bias /= 100;
- t.set(0);
-
- //set origin of the problem
-
- origin_KF = processor_imu->setOrigin(x_origin, t);
- processor_odo->setOrigin(origin_KF);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()),
- data_odo(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s
-
- TimeStamp t_imu(0.0), t_odo(0.0);
- Scalar dt_imu(0.001), dt_odo(.01);
-
- capture_imu = std::make_shared<CaptureIMU> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
-
- capture_odo = std::make_shared<CaptureOdom3D>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
- sensor_odo->process(capture_odo);
- t_odo += dt_odo; //first odometry data will be processed at this timestamp
-
- // ground truth for quaternion:
- Eigen::Quaternions quat_odo(Eigen::Quaternions::Identity());
- Eigen::Quaternions quat_imu(Eigen::Quaternions::Identity());
-
- WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
-
- for(unsigned int i = 1; i<=1000; i++)
- {
-
- // PROCESS IMU DATA
- // Time and data variables
- t_imu += dt_imu;
-
-// rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s
- rateOfTurn << 5, 10, 15; // deg/s
- data_imu.tail<3>() = rateOfTurn * M_PI/180.0;
- data_imu.head<3>() = quat_imu.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- quat_odo = quat_odo * wolf::v2q(data_imu.tail(3)*dt_imu);
- quat_imu = quat_imu * wolf::v2q(data_imu.tail(3)*dt_imu);
-
- //set timestamp, add bias, set data and process
- capture_imu->setTimeStamp(t_imu);
- data_imu = data_imu + origin_bias;
- capture_imu->setData(data_imu);
- sensor_imu->process(capture_imu);
-
- WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
-
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- if(t_imu.get() >= t_odo.get())
- {
- WOLF_TRACE("====== create ODOM KF ========");
-// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
-
- // PROCESS ODOM 3D DATA
- data_odo.head(3) << 0,0,0;
- data_odo.tail(3) = q2v(quat_odo);
- capture_odo->setTimeStamp(t_odo);
- capture_odo->setData(data_odo);
-
-// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
-
- sensor_odo->process(capture_odo);
-
-// WOLF_TRACE("Jac calib: ", processor_imu->getOrigin()->getJacobianCalib().row(0));
-// WOLF_TRACE("orig calib: ", processor_imu->getOrigin()->getCalibration().transpose());
-// WOLF_TRACE("orig calib preint: ", processor_imu->getOrigin()->getCalibrationPreint().transpose());
-
- //prepare next odometry measurement
- quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odo += dt_odo;
- break;
- }
- }
-
- expected_final_state.resize(10);
- expected_final_state << 0,0,0, quat_imu.x(), quat_imu.y(), quat_imu.z(), quat_imu.w(), 0,0,0;
-
- last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t_imu);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
-
- CaptureBasePtr origin_CB = origin_KF->getCaptureOf(sensor_imu);
- CaptureMotionPtr last_CM = std::static_pointer_cast<CaptureMotion>(last_KF ->getCaptureOf(sensor_imu));
-
-// WOLF_TRACE("KF1 calib : ", origin_CB->getCalibration().transpose());
-// WOLF_TRACE("KF2 calib pre: ", last_CM ->getCalibrationPreint().transpose());
-// WOLF_TRACE("KF2 calib : ", last_CM ->getCalibration().transpose());
-// WOLF_TRACE("KF2 delta pre: ", last_CM ->getDeltaPreint().transpose());
-// WOLF_TRACE("KF2 delta cor: ", last_CM ->getDeltaCorrected(origin_CB->getCalibration()).transpose());
-// WOLF_TRACE("KF2 jacob : ", last_CM ->getJacobianCalib().row(0));
-
- // ==================================================== show problem status
-
- problem->print(4,1,1,1);
-
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- SensorOdom3DPtr sen_odom3D;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- ProcessorOdom3DPtr processor_ptr_odom3D;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- // SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.9999;
- prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
- origin_bias *= .1;
- t.set(0);
- Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
- processor_ptr_odom3D->setOrigin(origin_KF);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn; //deg/s
- rateOfTurn << 0,90,0;
- VectorXs D_cor(10);
-
- Scalar dt(0.0010), dt_odom(1.0);
- TimeStamp ts(0.0), t_odom(0.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr);
- sen_odom3D->process(mot_ptr);
- //first odometry data will be processed at this timestamp
- t_odom.set(t_odom.get() + dt_odom);
-
- data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
-
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- for(unsigned int i = 1; i<=1000; i++)
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts.set(i*dt);
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- D_cor = processor_ptr_imu->getLast()->getDeltaCorrected(origin_bias);
-
- if(ts.get() >= t_odom.get())
- {
- WOLF_TRACE("X_preint(t) : ", wolf_problem_ptr_->getState(ts).transpose());
-
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odom.set(t_odom.get() + dt_odom);
- }
- }
-
- expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
- last_KF->setState(expected_final_state);
-
- WOLF_TRACE("X_correct(t) : ", imu::composeOverState(x_origin, D_cor, ts - t).transpose());
- WOLF_TRACE("X_true(t) : ", expected_final_state.transpose());
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- SensorOdom3DPtr sen_odom3D;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- ProcessorOdom3DPtr processor_ptr_odom3D;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- // SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.9999;
- prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
- t.set(0);
- Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
- processor_ptr_odom3D->setOrigin(origin_KF);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn; //deg/s
- rateOfTurn << 45,90,0;
-
- Scalar dt(0.0010), dt_odom(1.0);
- TimeStamp ts(0.0), t_odom(1.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr);
- sen_odom3D->process(mot_ptr);
- //first odometry data will be processed at this timestamp
- //t_odom.set(t_odom.get() + dt_odom);
-
- Eigen::Vector2s randomPart;
- data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
-
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- for(unsigned int i = 1; i<=500; i++)
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts.set(i*dt);
-
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- if(ts.get() >= t_odom.get())
- {
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odom.set(t_odom.get() + dt_odom);
- }
- }
-
- rateOfTurn << 30,10,0;
- data_imu.tail<3>() = rateOfTurn* M_PI/180.0;
-
- for(unsigned int j = 1; j<=500; j++)
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts.set((500 + j)*dt);
-
- /*data_imu.tail<3>() = rateOfTurn* M_PI/180.0;
- randomPart = Eigen::Vector2s::Random(); //to have rate of turn > 0.99 deg/s
- data_imu.segment<2>(3) += randomPart* M_PI/180.0;*/
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- if(ts.get() >= t_odom.get())
- {
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odom.set(t_odom.get() + dt_odom);
- }
- }
-
- expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-// tests with following conditions :
-// var(b1,b2), invar(p1,q1,v1,p2,q2,v2), factor : imu(p,q,v)
-
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
- KF0->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<1,2,3,1,2,3).finished());
-
- KF1->getP()->setState(expected_final_state.head(3));
- KF1->getO()->setState(expected_final_state.segment(3,4));
- KF1->getV()->setState(expected_final_state.segment(7,3));
-
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
- KF1->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<-1,-2,-3,-1,-2,-3).finished());
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-}
-
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
- }
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
-
- KF1->getP()->setState(expected_final_state.head(3));
- KF1->getO()->setState(expected_final_state.segment(3,4));
- KF1->getV()->setState(expected_final_state.segment(7,3));
-
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
- }
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->getP()->setState(expected_final_state.head(3));
- last_KF->getO()->setState(expected_final_state.segment(3,4));
- last_KF->getV()->setState(expected_final_state.segment(7,3));
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
-
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF ->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- }
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->getP()->unfix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-//TEST_F(FactorIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-//{
-// //prepare problem for solving
-// origin_KF->getP()->fix();
-// origin_KF->getO()->fix();
-// origin_KF->getV()->unfix();
-//
-// last_KF->getP()->unfix();
-// last_KF->getO()->fix();
-// last_KF->getV()->unfix();
-//
-// last_KF->setState(expected_final_state);
-//
-// std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-//
-// //Only biases are unfixed
-// ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
-// ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
-//
-//}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- //ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
-{
- //Add fix factor on yaw to make the problem observable
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
- featureFix_cov(5,5) = 0.1;
- CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
- FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
-
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->unfix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
-{
- //Add fix factor on yaw to make the problem observable
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
- featureFix_cov(5,5) = 0.1;
- CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
- FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
-
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->unfix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
-// WOLF_TRACE("real bias : ", origin_bias.transpose());
-// WOLF_TRACE("origin bias : ", origin_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
-// WOLF_TRACE("last bias : ", last_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
-// WOLF_TRACE("jacob bias : ", std::static_pointer_cast<CaptureIMU>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
- ceres_manager->computeCovariances(ALL);
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- problem->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- problem->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- problem->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
-// if(cov_stdev.tail(6).maxCoeff()>=1)
-// WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-//Tests related to noise
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
-
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
deleted file mode 100644
index 204c6ce633a36380d46eacbc65d66abe1a7747fe..0000000000000000000000000000000000000000
--- a/test/gtest_factor_absolute.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-/**
- * \file gtest_factor_absolute.cpp
- *
- * Created on: Dec 9, 2017
- * \author: datchuth
- */
-
-#include "utils_gtest.h"
-#include "base/factor/factor_block_absolute.h"
-#include "base/factor/factor_quaternion_absolute.h"
-#include "base/capture/capture_motion.h"
-
-#include "base/ceres_wrapper/ceres_manager.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::cout;
-using std::endl;
-
-Vector10s pose9toPose10(Vector9s _pose9)
-{
- return (Vector10s() << _pose9.head<3>() , v2q(_pose9.segment<3>(3)).coeffs(), _pose9.tail<3>()).finished();
-}
-
-// Input pose9 and covariance
-Vector9s pose(Vector9s::Random());
-Vector10s pose10 = pose9toPose10(pose);
-Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = 0.2 * Eigen::Matrix<Scalar,9,9>::Identity();
-
-// perturbated priors
-Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV 3D");
-CeresManager ceres_mgr(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
-
-// Capture, feature and factor
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
-
-////////////////////////////////////////////////////////
-/* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine
- * These are absolute factors related to a specific part of the frame's state
- * Both features and factors are created in the TEST(). Hence, tests will not interfere each others.
- */
-
-TEST(FactorBlockAbs, fac_block_abs_p)
-{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
- fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
-
- ASSERT_TRUE(problem_ptr->check(0));
-
- // Unfix frame 0, perturb frm0
- frm0->unfix();
- frm0->setState(x0);
-
- // solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
- //only orientation is constrained
- ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6);
-}
-
-TEST(FactorBlockAbs, fac_block_abs_p_tail2)
-{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
- fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
-
- // Unfix frame 0, perturb frm0
- frm0->unfix();
- frm0->setState(x0);
-
- // solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
- //only orientation is constrained
- ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
-}
-
-TEST(FactorBlockAbs, fac_block_abs_v)
-{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
- fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
-
- ASSERT_TRUE(problem_ptr->check(0));
-
- // Unfix frame 0, perturb frm0
- frm0->unfix();
- frm0->setState(x0);
-
- // solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
- //only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6);
-}
-
-TEST(FactorQuatAbs, fac_block_abs_o)
-{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
- fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
-
- ASSERT_TRUE(problem_ptr->check(0));
-
- // Unfix frame 0, perturb frm0
- frm0->unfix();
- frm0->setState(x0);
-
- // solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
- //only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
deleted file mode 100644
index a053b3f82438b8ac9c358fc45b4b656dd9df65ff..0000000000000000000000000000000000000000
--- a/test/gtest_factor_autodiff.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-/*
- * gtest_factor_autodiff.cpp
- *
- * Created on: May 24 2017
- * Author: jvallve
- */
-
-#include "utils_gtest.h"
-
-#include "base/sensor/sensor_odom_2D.h"
-#include "base/capture/capture_void.h"
-#include "base/feature/feature_odom_2D.h"
-#include "base/factor/factor_odom_2D.h"
-#include "base/factor/factor_odom_2D_analytic.h"
-#include "base/factor/factor_autodiff.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(CaptureAutodiff, ConstructorOdom2d)
-{
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
-
- // SENSOR
- IntrinsicsOdom2D intrinsics_odo;
- intrinsics_odo.k_disp_to_disp = 0.1;
- intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
-
- // CAPTURE
- CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- fr2_ptr->addCapture(capture_ptr);
-
- // FEATURE
- FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
- capture_ptr->addFeature(feature_ptr);
-
- // CONSTRAINT
- FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addFactor(factor_ptr);
- fr1_ptr->addConstrainedBy(factor_ptr);
-
- ASSERT_TRUE(factor_ptr->getFeature());
- ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
- ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getFrame());
- ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getSensor());
- ASSERT_TRUE(factor_ptr->getFrameOther());
-}
-
-TEST(CaptureAutodiff, ResidualOdom2d)
-{
- Eigen::Vector3s f1_pose, f2_pose;
- f1_pose << 2,1,M_PI;
- f2_pose << 3,5,3*M_PI/2;
-
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
-
- // SENSOR
- IntrinsicsOdom2D intrinsics_odo;
- intrinsics_odo.k_disp_to_disp = 0.1;
- intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
-
- // CAPTURE
- CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- fr2_ptr->addCapture(capture_ptr);
-
- // FEATURE
- Eigen::Vector3s d;
- d << -1, -4, M_PI/2;
- FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- capture_ptr->addFeature(feature_ptr);
-
- // CONSTRAINT
- FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addFactor(factor_ptr);
- fr1_ptr->addConstrainedBy(factor_ptr);
-
- // EVALUATE
-
- Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
- Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
- Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
- Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
-
- std::vector<Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
-
- double const* const* parameters = states_ptr.data();
- Eigen::VectorXs residuals(factor_ptr->getSize());
- factor_ptr->evaluate(parameters, residuals.data(), nullptr);
-
- ASSERT_TRUE(residuals.maxCoeff() < 1e-9);
- ASSERT_TRUE(residuals.minCoeff() > -1e-9);
-}
-
-TEST(CaptureAutodiff, JacobianOdom2d)
-{
- Eigen::Vector3s f1_pose, f2_pose;
- f1_pose << 2,1,M_PI;
- f2_pose << 3,5,3*M_PI/2;
-
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
-
- // SENSOR
- IntrinsicsOdom2D intrinsics_odo;
- intrinsics_odo.k_disp_to_disp = 0.1;
- intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
-
- // CAPTURE
- CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- fr2_ptr->addCapture(capture_ptr);
-
- // FEATURE
- Eigen::Vector3s d;
- d << -1, -4, M_PI/2;
- FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- capture_ptr->addFeature(feature_ptr);
-
- // CONSTRAINT
- FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addFactor(factor_ptr);
- fr1_ptr->addConstrainedBy(factor_ptr);
-
- // COMPUTE JACOBIANS
-
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
-
- std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
-
- std::vector<Eigen::MatrixXs> Jauto;
- Eigen::VectorXs residuals(factor_ptr->getSize());
- factor_ptr->evaluate(states_ptr, residuals, Jauto);
-
- std::cout << Jauto.size() << std::endl;
-
- // ANALYTIC JACOBIANS
- Eigen::MatrixXs J0(3,2);
- J0 << -cos(f1_pose(2)), -sin(f1_pose(2)),
- sin(f1_pose(2)), -cos(f1_pose(2)),
- 0, 0;
- ASSERT_MATRIX_APPROX(J0, Jauto[0], wolf::Constants::EPS);
- //ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9);
-
- Eigen::MatrixXs J1(3,1);
- J1 << -(f2_pose(0) - f1_pose(0)) * sin(f1_pose(2)) + (f2_pose(1) - f1_pose(1)) * cos(f1_pose(2)),
- -(f2_pose(0) - f1_pose(0)) * cos(f1_pose(2)) - (f2_pose(1) - f1_pose(1)) * sin(f1_pose(2)),
- -1;
- ASSERT_MATRIX_APPROX(J1, Jauto[1], wolf::Constants::EPS);
- //ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9);
-
- Eigen::MatrixXs J2(3,2);
- J2 << cos(f1_pose(2)), sin(f1_pose(2)),
- -sin(f1_pose(2)), cos(f1_pose(2)),
- 0, 0;
- ASSERT_MATRIX_APPROX(J2, Jauto[2], wolf::Constants::EPS);
- //ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9);
-
- Eigen::MatrixXs J3(3,1);
- J3 << 0, 0, 1;
- ASSERT_MATRIX_APPROX(J3, Jauto[3], wolf::Constants::EPS);
- //ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9);
-
-// std::cout << "autodiff J " << 0 << ":\n" << Jauto[0] << std::endl;
-// std::cout << "analytic J " << 0 << ":\n" << J0 << std::endl;
-// std::cout << "autodiff J " << 1 << ":\n" << Jauto[1] << std::endl;
-// std::cout << "analytic J " << 1 << ":\n" << J1 << std::endl;
-// std::cout << "autodiff J " << 2 << ":\n" << Jauto[2] << std::endl;
-// std::cout << "analytic J " << 2 << ":\n" << J2 << std::endl;
-// std::cout << "autodiff J " << 3 << ":\n" << Jauto[3] << std::endl;
-// std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl;
-}
-
-TEST(CaptureAutodiff, AutodiffVsAnalytic)
-{
- Eigen::Vector3s f1_pose, f2_pose;
- f1_pose << 2,1,M_PI;
- f2_pose << 3,5,3*M_PI/2;
-
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
-
- // SENSOR
- IntrinsicsOdom2D intrinsics_odo;
- intrinsics_odo.k_disp_to_disp = 0.1;
- intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
-
- // CAPTURE
- CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- fr2_ptr->addCapture(capture_ptr);
-
- // FEATURE
- Eigen::Vector3s d;
- d << -1, -4, M_PI/2;
- FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- capture_ptr->addFeature(feature_ptr);
-
- // CONSTRAINTS
- FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addFactor(fac_autodiff_ptr);
- fr1_ptr->addConstrainedBy(fac_autodiff_ptr);
- FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr);
- feature_ptr->addFactor(fac_analytic_ptr);
- fr1_ptr->addConstrainedBy(fac_analytic_ptr);
-
- // COMPUTE JACOBIANS
-
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
-
- std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
-
- std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
- Eigen::VectorXs residuals(fac_autodiff_ptr->getSize());
- clock_t t = clock();
- fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
- std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
- t = clock();
- //TODO FactorAnalytic::evaluate
-// fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
-// std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
-//
-// for (auto i = 0; i < Jautodiff.size(); i++)
-// ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
deleted file mode 100644
index 7559bfa7582aa1449232391806e498fb294d03be..0000000000000000000000000000000000000000
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-/**
- * \file gtest_factor_autodiff_distance_3D.cpp
- *
- * Created on: Apr 10, 2018
- * \author: jsola
- */
-
-#include "base/factor/factor_autodiff_distance_3D.h"
-#include "base/problem.h"
-#include "base/logging.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/rotations.h"
-
-#include "utils_gtest.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-class FactorAutodiffDistance3D_Test : public testing::Test
-{
- public:
- Vector3s pos1, pos2;
- Vector3s euler1, euler2;
- Quaternions quat1, quat2;
- Vector4s vquat1, vquat2; // quaternions as vectors
- Vector7s pose1, pose2;
-
- FrameBasePtr F1, F2;
- CaptureBasePtr C2;
- FeatureBasePtr f2;
- FactorAutodiffDistance3DPtr c2;
-
- Vector1s dist;
- Matrix1s dist_cov;
-
- ProblemPtr problem;
- CeresManagerPtr ceres_manager;
-
- void SetUp()
- {
- pos1 << 1, 0, 0;
- pos2 << 0, 1, 0;
- euler1 << 0, 0, M_PI; //euler1 *= M_TORAD;
- euler2 << 0, 0, -M_PI_2; //euler2 *= M_TORAD;
- quat1 = e2q(euler1);
- quat2 = e2q(euler2);
- vquat1 = quat1.coeffs();
- vquat2 = quat2.coeffs();
- pose1 << pos1, vquat1;
- pose2 << pos2, vquat2;
-
- dist = Vector1s(sqrt(2.0));
- dist_cov = Matrix1s(0.01);
-
- problem = Problem::create("PO 3D");
- ceres_manager = std::make_shared<CeresManager>(problem);
-
- F1 = problem->emplaceFrame (KEY_FRAME, pose1, 1.0);
-
- F2 = problem->emplaceFrame (KEY_FRAME, pose2, 2.0);
- C2 = std::make_shared<CaptureBase>("Base", 1.0);
- F2->addCapture(C2);
- f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
- C2->addFeature(f2);
- c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE);
- f2->addFactor(c2);
- F1->addConstrainedBy(c2);
-
- }
-
-};
-
-TEST_F(FactorAutodiffDistance3D_Test, ground_truth)
-{
- Scalar res;
-
- c2->operator ()(pos1.data(), pos2.data(), &res);
-
- ASSERT_NEAR(res, 0.0, 1e-8);
-}
-
-TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
-{
- Scalar measurement = 1.400;
-
- f2->setMeasurement(Vector1s(measurement));
-
- Scalar res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
-
- Scalar res;
- c2->operator ()(pos1.data(), pos2.data(), &res);
-
- ASSERT_NEAR(res, res_expected, 1e-8);
-}
-
-TEST_F(FactorAutodiffDistance3D_Test, solve)
-{
- Scalar measurement = 1.400;
- f2->setMeasurement(Vector1s(measurement));
-
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
-
- // Check distance between F1 and F2 positions -- must match the measurement
- ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index a01314ae7b3a7b554178816db74b9a1dcfb20abf..23983d60685be050ca2c6ad15a3d7621b10a3646 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -6,12 +6,12 @@
*/
-#include "base/factor/factor_gnss_fix_2D.h"
+#include "gnss/factor/factor_gnss_fix_2D.h"
#include "utils_gtest.h"
-#include "base/problem.h"
-#include "base/sensor/sensor_gnss.h"
-#include "base/processor/processor_gnss_fix.h"
+#include "base/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/processor/processor_gnss_fix.h"
#include "base/ceres_wrapper/ceres_manager.h"
@@ -58,17 +58,17 @@ void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_red
num_params_reduced = 0;
std::vector<Scalar*> param_blocks;
- ceres_mgr_ptr->getCeresProblemPtr()->GetParameterBlocks(¶m_blocks);
+ ceres_mgr_ptr->getCeresProblem()->GetParameterBlocks(¶m_blocks);
for (auto pb : param_blocks)
{
std::vector<ceres::ResidualBlockId> residual_blocks;
- ceres_mgr_ptr->getCeresProblemPtr()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
+ ceres_mgr_ptr->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
- if (!ceres_mgr_ptr->getCeresProblemPtr()->IsParameterBlockConstant(pb) && !residual_blocks.empty())
+ if (!ceres_mgr_ptr->getCeresProblem()->IsParameterBlockConstant(pb) && !residual_blocks.empty())
{
num_param_blocks_reduced ++;
- num_params_reduced += ceres_mgr_ptr->getCeresProblemPtr()->ParameterBlockLocalSize(pb);
+ num_params_reduced += ceres_mgr_ptr->getCeresProblem()->ParameterBlockLocalSize(pb);
}
}
}
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index c0f0021f60a48be7c3e20b3ccad5d021161fa855..20c14da102bb89a25b81f36fd010f0c7a4a4f037 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -6,15 +6,15 @@
*/
-#include "base/factor/factor_gnss_single_diff_2D.h"
+#include "gnss/factor/factor_gnss_single_diff_2D.h"
#include "utils_gtest.h"
#include "base/capture/capture_motion.h"
-#include "base/problem.h"
-#include "base/sensor/sensor_gnss.h"
+#include "base/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_2D.h"
-#include "base/processor/processor_gnss_single_diff.h"
+#include "gnss/processor/processor_gnss_single_diff.h"
#include "base/ceres_wrapper/ceres_manager.h"
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
deleted file mode 100644
index c3f767d56ea04682d2e33651c4e4e1a89f865585..0000000000000000000000000000000000000000
--- a/test/gtest_factor_odom_3D.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-/**
- * \file gtest_factor_odom_3D.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "base/factor/factor_odom_3D.h"
-#include "base/capture/capture_motion.h"
-
-#include "base/ceres_wrapper/ceres_manager.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::cout;
-using std::endl;
-
-Vector7s data2delta(Vector6s _data)
-{
- return (Vector7s() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
-}
-
-// Input odometry data and covariance
-Vector6s data(Vector6s::Random());
-Vector7s delta = data2delta(data);
-Vector6s data_var((Vector6s() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6s data_cov = data_var.asDiagonal();
-
-// perturbated priors
-Vector7s x0 = data2delta(Vector6s::Random()*0.25);
-Vector7s x1 = data2delta(data + Vector6s::Random()*0.25);
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO 3D");
-CeresManager ceres_mgr(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
-
-// Capture, feature and factor from frm1 to frm0
-CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
-FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
-FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add
-FactorBasePtr dummy = frm0->addConstrainedBy(ctr1);
-
-////////////////////////////////////////////////////////
-
-TEST(FactorOdom3D, check_tree)
-{
- ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST(FactorOdom3D, expectation)
-{
- ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
-}
-
-TEST(FactorOdom3D, fix_0_solve)
-{
-
- // Fix frame 0, perturb frm1
- frm0->fix();
- frm1->unfix();
- frm1->setState(x1);
-
- // solve for frm1
- std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(frm1->getState(), delta, 1e-6);
-
-}
-
-TEST(FactorOdom3D, fix_1_solve)
-{
- // Fix frame 1, perturb frm0
- frm0->unfix();
- frm1->fix();
- frm0->setState(x0);
-
- // solve for frm0
- std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(frm0->getState(), problem_ptr->zeroState(), 1e-6);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
deleted file mode 100644
index d7bce0c84f734613f1a2f143bebf2690897149f2..0000000000000000000000000000000000000000
--- a/test/gtest_factor_pose_2D.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/**
- * \file gtest_factor_pose_2D.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
-
-#include "base/factor/factor_pose_2D.h"
-#include "utils_gtest.h"
-
-#include "base/capture/capture_motion.h"
-
-#include "base/ceres_wrapper/ceres_manager.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::cout;
-using std::endl;
-
-// Input data and covariance
-Vector3s pose(Vector3s::Random());
-Vector3s data_var((Vector3s() << 0.2,0.2,0.2).finished());
-Matrix3s data_cov = data_var.asDiagonal();
-
-// perturbated priors
-Vector3s x0 = pose + Vector3s::Random()*0.25;
-
-// Problem and solver
-ProblemPtr problem = Problem::create("PO 2D");
-CeresManager ceres_mgr(problem);
-
-// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
-
-// Capture, feature and factor from frm1 to frm0
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
-FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
-FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
-
-////////////////////////////////////////////////////////
-
-TEST(FactorPose2D, check_tree)
-{
- ASSERT_TRUE(problem->check(0));
-}
-
-//TEST(FactorFix, expectation)
-//{
-// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
-//}
-
-TEST(FactorPose2D, solve)
-{
-
- // Fix frame 0, perturb frm1
- frm0->unfix();
- frm0->setState(x0);
-
- // solve for frm0
- std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(frm0->getState(), pose, 1e-6);
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
deleted file mode 100644
index 0af4c3c406f02c9c87cabb35b5e43fd7137fe28b..0000000000000000000000000000000000000000
--- a/test/gtest_factor_pose_3D.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-/**
- * \file gtest_factor_fix_3D.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
-
-#include "base/factor/factor_pose_3D.h"
-#include "utils_gtest.h"
-
-#include "base/capture/capture_motion.h"
-
-#include "base/ceres_wrapper/ceres_manager.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::cout;
-using std::endl;
-
-Vector7s pose6toPose7(Vector6s _pose6)
-{
- return (Vector7s() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished();
-}
-
-// Input pose6 and covariance
-Vector6s pose(Vector6s::Random());
-Vector7s pose7 = pose6toPose7(pose);
-Vector6s data_var((Vector6s() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6s data_cov = data_var.asDiagonal();
-
-// perturbated priors
-Vector7s x0 = pose6toPose7(pose + Vector6s::Random()*0.25);
-
-// Problem and solver
-ProblemPtr problem = Problem::create("PO 3D");
-CeresManager ceres_mgr(problem);
-
-// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
-
-// Capture, feature and factor
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
-FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
-FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0)));
-
-////////////////////////////////////////////////////////
-
-TEST(FactorPose3D, check_tree)
-{
- ASSERT_TRUE(problem->check(0));
-}
-
-//TEST(FactorFix3D, expectation)
-//{
-// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
-//}
-
-TEST(FactorPose3D, solve)
-{
-
- // Fix frame 0, perturb frm1
- frm0->unfix();
- frm0->setState(x0);
-
- // solve for frm0
- std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL);
-
- ASSERT_MATRIX_APPROX(frm0->getState(), pose7, 1e-6);
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_sparse.cpp b/test/gtest_factor_sparse.cpp
deleted file mode 100644
index 6c8f212be2030c5d04707eb5b5798848f36d8710..0000000000000000000000000000000000000000
--- a/test/gtest_factor_sparse.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/**
- * \file gtest_factor_sparse.cpp
- *
- * Created on: Apr 25, 2017
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "factor_sparse.h"
-
-using namespace wolf;
-
-// Dummy class for avoiding the pure virtual compilation error
-template <JacobianMethod J>
-class FactorSparseObject : public FactorSparse<1, 2, 1>
-{
- public:
- FactorSparseObject(FactorType _tp, bool _apply_loss_function, FactorStatus _status, StateBlockPtr _xy, StateBlockPtr _theta) :
- FactorSparse<1, 2, 1>(_tp, _apply_loss_function, _status, _xy, _theta)
- {
- //
- }
- virtual ~FactorSparseObject(){}
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return J;
- }
-};
-
-TEST(FactorSparseAnalytic, Constructor)
-{
- FactorSparseObject<JAC_ANALYTIC> fac_analytic(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(fac_analytic.getJacobianMethod(), JAC_ANALYTIC);
- ASSERT_EQ(fac_analytic.getApplyLossFunction(), false);
- ASSERT_EQ(fac_analytic.getStatus(), FAC_ACTIVE);
- ASSERT_EQ(fac_analytic.getSize(), 1);
-
- FactorSparseObject<JAC_AUTO> fac_auto(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(fac_auto.getJacobianMethod(), JAC_AUTO);
-
- FactorSparseObject<JAC_NUMERIC> fac_numeric(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(fac_numeric.getJacobianMethod(), JAC_NUMERIC);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
deleted file mode 100644
index 82828c6c274a3ed2e72d53f15e8f625583644697..0000000000000000000000000000000000000000
--- a/test/gtest_feature_IMU.cpp
+++ /dev/null
@@ -1,167 +0,0 @@
-//Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-class FeatureIMU_test : public testing::Test
-{
-
- public: //These can be accessed in fixtures
- wolf::ProblemPtr problem;
- wolf::TimeStamp ts;
- wolf::CaptureIMUPtr imu_ptr;
- Eigen::VectorXs state_vec;
- Eigen::VectorXs delta_preint;
- Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
- std::shared_ptr<wolf::FeatureIMU> feat_imu;
- wolf::FrameBasePtr last_frame;
- wolf::FrameBasePtr origin_frame;
- Eigen::MatrixXs dD_db_jacobians;
- wolf::ProcessorBasePtr processor_ptr_;
-
- //a new of this will be created for each new test
- virtual void SetUp()
- {
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- // Wolf problem
- problem = Problem::create("POV 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params);
- ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 0.5;
- prc_imu_params->max_buff_length = 10;
- prc_imu_params->dist_traveled = 5;
- prc_imu_params->angle_turned = 0.5;
- processor_ptr_ = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
-
- // Time and data variables
- TimeStamp t;
- Eigen::Vector6s data_;
-
- t.set(0);
-
- // Set the origin
- Eigen::VectorXs x0(10);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0; P0.setIdentity(9,9);
- origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
- imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases
-
- //process data
- data_ << 2, 0, 9.8, 0, 0, 0;
- t.set(0.1);
- // Expected state after one integration
- //x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- // assign data to capture
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- // process data in capture
- sensor_ptr->process(imu_ptr);
-
- //create Frame
- ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
- state_vec = problem->getProcessorMotion()->getCurrentState();
- last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
- problem->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
- delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
- VectorXs calib_preint = problem->getProcessorMotion()->getBuffer().getCalibrationPreint();
- dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
- feat_imu = std::make_shared<FeatureIMU>(delta_preint,
- delta_preint_cov,
- calib_preint,
- dD_db_jacobians,
- imu_ptr);
- feat_imu->setCapture(imu_ptr); //associate the feature to a capture
-
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-TEST_F(FeatureIMU_test, check_frame)
-{
- // set variables
- using namespace wolf;
-
- FrameBasePtr left_frame = feat_imu->getFrame();
- wolf::TimeStamp t;
- left_frame->getTimeStamp(t);
- origin_frame->getTimeStamp(ts);
-
- ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
- ASSERT_TRUE(origin_frame->isKey());
- ASSERT_TRUE(last_frame->isKey());
- ASSERT_TRUE(left_frame->isKey());
-
- wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
- origin_pptr = origin_frame->getP();
- origin_optr = origin_frame->getO();
- origin_vptr = origin_frame->getV();
- left_pptr = left_frame->getP();
- left_optr = left_frame->getO();
- left_vptr = left_frame->getV();
-
- ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
- Eigen::Map<const Eigen::Quaternions> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
- ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(origin_vptr->getState(), left_vptr->getState(), wolf::Constants::EPS_SMALL);
-
- ASSERT_EQ(origin_frame->id(), left_frame->id());
-}
-
-TEST_F(FeatureIMU_test, access_members)
-{
- using namespace wolf;
-
- Eigen::VectorXs delta(10);
- //dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98
- delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98;
- ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
-}
-
-//TEST_F(FeatureIMU_test, addFactor)
-//{
-// using namespace wolf;
-//
-// FrameBasePtr frm_imu = std::static_pointer_cast<FrameBase>(last_frame);
-// auto cap_imu = last_frame->getCaptureList().back();
-// FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, std::static_pointer_cast<CaptureIMU>(cap_imu), processor_ptr_);
-// feat_imu->addFactor(factor_imu);
-// origin_frame->addConstrainedBy(factor_imu);
-//}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
deleted file mode 100644
index f9da19cff79a01ca755472f3e816fc4edeac5312..0000000000000000000000000000000000000000
--- a/test/gtest_feature_base.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * gtest_feature_base.cpp
- *
- * Created on: Apr 10, 2017
- * Author: jsola
- */
-
-#include "base/feature/feature_base.h"
-
-#include "utils_gtest.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(FeatureBase, Constructor)
-{
- Vector3s m; m << 1,2,3;
- Matrix3s M; M.setRandom(); M = M*M.transpose();
- Matrix3s I = M.inverse();
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXs U = llt_of_info.matrixU();
- Eigen::MatrixXs L = llt_of_info.matrixL();
-
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, M));
-
- ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6);
- ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6);
- ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6);
- ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
-}
-
-TEST(FeatureBase, SetMeasurement)
-{
- Vector3s m; m << 1,2,3;
- Matrix3s M; M.setRandom(); M = M*M.transpose();
- Matrix3s I = M.inverse();
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXs U = llt_of_info.matrixU();
- Eigen::MatrixXs L = llt_of_info.matrixL();
-
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity()));
-
- f->setMeasurement(m);
-
- ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6);
-}
-
-TEST(FeatureBase, SetMeasurementCovariance)
-{
- Vector3s m; m << 1,2,3;
- Matrix3s M; M.setRandom(); M = M*M.transpose();
- Matrix3s I = M.inverse();
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXs U = llt_of_info.matrixU();
- Eigen::MatrixXs L = llt_of_info.matrixL();
-
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity()));
-
- f->setMeasurementCovariance(M);
-
- ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6);
- ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6);
- ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
deleted file mode 100644
index 06436282a934f4b61bcfe04e50c77b1f3f27eea8..0000000000000000000000000000000000000000
--- a/test/gtest_frame_base.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * gtest_frame_base.cpp
- *
- * Created on: Nov 15, 2016
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/frame_base.h"
-#include "base/sensor/sensor_odom_2D.h"
-#include "base/processor/processor_odom_2D.h"
-#include "base/factor/factor_odom_2D.h"
-#include "base/capture/capture_motion.h"
-
-#include <iostream>
-
-using namespace Eigen;
-using namespace std;
-using namespace wolf;
-
-TEST(FrameBase, GettersAndSetters)
-{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-
- // getters
- ASSERT_EQ(F->id(), (unsigned int) 1);
- ASSERT_EQ(F->getTimeStamp(), 1);
- TimeStamp t;
- F->getTimeStamp(t);
- ASSERT_EQ(t, 1);
- ASSERT_FALSE(F->isFixed());
- ASSERT_EQ(F->isKey(), false);
-}
-
-TEST(FrameBase, StateBlocks)
-{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-
- ASSERT_EQ(F->getStateBlockVec().size(), (unsigned int) 3);
- ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
- ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1);
- ASSERT_EQ(F->getV(), nullptr);
-}
-
-TEST(FrameBase, LinksBasic)
-{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-
- ASSERT_FALSE(F->getTrajectory());
- ASSERT_FALSE(F->getProblem());
- // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
- // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
- SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), IntrinsicsOdom2D());
- ASSERT_FALSE(F->getCaptureOf(S));
- ASSERT_TRUE(F->getCaptureList().empty());
- ASSERT_TRUE(F->getConstrainedByList().empty());
- ASSERT_EQ(F->getHits() , (unsigned int) 0);
-}
-
-TEST(FrameBase, LinksToTree)
-{
- // Problem with 2 frames and one motion factor between them
- ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectory();
- IntrinsicsOdom2D intrinsics_odo;
- intrinsics_odo.k_disp_to_disp = 1;
- intrinsics_odo.k_rot_to_rot = 1;
- SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
- P->getHardware()->addSensor(S);
- FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- T->addFrame(F1);
- FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- T->addFrame(F2);
- CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
- F1->addCapture(C);
- /// @todo link sensor & proccessor
- ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
- FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
- C->addFeature(f);
- FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p);
- f->addFactor(c);
-
- // c-by link F2 -> c not yet established
- ASSERT_TRUE(F2->getConstrainedByList().empty());
-
- // tree is inconsistent since we are missing the constrained_by link
- ASSERT_FALSE(P->check(0));
-
- // establish constrained_by link F2 -> c
- F2->addConstrainedBy(c);
-
- // tree is now consistent
- ASSERT_TRUE(P->check(0));
-
- // F1 has one capture and no factors-by
- ASSERT_FALSE(F1->getCaptureList().empty());
- ASSERT_TRUE(F1->getConstrainedByList().empty());
- ASSERT_EQ(F1->getHits() , (unsigned int) 0);
-
- // F2 has no capture and one factor-by
- ASSERT_TRUE(F2->getCaptureList().empty());
- ASSERT_FALSE(F2->getConstrainedByList().empty());
- ASSERT_EQ(F2->getHits() , (unsigned int) 1);
-
- // fix and unfix
- F1->fix();
- ASSERT_TRUE(F1->isFixed());
- F1->unfix();
- ASSERT_FALSE(F1->isFixed());
- F1->fix();
- ASSERT_TRUE(F1->isFixed());
- F1->getP()->unfix();
- ASSERT_FALSE(F1->isFixed());
- F1->unfix();
- ASSERT_FALSE(F1->isFixed());
- F1->getP()->fix();
- ASSERT_FALSE(F1->isFixed());
- F1->getO()->fix();
- ASSERT_TRUE(F1->isFixed());
-
- // set key
- F1->setKey();
- ASSERT_TRUE(F1->isKey());
-
- // Unlink
- F1->unlinkCapture(C);
- ASSERT_TRUE(F1->getCaptureList().empty());
-}
-
-#include "base/state_quaternion.h"
-TEST(FrameBase, GetSetState)
-{
- // Create PQV_3D state blocks
- StateBlockPtr sbp = make_shared<StateBlock>(3);
- StateBlockPtr sbv = make_shared<StateBlock>(3);
- StateQuaternionPtr sbq = make_shared<StateQuaternion>();
-
- // Create frame
- FrameBase F(1, sbp, sbq, sbv);
-
- // Give values to vectors and vector blocks
- VectorXs x(10), x1(10), x2(10);
- VectorXs p(3), v(3), q(4);
- p << 1,2,3;
- v << 9,8,7;
- q << .5, -.5, .5, -.5;
-
- x << p, q, v;
-
- // Set the state, check that state blocks hold the current states
- F.setState(x);
- ASSERT_TRUE((p - F.getP()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((q - F.getO()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((v - F.getV()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
-
- // Get the state, form 1 by reference
- F.getState(x1);
- ASSERT_TRUE((x1 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
-
- // get the state, form 2 by return value
- x2 = F.getState();
- ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
deleted file mode 100644
index 1ead19daedc9b85bb4ef797309b24778ed4bdefe..0000000000000000000000000000000000000000
--- a/test/gtest_landmark_polyline.cpp
+++ /dev/null
@@ -1,142 +0,0 @@
-#include "utils_gtest.h"
-#include "base/landmark/landmark_polyline_2D.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-class LandmarkPolylineTest : public testing::Test
-{
- public:
-
- LandmarkPolyline2DPtr lmk_ptr;
- std::vector<LandmarkPolyline2DPtr> lmks_class_list;
- std::vector<VectorXs> lmks_groundtruth;
- std::vector<MatrixXs> points_groundtruth;
- std::vector<PolylineRectangularClass> classification_groundtruth;
- std::vector<PolylineRectangularClass> classification_candidates;
-
- virtual void SetUp()
- {
- classification_candidates.push_back(PolylineClassContainer());
- classification_candidates.push_back(PolylineClassSmallContainer());
- classification_candidates.push_back(PolylineClassPallet());
- classification_candidates.push_back(PolylineRectangularClass(OTHER,45.6, 3.1));
-
- //std::vector<Scalar> object_L({12, 5.9, 1.2});
- //std::vector<Scalar> object_W({2.345, 2.345, 0.9});
- //std::vector<PolylineClassType> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
-
- StateBlockPtr lmk_p_ptr = std::make_shared<StateBlock>(Vector2s::Zero());
- StateBlockPtr lmk_o_ptr = std::make_shared<StateBlock>(Vector1s::Zero());
-
- MatrixXs points = MatrixXs::Zero(2,5);
- points << 1, 1,-1,-1, 1,
- 2,-2,-2, 2, 2;
-
- lmk_ptr = std::make_shared<LandmarkPolyline2D>(lmk_p_ptr, lmk_o_ptr, points, true, true);
-
- // classification test
- MatrixXs class_points = MatrixXs::Zero(2,4);
-
- for (auto configuration = 1; configuration <= 2; configuration++)
- for (auto candidate_class : classification_candidates)
- //for (unsigned int classification = 0; classification < object_L.size(); classification++)
- {
- const Scalar& L = candidate_class.L;
- const Scalar& W = candidate_class.W;
- //const Scalar& L = object_L[classification];
- //const Scalar& W = object_W[classification];
-
- // create trivial points in corresponding configuration
- if (configuration == 1)
- class_points << L/2,-L/2,-L/2, L/2,
- -W/2,-W/2, W/2, W/2;
- else
- class_points <<-L/2,-L/2, L/2, L/2,
- -W/2, W/2, W/2,-W/2;
-
-
- // rotate and translate "randomly"
- Vector3s center_pose; center_pose << 4, -9, M_PI/5;
- Matrix2s R = Rotation2D<Scalar>(center_pose(2)).matrix();
- class_points = R * class_points;
- class_points += center_pose.head<2>() * VectorXs::Ones(4).transpose();
-
- // store object and points groundtruth
- lmks_groundtruth.push_back(center_pose);
- points_groundtruth.push_back(class_points);
-
- // store classification groundtruth
- classification_groundtruth.push_back(candidate_class);
-
- // create a landmark polyline using the points
- StateBlockPtr p_ptr = std::make_shared<StateBlock>(center_pose.head<2>());
- StateBlockPtr o_ptr = std::make_shared<StateBlock>(center_pose.tail<1>());
- lmks_class_list.push_back(std::make_shared<LandmarkPolyline2D>(p_ptr, o_ptr, class_points, true, true));
- }
- }
-};
-
-TEST_F(LandmarkPolylineTest, Merge)
-{
- ASSERT_EQ(lmk_ptr->getNPoints(), 5);
-
- lmk_ptr->mergePoints(0, 4);
-
- ASSERT_EQ(lmk_ptr->getNPoints(), 4);
- ASSERT_EQ(lmk_ptr->getFirstId(), 1);
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST_F(LandmarkPolylineTest, Close)
-{
- ASSERT_TRUE(lmk_ptr->tryClose(0.1));
-
- ASSERT_EQ(lmk_ptr->getNPoints(), 4);
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST_F(LandmarkPolylineTest, Classify)
-{
- for (unsigned int i = 0; i < lmks_class_list.size(); i++)
- {
- LandmarkPolyline2DPtr lmk_ptr = lmks_class_list[i];
-
- // test classify
- ASSERT_TRUE(lmk_ptr->tryClassify(0.1, classification_candidates));
-
- // test classification
- ASSERT_EQ(lmk_ptr->getClassification().type, classification_groundtruth[i].type);
- ASSERT_EQ(lmk_ptr->getClassification().L, classification_groundtruth[i].L);
- ASSERT_EQ(lmk_ptr->getClassification().W, classification_groundtruth[i].W);
- ASSERT_EQ(lmk_ptr->getClassification().D, classification_groundtruth[i].D);
-
- // test center pose
- ASSERT_MATRIX_APPROX(lmk_ptr->getState(), lmks_groundtruth[i], 1e-9);
-
- // test closed
- ASSERT_TRUE(lmk_ptr->isClosed());
-
- // test points fixed and center unfixed
- ASSERT_FALSE(lmk_ptr->getP()->isFixed());
- ASSERT_FALSE(lmk_ptr->getO()->isFixed());
- for (auto st_pair : lmk_ptr->getPointStatePtrMap())
- ASSERT_TRUE(st_pair.second->isFixed());
-
- // test points are in the same place (compoping with object pose)
- Matrix2s R = Rotation2D<Scalar>(lmk_ptr->getO()->getState()(0)).matrix();
- VectorXs t = lmk_ptr->getP()->getState();
- for (unsigned int point_i = 0; point_i < 4; point_i++)
- ASSERT_MATRIX_APPROX(R*lmk_ptr->getPointVector(point_i)+t, points_groundtruth[i].col(point_i), 1e-9);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
deleted file mode 100644
index 42e01489ed722497bdaad1d818ae572e1d2c0100..0000000000000000000000000000000000000000
--- a/test/gtest_local_param.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * \file test_local_param_quat.cpp
- *
- * Created on: Feb 19, 2016
- * author: jsola
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/local_parametrization_quaternion.h"
-#include "base/local_parametrization_homogeneous.h"
-#include "base/rotations.h"
-
-#include "base/wolf.h"
-
-#include <iostream>
-
-#define JAC_NUMERIC(T, _x0, _J, dx) \
-{ \
- VectorXs dv(3); \
- Map<const VectorXs> _dv (dv.data(), 3); \
- VectorXs xo(4); \
- Map<VectorXs> _xo (xo.data(), 4); \
- for (int i = 0; i< 3; i++) \
- {\
- dv.setZero();\
- dv(i) = dx;\
- T.plus(_x0, _dv, _xo);\
- _J.col(i) = (_xo - _x0)/dx; \
- } \
-}
-
-using namespace Eigen;
-using namespace std;
-using namespace wolf;
-
-TEST(TestLocalParametrization, QuaternionLocal)
-{
-
- // Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
- x_storage.setRandom();
- M_storage.setZero();
-
- // QUATERNION ------------------------------------------
- Map<VectorXs> q(&x_storage(0),4);
- q.normalize();
-
- Map<VectorXs> da(&x_storage(4),3);
- da /= 10.0;
- Map<VectorXs> qo_m(&x_storage(7),4);
- Map<MatrixXs> J(&M_storage(0,0),4,3);
- MatrixXs J_num(4,3);
-
- LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc;
-
- Map<const VectorXs> q_m(q.data(),4);
- Map<const VectorXs> da_m(da.data(),3);
-
- Qpar_loc.plus(q_m,da_m,qo_m);
-
- ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
-
- Quaternions qref = Map<Quaternions>(q.data()) * wolf::v2q(da);
- ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
-
- Qpar_loc.computeJacobian(q_m,J);
-
- JAC_NUMERIC(Qpar_loc, q_m, J_num, 1e-9)
-
- ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
-
- Map<const VectorXs> qoc_m(qo_m.data(), 4);
- Map<VectorXs> da2_m(x_storage.data() + 10, 3);
-
- Qpar_loc.minus(q_m, qoc_m, da2_m);
-
- ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10);
-
-}
-
-TEST(TestLocalParametrization, QuaternionGlobal)
-{
- // Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
- x_storage.setRandom();
- M_storage.setZero();
-
- // QUATERNION ------------------------------------------
- Map<VectorXs> q(&x_storage(0),4);
- q.normalize();
-
- Map<VectorXs> da(&x_storage(4),3);
- da /= 10.0;
- Map<VectorXs> qo_m(&x_storage(7),4);
- Map<MatrixXs> J(&M_storage(0,0),4,3);
- MatrixXs J_num(4,3);
-
- LocalParametrizationQuaternion<DQ_GLOBAL> Qpar_glob;
-
- Map<const VectorXs> q_m(q.data(),4);
- Map<const VectorXs> da_m(da.data(),3);
-
- Qpar_glob.plus(q_m,da_m,qo_m);
-
- ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
-
- Quaternions qref = wolf::v2q(da) * Map<Quaternions>(q.data());
- ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
-
- Qpar_glob.computeJacobian(q_m,J);
-
- JAC_NUMERIC(Qpar_glob, q_m, J_num, 1e-9)
-
- ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
-
- Map<const VectorXs> qoc_m(qo_m.data(), 4);
- Map<VectorXs> da2_m(x_storage.data() + 10, 3);
-
- Qpar_glob.minus(q_m, qoc_m, da2_m);
-
- ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10);
-
-}
-
-TEST(TestLocalParametrization, Homogeneous)
-{
- // Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
-
- // HOMOGENEOUS ----------------------------------------
- Map<VectorXs> h(&x_storage(11),4);
- h.setRandom();
- Map<VectorXs> d(&x_storage(15),3);
- d << .1,.2,.3;
- Map<VectorXs> ho_m(&x_storage(18),4);
- Map<MatrixXs> J(&M_storage(0,0),4,3);
- MatrixXs J_num(4,3);
-
- LocalParametrizationHomogeneous Hpar;
- Map<const VectorXs> h_m(h.data(),4);
- Map<const VectorXs> d_m(d.data(),3);
-
- Hpar.plus(h_m,d_m,ho_m);
-
- ASSERT_DOUBLE_EQ(ho_m.norm(), h.norm());
-
- Hpar.computeJacobian(h_m,J);
-
- JAC_NUMERIC(Hpar, h_m, J_num, 1e-9)
-
- ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
-
- Map<const VectorXs> hoc_m(ho_m.data(), 4);
- Map<VectorXs> d2_m(x_storage.data() + 10, 3);
-
- Hpar.minus(h_m, hoc_m, d2_m);
-
- ASSERT_MATRIX_APPROX(d_m, d2_m, 1e-10);
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
deleted file mode 100644
index 1dfcce9353f5726f5e69593f99e8a7dc3782cffd..0000000000000000000000000000000000000000
--- a/test/gtest_make_posdef.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-#include "utils_gtest.h"
-#include "base/wolf.h"
-
-using namespace Eigen;
-using namespace wolf;
-
-TEST(MakePosDefTest, OkTest)
-{
- MatrixXs M = MatrixXs::Identity(3,3);
-
- EXPECT_TRUE(isCovariance(M));
- EXPECT_FALSE(makePosDef(M));
-}
-
-TEST(MakePosDefTest, FixTest)
-{
- MatrixXs M = MatrixXs::Zero(3,3);
-
- EXPECT_FALSE(isCovariance(M));
- EXPECT_TRUE(makePosDef(M));
- EXPECT_TRUE(isCovariance(M));
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
deleted file mode 100644
index 6d0301692fc5bea5b444a6aa582e8170d489294c..0000000000000000000000000000000000000000
--- a/test/gtest_motion_buffer.cpp
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
- * gtest_motion_buffer.cpp
- *
- * Created on: Nov 12, 2016
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/motion_buffer.h"
-
-#include "base/wolf.h"
-
-#include <iostream>
-
-using namespace Eigen;
-using namespace std;
-using namespace wolf;
-
-Motion newMotion(TimeStamp t, Scalar d, Scalar D, Scalar C, Scalar J_d, Scalar J_D)
-{
- Motion m(t, 1, 1, 1, 0);
- m.delta_(0) = d;
- m.delta_integr_(0) = D;
- m.delta_cov_(0) = C;
- m.jacobian_delta_(0,0) = J_d;
- m.jacobian_delta_integr_(0,0) = J_D;
- return m;
-}
-
-namespace{
-TimeStamp t0(0), t1(1), t2(2), t3(3), t4(4);
-Motion m0 = newMotion(t0, 0, 0 , 0, .1, 1); // ts, delta, Delta, delta_cov, J_delta, J_Delta
-Motion m1 = newMotion(t1, 1, 1 , 1, .1, 1);
-Motion m2 = newMotion(t2, 2, 3 , 1, .1, 1);
-Motion m3 = newMotion(t3, 3, 6 , 1, .1, 1);
-Motion m4 = newMotion(t4, 4, 10, 1, .1, 1);
-}
-
-TEST(MotionBuffer, QueryTimeStamps)
-{
- MotionBuffer MB(1,1,1,0);
-
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- TimeStamp t;
-
- t.set(-1); // t is older than m0.ts_ -> return m0
- ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_);
-
- t.set( 0); // t is exactly m0.ts_ -> return m0
- ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_);
-
- t.set(0.5); // t is between m0.ts_ and m1.ts_ -> return m0
- ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_);
-
- t.set(+1); // t is exactly m1.ts_ -> return m1
- ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_);
-
- t.set(+2); // t is newer than m1.ts_ -> return m1
- ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_);
-}
-
-TEST(MotionBuffer, getMotion)
-{
- MotionBuffer MB(1,1,1,0);
-
- MB.get().push_back(m0);
- ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
-
- MB.get().push_back(m1);
- ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
- ASSERT_EQ(MB.getMotion(t1).delta_, m1.delta_);
- ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
- ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_);
-}
-
-TEST(MotionBuffer, getDelta)
-{
- MotionBuffer MB(1,1,1,0);
-
- MB.get().push_back(m0);
-
- ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
-
- MB.get().push_back(m1);
-
- ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
- ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_);
-}
-
-TEST(MotionBuffer, Split)
-{
- MotionBuffer MB(1,1,1,0);
-
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- MB.get().push_back(m2);
- MB.get().push_back(m3);
- MB.get().push_back(m4); // put 5 motions
-
- MotionBuffer MB_old(1,1,1,0);
-
- TimeStamp t = 1.5; // between m1 and m2
- MB.split(t, MB_old);
-
- ASSERT_EQ(MB_old.get().size(), (unsigned int) 2);
- ASSERT_EQ(MB .get().size(), (unsigned int) 3);
-
- ASSERT_EQ(MB_old.getMotion(t1).ts_, t1);
- ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1
- ASSERT_EQ(MB .getMotion(t1).ts_, t2); // first ts is t2
- ASSERT_EQ(MB .getMotion(t2).ts_, t2);
-}
-
-// TEST(MotionBuffer, integrateCovariance)
-// {
-// MotionBuffer MB(1,1,1,0);
-//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
-//
-// Eigen::MatrixXs cov = MB.integrateCovariance();
-// ASSERT_NEAR(cov(0), 0.04, 1e-8);
-//
-// }
-//
-// TEST(MotionBuffer, integrateCovariance_ts)
-// {
-// MotionBuffer MB(1,1,1,0);
-//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
-//
-// Eigen::MatrixXs cov = MB.integrateCovariance(t2);
-// ASSERT_NEAR(cov(0), 0.02, 1e-8);
-// }
-//
-// TEST(MotionBuffer, integrateCovariance_ti_tf)
-// {
-// MotionBuffer MB(1,1,1,0);
-//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
-//
-// Eigen::MatrixXs cov = MB.integrateCovariance(t1, t3);
-// ASSERT_NEAR(cov(0), 0.03, 1e-8);
-//
-// cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integate
-// ASSERT_NEAR(cov(0), 0.03, 1e-8);
-// }
-
-TEST(MotionBuffer, print)
-{
- MotionBuffer MB(1,1,1,0);
-
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- MB.get().push_back(m2);
-
- MB.print();
- MB.print(0,0,0,0);
- MB.print(1,0,0,0);
- MB.print(0,1,0,0);
- MB.print(0,0,1,0);
- MB.print(0,0,0,1);
- MB.print(1,1,1,1);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
deleted file mode 100644
index c6d7cfff118162c5f81c2345a19e57313a78b5b8..0000000000000000000000000000000000000000
--- a/test/gtest_odom_2D.cpp
+++ /dev/null
@@ -1,492 +0,0 @@
-/**
- * \file test_odom_2D.cpp
- *
- * Created on: Mar 15, 2016
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-// Classes under test
-#include "base/processor/processor_odom_2D.h"
-#include "base/factor/factor_odom_2D.h"
-
-// Wolf includes
-#include "base/sensor/sensor_odom_2D.h"
-#include "base/state_block.h"
-#include "base/wolf.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-
-// STL includes
-#include <map>
-#include <list>
-#include <algorithm>
-#include <iterator>
-
-// General includes
-#include <iostream>
-#include <iomanip> // std::setprecision
-#include "base/capture/capture_pose.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-VectorXs plus(const VectorXs& _pose, const Vector2s& _data)
-{
- VectorXs _pose_plus_data(3);
- _pose_plus_data(0) = _pose(0) + cos(_pose(2) + _data(1) / 2) * _data(0);
- _pose_plus_data(1) = _pose(1) + sin(_pose(2) + _data(1) / 2) * _data(0);
- _pose_plus_data(2) = pi2pi(_pose(2) + _data(1));
- return _pose_plus_data;
-}
-
-MatrixXs plus_jac_u(const VectorXs& _pose, const Vector2s& _data)
-{
- MatrixXs Ju(3,2);
- Ju(0, 0) = cos(_pose(2) + _data(1) / 2);
- Ju(0, 1) = -sin(_pose(2) + _data(1) / 2) * _data(0) / 2 ;
- Ju(1, 0) = sin(_pose(2) + _data(1) / 2);
- Ju(1, 1) = cos(_pose(2) + _data(1) / 2) * _data(0) / 2 ;
- Ju(2, 0) = 0.0;
- Ju(2, 1) = 1.0;
- return Ju;
-}
-
-MatrixXs plus_jac_x(const VectorXs& _pose, const Vector2s& _data)
-{
- Matrix3s Jx;
- Jx(0, 0) = 1.0;
- Jx(0, 1) = 0.0;
- Jx(0, 2) = -sin(_pose(2) + _data(1) / 2) * _data(0);
- Jx(1, 0) = 0.0;
- Jx(1, 1) = 1.0;
- Jx(1, 2) = cos(_pose(2) + _data(1) / 2) * _data(0);
- Jx(2, 0) = 0.0;
- Jx(2, 1) = 0.0;
- Jx(2, 2) = 1.0;
- return Jx;
-}
-
-void show(const ProblemPtr& problem)
-{
- using std::cout;
- using std::endl;
- cout << std::setprecision(4);
-
- for (FrameBasePtr F : problem->getTrajectory()->getFrameList())
- {
- if (F->isKey())
- {
- cout << "----- Key Frame " << F->id() << " -----" << endl;
- if (!F->getCaptureList().empty())
- {
- auto C = F->getCaptureList().front();
- if (!C->getFeatureList().empty())
- {
- auto f = C->getFeatureList().front();
- cout << " feature measure: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose()
- << endl;
- cout << " feature covariance: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
- }
- }
- cout << " state: \n" << F->getState().transpose() << endl;
- Eigen::MatrixXs cov;
- problem->getFrameCovariance(F,cov);
- cout << " covariance: \n" << cov << endl;
- }
- }
-}
-
-TEST(Odom2D, FactorFix_and_FactorOdom2D)
-{
- using std::cout;
- using std::endl;
-
- // RATIONALE:
- // We build this tree (a is `absolute`, m is `motion`):
- // KF0 -- m -- KF1 -- m -- KF2
- // |
- // a
- // |
- // GND
- // `absolute` is made with FactorFix
- // `motion` is made with FactorOdom2D
-
- std::cout << std::setprecision(4);
-
- TimeStamp t0(0.0), t = t0;
- Scalar dt = .01;
- Vector3s x0 (0,0,0);
- Eigen::Matrix3s P0 = Eigen::Matrix3s::Identity() * 0.1;
- Vector3s delta (2,0,0);
- Matrix3s delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
-
- ProblemPtr Pr = Problem::create("PO 2D");
- CeresManager ceres_manager(Pr);
-
- // KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPrior(x0, P0,t0, dt/2);
-
- // KF1 and motion from KF0
- t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
- CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
- FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
- FactorBasePtr c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr));
- F0->addConstrainedBy(c1);
-
- // KF2 and motion from KF1
- t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
- CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
- FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
- FactorBasePtr c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr));
- F1->addConstrainedBy(c2);
-
- ASSERT_TRUE(Pr->check(0));
-
-// cout << "===== full ====" << endl;
- F0->setState(Vector3s(1,2,3));
- F1->setState(Vector3s(2,3,1));
- F2->setState(Vector3s(3,1,2));
- std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);
-// std::cout << report << std::endl;
-
- ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-// show(Pr);
-
- Matrix3s P1, P2;
- P1 << 0.12, 0, 0,
- 0, 0.525, 0.22,
- 0, 0.22, 0.12;
- P2 << 0.14, 0, 0,
- 0, 1.91, 0.48,
- 0, 0.48, 0.14;
-
- // get covariances
- MatrixXs P0_solver, P1_solver, P2_solver;
- ASSERT_TRUE(Pr->getFrameCovariance(F0, P0_solver));
- ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
- ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
-
- ASSERT_POSE2D_APPROX(F0->getState(), Vector3s(0,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
- ASSERT_POSE2D_APPROX(F1->getState(), Vector3s(2,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
- ASSERT_POSE2D_APPROX(F2->getState(), Vector3s(4,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
-}
-
-TEST(Odom2D, VoteForKfAndSolve)
-{
- std::cout << std::setprecision(4);
- // time
- TimeStamp t0(0.0), t = t0;
- Scalar dt = .01;
- // Origin frame:
- Vector3s x0(0,0,0);
- Eigen::Matrix3s P0 = Eigen::Matrix3s::Identity() * 0.1;
- // motion data
- VectorXs data(Vector2s(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
- Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
- int N = 7; // number of process() steps
-
- // Create Wolf tree nodes
- ProblemPtr problem = Problem::create("PO 2D");
- SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
- ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
- params->voting_active = true;
- params->dist_traveled = 100;
- params->angle_turned = 6.28;
- params->max_time_span = 2.5*dt; // force KF at every third process()
- params->cov_det = 100;
- params->unmeasured_perturbation_std = 0.00;
- Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
- ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
-
- // NOTE: We integrate and create KFs as follows:
- // i= 0 1 2 3 4 5 6
- // KF -- * -- * -- KF - * -- * -- KF - *
-
- // Ceres wrapper
- CeresManager ceres_manager(problem);
-
- // Origin Key Frame
- FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2);
- ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
- ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-
- // std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
- // std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl;
- // std::cout << "Motion data : " << data.transpose() << std::endl;
-
- // Check covariance values
- Eigen::Vector3s integrated_pose = x0;
- Eigen::Matrix3s integrated_cov = P0;
- Eigen::Vector3s integrated_delta = Eigen::Vector3s::Zero();
- Eigen::Matrix3s integrated_delta_cov = Eigen::Matrix3s::Zero();
- Eigen::MatrixXs Ju(3, 2);
- Eigen::Matrix3s Jx;
- std::vector<Eigen::VectorXs> integrated_pose_vector;
- std::vector<Eigen::MatrixXs> integrated_cov_vector;
-
-// std::cout << "\nIntegrating data..." << std::endl;
-
- t += dt;
- // Capture to use as container for all incoming data
- CaptureMotionPtr capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
-
- for (int n=1; n<=N; n++)
- {
- // std::cout << "-------------------\nStep " << i << " at time " << t << std::endl;
- // re-use capture with updated timestamp
- capture->setTimeStamp(t);
-
- // Processor
- sensor_odom2d->process(capture);
- ASSERT_TRUE(problem->check(0));
-// Matrix3s odom2d_delta_cov = processor_odom2d->integrateBufferCovariance(processor_odom2d->getBuffer());
- Matrix3s odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_;
- // std::cout << "State(" << (t - t0) << ") : " << processor_odom2d->getCurrentState().transpose() << std::endl;
- // std::cout << "PRC cov: \n" << odom2d_delta_cov << std::endl;
-
- // Integrate Delta
- if (n==3 || n==6) // keyframes
- {
- integrated_delta.setZero();
- integrated_delta_cov.setZero();
- }
- else
- {
- Ju = plus_jac_u(integrated_delta, data);
- Jx = plus_jac_x(integrated_delta, data);
- integrated_delta = plus(integrated_delta, data);
- integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
- }
-
- WOLF_DEBUG("n: ", n, " t:", t);
- WOLF_DEBUG("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
- WOLF_DEBUG("test delta: ", integrated_delta .transpose());
-
- ASSERT_POSE2D_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6);
- ASSERT_MATRIX_APPROX(odom2d_delta_cov, integrated_delta_cov, 1e-6);
-
- // Integrate pose
- Ju = plus_jac_u(integrated_pose, data);
- Jx = plus_jac_x(integrated_pose, data);
- integrated_pose = plus(integrated_pose, data);
- integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
-
- ASSERT_POSE2D_APPROX(processor_odom2d->getCurrentState(), integrated_pose, 1e-6);
-
- integrated_pose_vector.push_back(integrated_pose);
- integrated_cov_vector.push_back(integrated_cov);
-
- t += dt;
- }
-
- // Solve
- std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
-// std::cout << report << std::endl;
- ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-
- MatrixXs computed_cov;
- ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
- ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6);
-}
-
-TEST(Odom2D, KF_callback)
-{
- using std::cout;
- using std::endl;
-
- std::cout << std::setprecision(4);
- // time
- TimeStamp t0(0.0), t = t0;
- Scalar dt = .01;
- Vector3s x0(0,0,0);
- Eigen::Matrix3s x0_cov = Eigen::Matrix3s::Identity() * 0.1;
- VectorXs data(Vector2s(1, M_PI/4) ); // advance 1m
- Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
- int N = 16; // number of process() steps
-
- // NOTE: We integrate and create KFs as follows:
- // n = 0 1 2 3 4 5 6 7 8
- // t = 0 dt 2dt 3dt 4dt 5dt 6dt 7dt 8dt
- // KF8-- * -- * -- * -- * -- * -- * -- * -- * --> : no keyframes during integration
- // And we create KFs as follows:
- // KF10
- // KF11
-
- // Create Wolf tree nodes
- ProblemPtr problem = Problem::create("PO 2D");
- SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
- ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
- params->dist_traveled = 100;
- params->angle_turned = 6.28;
- params->max_time_span = 100;
- params->cov_det = 100;
- params->unmeasured_perturbation_std = 0.001;
- Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
- ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
- processor_odom2d->setTimeTolerance(dt/2);
-
- // Ceres wrapper
- CeresManager ceres_manager(problem);
-
- // Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0, dt/2);
-
- // Check covariance values
- Eigen::Vector3s integrated_pose = x0;
- Eigen::Matrix3s integrated_cov = x0_cov;
- Eigen::Vector3s integrated_delta = Eigen::Vector3s::Zero();
- Eigen::Matrix3s integrated_delta_cov = Eigen::Matrix3s::Zero();
- Eigen::MatrixXs Ju(3, 2);
- Eigen::Matrix3s Jx;
- std::vector<Eigen::VectorXs> integrated_pose_vector;
- std::vector<Eigen::MatrixXs> integrated_cov_vector;
-
- // Store integrals
- integrated_pose_vector.push_back(integrated_pose);
- integrated_cov_vector.push_back(integrated_cov);
-
-// std::cout << "\nIntegrating data..." << std::endl;
-
- // Capture to use as container for all incoming data
- CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
-
- std::cout << "t: " << t << std::endl;
- for (int n=1; n<=N; n++)
- {
- t += dt;
-
- // re-use capture with updated timestamp
- capture->setTimeStamp(t);
-
- std::cout << "capture ts: " << capture->getTimeStamp() << " - " << capture->getTimeStamp().get();
- std::cout << "nsec: " << capture->getTimeStamp().getNanoSeconds() << std::endl;
- std::cout << "filled nsec: " << std::setfill('0') << std::setw(9) << std::right << capture->getTimeStamp().getNanoSeconds() << std::endl;
- std::cout << std::setfill(' ');
-
- // Processor
- sensor_odom2d->process(capture);
- ASSERT_TRUE(problem->check(0));
-
- // Integrate Delta
- Ju = plus_jac_u(integrated_delta, data);
- Jx = plus_jac_x(integrated_delta, data);
- integrated_delta = plus(integrated_delta, data);
- integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
-
- // Integrate pose
- Ju = plus_jac_u(integrated_pose, data);
- Jx = plus_jac_x(integrated_pose, data);
- integrated_pose = plus(integrated_pose, data);
- integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
-
- // Store integrals
- integrated_pose_vector.push_back(integrated_pose);
- integrated_cov_vector.push_back(integrated_cov);
-
- }
-
-// std::cout << "=============================" << std::endl;
-
- ////////////////////////////////////////////////////////////////
- // Split after the last keyframe, exact timestamp
- int n_split = 8;
- TimeStamp t_split = t0 + n_split*dt;
-
- std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
-
- Vector3s x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
-
- ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
- ASSERT_TRUE(problem->check(0));
- t += dt;
- capture->setTimeStamp(t);
- processor_odom2d->process(capture);
- ASSERT_TRUE(problem->check(0));
-
- CaptureMotionPtr key_capture_n = std::static_pointer_cast<CaptureMotion>(keyframe_2->getCaptureList().front());
-
- MotionBuffer key_buffer_n = key_capture_n->getBuffer();
-
- std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
-// std::cout << report << std::endl;
- ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-
- ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
- MatrixXs computed_cov;
- ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
- ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
-
- ////////////////////////////////////////////////////////////////
- // Split between keyframes, exact timestamp
- int m_split = 4;
- t_split = t0 + m_split*dt;
- std::cout << "-----------------------------\nSplit between KFs; time: " << t_split << std::endl;
-
- problem->print(4,1,1,1);
-
- x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
-
- ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
- ASSERT_TRUE(problem->check(0));
- t += dt;
- capture->setTimeStamp(t);
- processor_odom2d->process(capture);
- ASSERT_TRUE(problem->check(0));
-
- CaptureMotionPtr key_capture_m = std::static_pointer_cast<CaptureMotion>(keyframe_1->getCaptureList().front());
- MotionBuffer key_buffer_m = key_capture_m->getBuffer();
-
- // solve
-// cout << "===== full ====" << endl;
- keyframe_0->setState(Vector3s(1,2,3));
- keyframe_1->setState(Vector3s(2,3,1));
- keyframe_2->setState(Vector3s(3,1,2));
-
- report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
- ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-
- problem->print(4,1,1,1);
-
- // check the split KF
- MatrixXs KF1_cov;
- ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
- ASSERT_POSE2D_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
- ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
-
- // check other KF in the future of the split KF
- MatrixXs KF2_cov;
- ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
- ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
- ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
-
- // Check full trajectory
- t = t0;
- for (int n=1; n<=N; n++)
- {
- t += dt;
- // WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
- // WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
- EXPECT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
- }
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
deleted file mode 100644
index ab4f82bb186b949e5dd0d71b5e9e812972a6708e..0000000000000000000000000000000000000000
--- a/test/gtest_odom_3D.cpp
+++ /dev/null
@@ -1,580 +0,0 @@
-/**
- * \file gtest_odom_3D.cpp
- *
- * Created on: Nov 11, 2016
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "base/wolf.h"
-#include "base/logging.h"
-
-#include "base/processor/processor_odom_3D.h"
-
-#include <iostream>
-
-#define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \
-{ VectorXs Do(7); \
- prc_ptr->deltaPlusDelta(D, d, dt, Do); \
- VectorXs dd(7); \
- VectorXs DD(7); \
- VectorXs DDo(7); \
- for (int i = 0; i< 7; i++) {\
- dd = d;\
- DD = D; \
- dd(i) += dx;\
- prc_ptr->deltaPlusDelta(D, dd, dt, DDo);\
- J_d.col(i) = (DDo - Do)/dx; \
- dd = d;\
- DD = D; \
- DD(i) += dx; \
- prc_ptr->deltaPlusDelta(DD, d, dt, DDo); \
- J_D.col(i) = (DDo - Do)/dx; \
- }\
-}
-
-using namespace Eigen;
-using namespace std;
-using namespace wolf;
-
-/** Gain access to members of ProcessorOdom3D
- */
-class ProcessorOdom3DTest : public ProcessorOdom3D
-{
- public:
- ProcessorOdom3DTest();
-
- // getters :-D !!
- VectorXs& delta() {return delta_;}
- VectorXs& deltaInt() {return delta_integrated_;}
- MatrixXs& deltaCov() {return delta_cov_;}
- MatrixXs& deltaIntCov() {return delta_integrated_cov_;}
- Scalar& kdd() {return k_disp_to_disp_;}
- Scalar& kdr() {return k_disp_to_rot_;}
- Scalar& krr() {return k_rot_to_rot_;}
- Scalar& dvar_min() {return min_disp_var_;}
- Scalar& rvar_min() {return min_rot_var_;}
-};
-ProcessorOdom3DTest::ProcessorOdom3DTest() : ProcessorOdom3D(std::make_shared<ProcessorParamsOdom3D>())
-{
- dvar_min() = 0.5;
- rvar_min() = 0.25;
-}
-
-TEST(ProcessorOdom3D, computeCurrentDelta)
-{
- // One instance of the processor to test
- ProcessorOdom3DTest prc;
-
- // input data
- Vector6s data; data.setRandom();
- Scalar dt = 1; // irrelevant, just for the API.
-
- // Build delta from Eigen tools
- Vector3s data_dp = data.head<3>();
- Vector3s data_do = data.tail<3>();
- Vector3s delta_dp = data_dp;
- Quaternions delta_dq = v2q(data_do);
- Vector7s delta;
- delta.head<3>() = delta_dp;
- delta.tail<4>() = delta_dq.coeffs();
-
- // construct covariance from processor parameters and motion magnitudes
- Scalar disp = data_dp.norm();
- Scalar rot = data_do.norm();
- Scalar dvar = prc.dvar_min() + prc.kdd()*disp;
- Scalar rvar = prc.rvar_min() + prc.kdr()*disp + prc.krr()*rot;
- Vector6s diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
- Matrix6s data_cov = diag.asDiagonal();
- Matrix6s delta_cov = data_cov;
-
- // return values for data2delta()
- VectorXs delta_ret(7);
- MatrixXs delta_cov_ret(6,6);
- MatrixXs jac_delta_calib(6,0);
-
- // call the function under test
- prc.computeCurrentDelta(data, data_cov, VectorXs::Zero(0), dt, delta_ret, delta_cov_ret, jac_delta_calib);
-
- ASSERT_MATRIX_APPROX(delta_ret , delta, Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(delta_cov_ret , delta_cov, Constants::EPS_SMALL);
-
-}
-
-TEST(ProcessorOdom3D, deltaPlusDelta)
-{
- ProcessorOdom3DTest prc;
-
- VectorXs D(7); D.setRandom(); D.tail<4>().normalize();
- VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
-
- // Integrated delta value to check aginst
- // Dp_int <-- Dp + Dq * dp
- // Dq_int <-- Dq * dq
- VectorXs D_int_check(7);
- D_int_check.head<3>() = D.head<3>() + Quaternions(D.data()+3) * d.head<3>();
- D_int_check.tail<4>() = (Quaternions(D.data()+3) * Quaternions(d.data()+3)).coeffs();
-
- Scalar dt = 1; // dummy, not used in Odom3D
-
- VectorXs D_int(7);
-
- prc.deltaPlusDelta(D, d, dt, D_int);
-
- ASSERT_MATRIX_APPROX(D_int , D_int_check, 1e-10);
-// << "\nDpd : " << D_int.transpose()
-// << "\ncheck: " << D_int_check.transpose();
-}
-
-TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
-{
- std::shared_ptr<ProcessorOdom3DTest> prc_ptr = std::make_shared<ProcessorOdom3DTest>();
-
- VectorXs D(7); D.setRandom(); D.tail<4>().normalize();
- VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
- Scalar dt = 1;
- VectorXs Do(7);
- MatrixXs D_D(6,6);
- MatrixXs D_d(6,6);
-
- prc_ptr->deltaPlusDelta(D, d, dt, Do, D_D, D_d);
-
- WOLF_DEBUG("DD:\n ", D_D);
- WOLF_DEBUG("Dd:\n ", D_d);
-
- MatrixXs J_D(7,7), J_d(7,7);
-
- JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS);
- WOLF_DEBUG("J_D:\n ", J_D);
- WOLF_DEBUG("J_d:\n ", J_d);
-
-}
-
-TEST(ProcessorOdom3D, Interpolate0) // basic test
-{
- /* Conditions:
- * ref d = id
- * ref D = id
- * fin d = pos
- * fin D = id
- */
-
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- Motion ref(0.0,6,7,6,0), final(0.0,6,7,6,0), interpolated(0.0,6,7,6,0);
-
- // set ref
- ref.ts_ = 1;
- ref.delta_ << 0,0,0, 0,0,0,1;
- ref.delta_integr_ << 0,0,0, 0,0,0,1;
-
- WOLF_DEBUG("ref delta= ", ref.delta_.transpose());
- WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose());
-
- // set final
- final.ts_ = 5;
- final.delta_ << 1,0,0, 0,0,0,1;
- final.delta_integr_ << 0,0,0, 0,0,0,1;
- prc.deltaPlusDelta(ref.delta_integr_, final.delta_, (final.ts_ - ref.ts_), final.delta_integr_);
-
- WOLF_DEBUG("final delta= ", final.delta_.transpose());
- WOLF_DEBUG("final Delta= ", final.delta_integr_.transpose());
-
- // interpolate!
- Motion second = final;
- TimeStamp t; t = 2;
- // +--+--------+---> time(s)
- // 1 2 3 4 5 // 2 = 25% into interpolated, 75% into second
- interpolated = prc.interpolate(ref, second, t);
-
- WOLF_DEBUG("interpolated delta= ", interpolated.delta_.transpose());
- WOLF_DEBUG("interpolated Delta= ", interpolated.delta_integr_.transpose());
-
- // delta
- ASSERT_MATRIX_APPROX(interpolated.delta_.head<3>() , 0.25 * final.delta_.head<3>(), Constants::EPS);
- ASSERT_MATRIX_APPROX(second.delta_.head<3>() , 0.75 * final.delta_.head<3>(), Constants::EPS);
-
-}
-
-TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
-{
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- /*
- * We create several poses: origin, ref, int, second, final, as follows:
- *
- * +---+---+---+---->
- * o r i s,f
- *
- * We compute all deltas between them: d_or, d_ri, d_is, d_rf
- * We create the motions R, F
- * We interpolate, and get I, S
- */
-
- // absolute poses: origin, ref, interp, second=final
- Vector7s x_o, x_r, x_i, x_s, x_f;
- Map<Vector3s> p_o(x_o.data(), 3);
- Map<Quaternions> q_o(x_o.data() +3);
- Map<Vector3s> p_r(x_r.data(), 3);
- Map<Quaternions> q_r(x_r.data() +3);
- Map<Vector3s> p_i(x_i.data(), 3);
- Map<Quaternions> q_i(x_i.data() +3);
- Map<Vector3s> p_s(x_s.data(), 3);
- Map<Quaternions> q_s(x_s.data() +3);
- Map<Vector3s> p_f(x_f.data(), 3);
- Map<Quaternions> q_f(x_f.data() +3);
-
- // deltas -- referred to previous delta
- // o-r r-i i-s s-f
- Vector7s dx_or, dx_ri, dx_is, dx_rf;
- Map<Vector3s> dp_or(dx_or.data(), 3);
- Map<Quaternions> dq_or(dx_or.data() +3);
- Map<Vector3s> dp_ri(dx_ri.data(), 3);
- Map<Quaternions> dq_ri(dx_ri.data() +3);
- Map<Vector3s> dp_is(dx_is.data(), 3);
- Map<Quaternions> dq_is(dx_is.data() +3);
- Map<Vector3s> dp_rf(dx_rf.data(), 3);
- Map<Quaternions> dq_rf(dx_rf.data() +3);
- Map<Vector7s> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-
- // Deltas -- always referred to origin
- // o-r o-i o-s o-f
- Vector7s Dx_or, Dx_oi, Dx_os, Dx_of;
- Map<Vector3s> Dp_or(Dx_or.data(), 3);
- Map<Quaternions> Dq_or(Dx_or.data() +3);
- Map<Vector3s> Dp_oi(Dx_oi.data(), 3);
- Map<Quaternions> Dq_oi(Dx_oi.data() +3);
- Map<Vector3s> Dp_os(Dx_os.data(), 3);
- Map<Quaternions> Dq_os(Dx_os.data() +3);
- Map<Vector3s> Dp_of(Dx_of.data(), 3);
- Map<Quaternions> Dq_of(Dx_of.data() +3);
-
- // time stamps and intervals
- TimeStamp t_o(0), t_r(1), t_i(2.3), t_f(5); // t_i=2: 25% of motion; t_i=2.3: a general interpolation point
- Scalar dt_ri = t_i - t_r;
- Scalar dt_rf = t_f - t_r;
-
- WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_i: ", t_i.get(), "; t_f: ", t_f.get());
- WOLF_DEBUG("dt_ri: ", dt_ri, "; dt_rf ", dt_rf)
-
- // Constant velocity model
- Vector3s v;
- Vector3s w;
-
- // Motion structures
- Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
- /////////// start experiment ///////////////
-
- // set origin and ref states
- x_o << 1,2,3, 4,5,6,7; q_o.normalize();
- x_r << 7,6,5, 4,3,2,1; q_r.normalize();
-
- // set constant velocity params
- v << 3,2,1; // linear velocity
- w << .1,.2,.3; // angular velocity
-
- // compute other poses from model
- p_i = p_r + v * dt_ri;
- q_i = q_r * v2q (w * dt_ri);
- p_f = p_r + v * dt_rf;
- q_f = q_r * v2q (w * dt_rf);
- x_s = x_f;
-
- WOLF_DEBUG("o = ", x_o.transpose());
- WOLF_DEBUG("r = ", x_r.transpose());
- WOLF_DEBUG("i = ", x_i.transpose());
- WOLF_DEBUG("s = ", x_s.transpose());
- WOLF_DEBUG("f = ", x_f.transpose());
-
- // deltas -- referred to previous delta
- dp_or = q_o.conjugate() * (p_r - p_o);
- dq_or = q_o.conjugate() * q_r;
- dp_ri = q_r.conjugate() * (p_i - p_r);
- dq_ri = q_r.conjugate() * q_i;
- dp_is = q_i.conjugate() * (p_s - p_i);
- dq_is = q_i.conjugate() * q_s;
- dp_rf = q_r.conjugate() * (p_f - p_r);
- dq_rf = q_r.conjugate() * q_f;
-
- // Deltas -- always referred to origin
- Dp_or = q_o.conjugate() * (p_r - p_o);
- Dq_or = q_o.conjugate() * q_r;
- Dp_oi = q_o.conjugate() * (p_i - p_o);
- Dq_oi = q_o.conjugate() * q_i;
- Dp_os = q_o.conjugate() * (p_s - p_o);
- Dq_os = q_o.conjugate() * q_s;
- Dp_of = q_o.conjugate() * (p_f - p_o);
- Dq_of = q_o.conjugate() * q_f;
-
- // set ref
- R.ts_ = t_r;
- R.delta_ = dx_or; // origin to ref
- R.delta_integr_ = Dx_or; // origin to ref
-
- WOLF_DEBUG("* R.d = ", R.delta_.transpose());
- WOLF_DEBUG(" or = ", dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-
- WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
- WOLF_DEBUG(" or = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-
- // set final
- F.ts_ = t_f;
- F.delta_ = dx_rf; // ref to final
- F.delta_integr_ = Dx_of; // origin to final
-
- WOLF_DEBUG("* F.d = ", F.delta_.transpose());
- WOLF_DEBUG(" rf = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
- WOLF_DEBUG(" of = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-
- S = F; // avoid overwriting final
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" rs = ", dx_rs.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_os.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
- // interpolate!
- WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
- I = prc.interpolate(R, S, t_i);
-
- WOLF_DEBUG("* I.d = ", I.delta_.transpose());
- WOLF_DEBUG(" ri = ", dx_ri.transpose());
- ASSERT_MATRIX_APPROX(I.delta_ , dx_ri, Constants::EPS);
-
- WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
- WOLF_DEBUG(" oi = ", Dx_oi.transpose());
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
-
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" is = ", dx_is.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_is, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_os.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
-}
-
-TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
-{
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- /*
- * We create several poses: origin, ref, int, second, final, as follows:
- *
- * +---+---+---+---->
- * o r i s,f
- *
- * We compute all deltas between them: d_or, d_ri, d_is, d_rf
- * We create the motions R, F
- * We interpolate, and get I, S
- */
-
- // deltas -- referred to previous delta
- // o-r r-i i-s s-f
- VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_rf(7);
- Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-
- // Deltas -- always referred to origin
- // o-r o-i o-s o-f
- VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7);
-
- // time stamps and intervals
- TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later
-
- WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get());
-
- // Motion structures
- Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
- /////////// start experiment ///////////////
-
- // set final and ref deltas
- dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize();
- dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize();
- Dx_or = dx_or;
- prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of);
- Dx_os = Dx_of;
-
- // set ref
- R.ts_ = t_r;
- R.delta_ = dx_or; // origin to ref
- R.delta_integr_ = Dx_or; // origin to ref
-
- WOLF_DEBUG("* R.d = ", R.delta_.transpose());
- WOLF_DEBUG(" or = ", dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-
- WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
- WOLF_DEBUG(" or = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-
- // set final
- F.ts_ = t_f;
- F.delta_ = dx_rf; // ref to final
- F.delta_integr_ = Dx_of; // origin to final
-
- WOLF_DEBUG("* F.d = ", F.delta_.transpose());
- WOLF_DEBUG(" rf = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
- WOLF_DEBUG(" of = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-
- S = F; // avoid overwriting final
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" rs = ", dx_rs.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_os.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
- // interpolate!
- t_i = 0.5; /// before ref!
- WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
- I = prc.interpolate(R, S, t_i);
-
- WOLF_DEBUG("* I.d = ", I.delta_.transpose());
- WOLF_DEBUG(" ri = ", prc.deltaZero().transpose());
- ASSERT_MATRIX_APPROX(I.delta_ , prc.deltaZero(), Constants::EPS);
-
- WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
- WOLF_DEBUG(" oi = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_or, Constants::EPS);
-
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" is = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-
- // interpolate!
- t_i = 5.5; /// after ref!
- S = F;
- WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
- I = prc.interpolate(R, S, t_i);
-
- WOLF_DEBUG("* I.d = ", I.delta_.transpose());
- WOLF_DEBUG(" ri = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(I.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
- WOLF_DEBUG(" oi = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_of, Constants::EPS);
-
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" is = ", prc.deltaZero().transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , prc.deltaZero(), Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-
-}
-
-
-TEST(ProcessorOdom3D, Interpolate3) // timestamp out of bounds test
-{
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- /*
- * We create several poses: origin, ref, int, second, final, as follows:
- *
- * +---+---+---+---->
- * o r i s,f
- *
- * We compute all deltas between them: d_or, d_ri, d_is, d_rf
- * We create the motions R, F
- * We interpolate, and get I, S
- */
-
- // deltas -- referred to previous delta
- // o-r r-i i-s s-f
- VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_if(7), dx_rf(7);
- Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-
- // Deltas -- always referred to origin
- // o-r o-i o-s o-f
- VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7);
-
- // time stamps and intervals
- TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later
-
- WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get());
-
- // Motion structures
- Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
-
- /////////// start experiment ///////////////
-
- // set ref and final deltas
- dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize();
- dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize();
- Dx_or = dx_or;
- prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of);
- Dx_os = Dx_of;
-
- // set ref
- R.ts_ = t_r;
- R.delta_ = dx_or; // origin to ref
- R.delta_integr_ = Dx_or; // origin to ref
-
- WOLF_DEBUG("* R.d = ", R.delta_.transpose());
- WOLF_DEBUG(" or = ", dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-
- WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
- WOLF_DEBUG(" or = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-
- // set final
- F.ts_ = t_f;
- F.delta_ = dx_rf; // ref to final
- F.delta_integr_ = Dx_of; // origin to final
-
- WOLF_DEBUG("* F.d = ", F.delta_.transpose());
- WOLF_DEBUG(" rf = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
- WOLF_DEBUG(" of = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-
- // interpolate!
- t_i = 2; /// 25% between refs!
- WOLF_DEBUG("*** INTERPOLATE *** I and S have been computed.");
- I = prc.interpolate(R, F, t_i, S);
-
-
- prc.deltaPlusDelta(R.delta_integr_, I.delta_, t_i-t_r, Dx_oi);
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
-
- prc.deltaPlusDelta(I.delta_, S.delta_, t_f-t_i, dx_rf);
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
-}
-
-
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
deleted file mode 100644
index 8f68a5bd532c5a1408d0ba9654ec9f27283c7ba3..0000000000000000000000000000000000000000
--- a/test/gtest_pack_KF_buffer.cpp
+++ /dev/null
@@ -1,244 +0,0 @@
-/*
- * gtest_pack_KF_buffer.cpp
- *
- * Created on: Mar 5, 2018
- * Author: jsola
- */
-//Wolf
-#include "utils_gtest.h"
-
-#include "base/processor/processor_odom_2D.h"
-#include "base/sensor/sensor_odom_2D.h"
-
-#include "base/processor/processor_tracker_feature_dummy.h"
-#include "base/capture/capture_void.h"
-
-#include "base/problem.h"
-
-// STL
-#include <iterator>
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-class BufferPackKeyFrameTest : public testing::Test
-{
- public:
-
- PackKeyFrameBuffer pack_kf_buffer;
- FrameBasePtr f10, f20, f21, f28;
- Scalar tt10, tt20, tt21, tt28;
- TimeStamp timestamp;
- Scalar timetolerance;
-
- void SetUp(void)
- {
- f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
- f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
- f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
- f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
-
- tt10 = 1.0;
- tt20 = 1.0;
- tt21 = 1.0;
- tt28 = 1.0;
- };
-};
-
-TEST_F(BufferPackKeyFrameTest, empty)
-{
- ASSERT_TRUE(pack_kf_buffer.empty());
-}
-
-TEST_F(BufferPackKeyFrameTest, add)
-{
- pack_kf_buffer.add(f10, tt10);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- pack_kf_buffer.add(f20, tt20);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
-}
-
-TEST_F(BufferPackKeyFrameTest, clear)
-{
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- pack_kf_buffer.clear();
- ASSERT_TRUE(pack_kf_buffer.empty());
-}
-
-//TEST_F(BufferPackKeyFrameTest, print)
-//{
-// kfpackbuffer.clear();
-// kfpackbuffer.add(f10, tt10);
-// kfpackbuffer.add(f20, tt20);
-// kfpackbuffer.print();
-//}
-
-TEST_F(BufferPackKeyFrameTest, checkTimeTolerance)
-{
- pack_kf_buffer.clear();
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- // min time tolerance > diff between time stamps. It should return true
- ASSERT_TRUE(pack_kf_buffer.checkTimeTolerance(10, 20, 20, 20));
- // min time tolerance < diff between time stamps. It should return true
- ASSERT_FALSE(pack_kf_buffer.checkTimeTolerance(10, 1, 20, 20));
-}
-
-TEST_F(BufferPackKeyFrameTest, selectPack)
-{
- // Evaluation using two packs (p1,p2)
- // with different time tolerances (tp1,tp2)
- // using a query pack (q) with also different time tolerances
- // depending on these tolerances we will get one (p1) or the other (p2)
- // packages from the buffer (res).
- // This can be summarized in the table hereafter:
- //
- // p1 p2 q | resulting pack time stamp
- // --------------------------------
- // 2 2 2 | nullptr
- // 2 2 5 | nullptr
- // 2 2 7 | nullptr
- // 2 7 2 | nullptr
- // 2 7 5 | 20
- // 2 7 7 | 20
- // 7 2 2 | nullptr
- // 7 2 5 | nullptr
- // 7 2 7 | 10
- // 7 7 2 | nullptr
- // 7 7 5 | 20
- // 7 7 7 | 20
-
- pack_kf_buffer.clear();
-
- // input packages
- std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances
- std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances
- std::vector<int> q = {2, 5, 7}; // Query pack time tolerances
-
- // Solution matrix
- Eigen::VectorXi res = Eigen::VectorXi::Zero(12);
- res(4) = 20;
- res(5) = 20;
- res(8) = 10;
- res(10) = 20;
- res(11) = 20;
-
- // test
- for (unsigned int ip1=0;ip1<p1.size();++ip1)
- {
- for (unsigned int ip2=0;ip2<p2.size();++ip2)
- {
- pack_kf_buffer.add(f10, p1[ip1]);
- pack_kf_buffer.add(f20, p2[ip2]);
- for (unsigned int iq=0;iq<q.size();++iq)
- {
- PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]);
- if (packQ!=nullptr)
- ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
- }
- pack_kf_buffer.clear();
- }
- }
-}
-
-TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
-{
- pack_kf_buffer.clear();
-
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- pack_kf_buffer.add(f21, tt21);
-
- // input time stamps
- std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
- Scalar tt = 0.01;
-
- // Solution matrix
- // q_ts | pack
- //=================
- // first time
- //-----------------
- // 9.5 nullptr
- // 9.995 10
- // 10,005 10
- // 19.5 10
- // 20.5 10
- // 21.5 10
- // second time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 20
- // 21.5 20
- // third time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 null
- // 21.5 21
-
- Eigen::MatrixXs truth(3,6), res(3,6);
- truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
- res.setZero();
-
- for (int i=0; i<3; i++)
- {
- PackKeyFramePtr packQ;
- int j = 0;
- for (auto ts : q_ts)
- {
- packQ = pack_kf_buffer.selectFirstPackBefore(ts, tt);
- if (packQ)
- res(i,j) = packQ->key_frame->getTimeStamp().get();
-
- j++;
- }
- pack_kf_buffer.removeUpTo(packQ->key_frame->getTimeStamp());
- }
-
- ASSERT_MATRIX_APPROX(res, truth, 1e-6);
-}
-
-TEST_F(BufferPackKeyFrameTest, removeUpTo)
-{
- // Small time tolerance for all test asserts
- Scalar tt = 0.1;
- pack_kf_buffer.clear();
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- pack_kf_buffer.add(f21, tt21);
-
- // it should remove f20 and f10, thus size should be 1 after removal
- // Specifically, only f21 should remain
- PackKeyFramePtr pack20 = std::make_shared<PackKeyFrame>(f20,tt20);
- pack_kf_buffer.removeUpTo( pack20->key_frame->getTimeStamp() );
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f10->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f20->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)!=nullptr);
-
- // Chech removal of an imprecise time stamp
- // Specifically, only f28 should remain
- pack_kf_buffer.add(f28, tt28);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
- PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
- pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() );
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f28->getTimeStamp(),tt)!=nullptr);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
deleted file mode 100644
index e39c870881bf6e2abb57e771a2b79a3ed3706ce2..0000000000000000000000000000000000000000
--- a/test/gtest_param_prior.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * gtest_param_prior.cpp
- *
- * Created on: Feb 6, 2019
- * Author: jvallve
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/problem.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/sensor/sensor_odom_3D.h"
-
-#include <iostream>
-
-using namespace wolf;
-
-ProblemPtr problem_ptr = Problem::create("PO 3D");
-CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
-Eigen::Vector7s initial_extrinsics((Eigen::Vector7s() << 1, 2, 3, 1, 0, 0, 0).finished());
-Eigen::Vector7s prior_extrinsics((Eigen::Vector7s() << 10, 20, 30, 0, 0, 0, 1).finished());
-Eigen::Vector7s prior2_extrinsics((Eigen::Vector7s() << 100, 200, 300, 0, 0, 0, 1).finished());
-
-SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>()));
-
-TEST(ParameterPrior, initial_extrinsics)
-{
- ASSERT_TRUE(problem_ptr->check(0));
- ASSERT_TRUE(odom_sensor_ptr_->getP());
- ASSERT_TRUE(odom_sensor_ptr_->getO());
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),initial_extrinsics.head(3),1e-9);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),initial_extrinsics.tail(4),1e-9);
-}
-
-TEST(ParameterPrior, prior_p)
-{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3s::Identity());
-
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6);
-}
-
-TEST(ParameterPrior, prior_o)
-{
- odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3s::Identity());
-
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
-}
-
-TEST(ParameterPrior, prior_p_overwrite)
-{
- odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3s::Identity());
-
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6);
-}
-
-TEST(ParameterPrior, prior_p_segment)
-{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2);
-
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
-}
-
-TEST(ParameterPrior, prior_o_segment)
-{
- // constraining segment of quaternion is not allowed
- ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter(1, prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),"");
-}
-
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp
deleted file mode 100644
index fb36c6d127b11d14b86c8887fa32aa1b23d07391..0000000000000000000000000000000000000000
--- a/test/gtest_pinhole.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-/*
- * gtest_pinhole.cpp
- *
- * Created on: Nov 27, 2016
- * Author: jsola
- */
-
-#include "base/pinhole_tools.h"
-#include "utils_gtest.h"
-
-using namespace Eigen;
-using namespace wolf;
-using namespace pinhole;
-using Constants::EPS;
-using Constants::EPS_SMALL;
-
-TEST(Pinhole, EigenTypes)
-{
- Vector3s vs; vs << 1,2,4;
- Vector2s ps;
- Map<Vector3s> vm(vs.data());
- Map<Vector2s> pm(ps.data());
- Map<const Vector3s> cvm(vs.data());
- VectorXs vd(3); vd = vs;
- VectorXs pd(2);
-
- Vector2s pe; pe << 0.25, 0.5; // expected result
-
- // static size
- projectPointToNormalizedPlane(vs,ps);
- ASSERT_TRUE((ps - pe).isMuchSmallerThan(1, EPS_SMALL));
-
- // dynamic size
- projectPointToNormalizedPlane(vd,pd);
- ASSERT_TRUE((pd - pe).isMuchSmallerThan(1, EPS_SMALL));
-
- // Map
- projectPointToNormalizedPlane(vm,pm);
- ASSERT_TRUE((pm - pe).isMuchSmallerThan(1, EPS_SMALL));
-
- // Map const
- projectPointToNormalizedPlane(cvm,pm);
- ASSERT_TRUE((pm - pe).isMuchSmallerThan(1, EPS_SMALL));
-}
-
-TEST(Pinhole, pix_pnt_pix)
-{
- Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example
- Vector2s d; d << -0.301701, 0.0963189;
- Vector2s u0;
- Vector3s p;
- Vector2s u1;
- Vector2s c; // should be close to (0.297923, 0.216263)
- Scalar depth = 10;
- Scalar pix_error_allowed = 0.1;
-
- computeCorrectionModel(k, d, c);
-
- WOLF_DEBUG("correction: ", c.transpose(), " // should be close to (0.297923, 0.216263)");
-
- // choose some random points
- u0 << 123, 321;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-
- u0 << 1, 1;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-
- u0 << 320, 240;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-
- u0 << 640, 480;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-}
-
-TEST(Pinhole, Jacobians)
-{
- Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example
- Vector2s d; d << -0.301701, 0.0963189;
- Vector3s v;
- Vector2s u;
- MatrixXs U_v(2, 3);
-
- v << 1,2,4;
- projectPoint(k, d, v, u, U_v);
-
- Vector2s c;
- computeCorrectionModel(k, d, c);
-
- Vector3s p;
- MatrixXs P_u(3,2), P_depth(3,1);
- backprojectPoint(k, c, u, 4, p, P_u, P_depth);
-
- // check that reprojected point is close to original
- ASSERT_LT((p-v).norm(), 1e-3) << "p: " << p.transpose() << "\nv: " << v.transpose();
-
- // Check that both Jacobians are inverse one another (in the smallest dimension)
- ASSERT_TRUE((U_v*P_u - Matrix2s::Identity()).isMuchSmallerThan(1, 1e-3)) << "U_v*P_u: " << U_v*P_u;
-
- WOLF_DEBUG("U_v*P_u: \n", U_v*P_u);
- WOLF_DEBUG("P_u*U_v: \n", P_u*U_v);
-}
-
-TEST(Pinhole, JacobiansDist)
-{
- Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example
- Vector2s d; d << -0.301701, 0.0963189;
- Vector3s v;
- Vector2s u;
- MatrixXs U_v(2, 3);
- Scalar dist;
-
- v << 1,2,4;
- projectPoint(k, d, v, u, dist, U_v);
-
- // check that the recovered distance is indeed the distance to v
- ASSERT_DOUBLE_EQ(dist, v.norm());
-
- Vector2s c;
- computeCorrectionModel(k, d, c);
-
- Vector3s p;
- MatrixXs P_u(3,2), P_depth(3,1);
- backprojectPoint(k, c, u, 4, p, P_u, P_depth);
-
- // check that reprojected point is close to original
- ASSERT_LT((p-v).norm(), 1e-3) << "p: " << p.transpose() << "\nv: " << v.transpose();
-
- // Check that both Jacobians are inverse one another (in the smallest dimension)
- ASSERT_TRUE((U_v*P_u - Matrix2s::Identity()).isMuchSmallerThan(1, 1e-3)) << "U_v*P_u: " << U_v*P_u;
-}
-
-TEST(Pinhole, isInRoi)
-{
- Vector2s p;
- p << 15, 15;
-
- ASSERT_TRUE (isInRoi(p, 10, 10, 10, 10));
- ASSERT_FALSE(isInRoi(p, 0, 0, 10, 10));
- ASSERT_FALSE(isInRoi(p, 0, 10, 10, 10));
- ASSERT_FALSE(isInRoi(p, 0, 20, 10, 10));
- ASSERT_FALSE(isInRoi(p, 10, 0, 10, 10));
- ASSERT_FALSE(isInRoi(p, 10, 20, 10, 10));
- ASSERT_FALSE(isInRoi(p, 20, 0, 10, 10));
- ASSERT_FALSE(isInRoi(p, 20, 10, 10, 10));
- ASSERT_FALSE(isInRoi(p, 20, 20, 10, 10));
-}
-
-TEST(Pinhole, isInImage)
-{
- Vector2s p;
- p << 15, 15;
- ASSERT_TRUE (isInImage(p, 640, 480));
-
- p << -15, -15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << -15, 15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << -15, 500;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 15, -15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 15, 500;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 700, -15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 700, 15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 700, 500;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp
deleted file mode 100644
index 05d2f706f6891ebc3de8ffc2bd0504c5683f476e..0000000000000000000000000000000000000000
--- a/test/gtest_polyline_2D.cpp
+++ /dev/null
@@ -1,207 +0,0 @@
-#include <stdlib.h>
-#include "utils_gtest.h"
-#include "base/processor/polyline_2D_utils.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(Polyline2DTest, Transformations)
-{
- ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector2s::Zero(),0), 1e-12);
- ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector3s::Zero()), 1e-12);
- ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-12);
-}
-
-TEST(Polyline2DTest, RigidTransformation8points)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,8);
- points_last.bottomRows(1) = MatrixXs::Ones(1,8);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // compute rigit transformation
- Vector3s v = computeRigidTrans(points_incoming, points_last);
-
- ASSERT_MATRIX_APPROX(v.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation2points2defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,2);
- points_last.bottomRows(1) = MatrixXs::Ones(1,2);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation3points2defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,3);
- points_last.bottomRows(1) = MatrixXs::Ones(1,3);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // change not defined point (first)
- points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1));
- points_incoming(2,0) = 1;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation3points1defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,3);
- points_last.bottomRows(1) = MatrixXs::Ones(1,3);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // change 2 not defined points
- points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(1) - points_incoming.col(0));
- points_incoming(2,0) = 1;
- points_incoming.col(2) = points_incoming.col(2) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(2) - points_incoming.col(1));
- points_incoming(2,2) = 1;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, false);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation2points1defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,2);
- points_last.bottomRows(1) = MatrixXs::Ones(1,2);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // change not defined point
- points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1));
- points_incoming(2,0) = 1;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed .head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
deleted file mode 100644
index e7a1d6a32d705fef8c5085ef475e3a358a4624e7..0000000000000000000000000000000000000000
--- a/test/gtest_problem.cpp
+++ /dev/null
@@ -1,287 +0,0 @@
-/*
- * gtest_problem.cpp
- *
- * Created on: Nov 23, 2016
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/problem.h"
-#include "base/sensor/sensor_base.h"
-#include "base/sensor/sensor_odom_3D.h"
-#include "base/processor/processor_odom_3D.h"
-#include "base/processor/processor_tracker_feature_dummy.h"
-
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(Problem, create)
-{
- ProblemPtr P = Problem::create("POV 3D");
-
- // check double ointers to branches
- ASSERT_EQ(P, P->getHardware()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getMap()->getProblem());
-
- // check frame structure through the state size
- ASSERT_EQ(P->getFrameStructureSize(), 10);
-}
-
-TEST(Problem, Sensors)
-{
- ProblemPtr P = Problem::create("POV 3D");
-
- // add a dummy sensor
- SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
- P->addSensor(S);
-
- // check pointers
- ASSERT_EQ(P, S->getProblem());
- ASSERT_EQ(P->getHardware(), S->getHardware());
-
-}
-
-TEST(Problem, Processor)
-{
- ProblemPtr P = Problem::create("PO 3D");
-
- // check motion processor is null
- ASSERT_FALSE(P->getProcessorMotion());
-
- // add a motion sensor and processor
- SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics
- P->addSensor(Sm);
-
- // add motion processor
- ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>());
- Sm->addProcessor(Pm);
-
- // check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers
- ASSERT_FALSE(P->getProcessorMotion());
-
- // set processor motion
- P->setProcessorMotion(Pm);
- // re-check motion processor IS set by addSensor
- ASSERT_EQ(P->getProcessorMotion(), Pm);
-}
-
-TEST(Problem, Installers)
-{
- std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr P = Problem::create("PO 3D");
- Eigen::Vector7s xs;
-
- SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
-
- // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = 0.1;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 10;
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
- S->addProcessor(pt);
-
- // check motion processor IS NOT set
- ASSERT_FALSE(P->getProcessorMotion());
-
- // install processor motion
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
-
- // check motion processor is set
- ASSERT_TRUE(P->getProcessorMotion());
-
- // check motion processor is correct
- ASSERT_EQ(P->getProcessorMotion(), pm);
-}
-
-TEST(Problem, SetOrigin_PO_2D)
-{
- ProblemPtr P = Problem::create("PO 2D");
- TimeStamp t0(0);
- Eigen::VectorXs x0(3); x0 << 1,2,3;
- Eigen::MatrixXs P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
-
- P->setPrior(x0, P0, t0, 1.0);
-
- // check that no sensor has been added
- ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
-
- // check that the state is correct
- ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
-
- // check that we have one frame, one capture, one feature, one factor
- TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
- FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
- CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
-
- // check that the factor is absolute (no pointers to other F, f, or L)
- FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOther());
- ASSERT_FALSE(c->getLandmarkOther());
-
- // check that the Feature measurement and covariance are the ones provided
- ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));
-
- // P->print(4,1,1,1);
-}
-
-TEST(Problem, SetOrigin_PO_3D)
-{
- ProblemPtr P = Problem::create("PO 3D");
- TimeStamp t0(0);
- Eigen::VectorXs x0(7); x0 << 1,2,3,4,5,6,7;
- Eigen::MatrixXs P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
-
- P->setPrior(x0, P0, t0, 1.0);
-
- // check that no sensor has been added
- ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
-
- // check that the state is correct
- ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
-
- // check that we have one frame, one capture, one feature, one factor
- TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
- FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
- CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
-
- // check that the factor is absolute (no pointers to other F, f, or L)
- FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOther());
- ASSERT_FALSE(c->getLandmarkOther());
-
- // check that the Feature measurement and covariance are the ones provided
- ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));
-
- // P->print(4,1,1,1);
-}
-
-TEST(Problem, emplaceFrame_factory)
-{
- ProblemPtr P = Problem::create("PO 2D");
-
- FrameBasePtr f0 = P->emplaceFrame("PO 2D", KEY_FRAME, VectorXs(3), TimeStamp(0.0));
- FrameBasePtr f1 = P->emplaceFrame("PO 3D", KEY_FRAME, VectorXs(7), TimeStamp(1.0));
- FrameBasePtr f2 = P->emplaceFrame("POV 3D", KEY_FRAME, VectorXs(10), TimeStamp(2.0));
-
- // std::cout << "f0: type PO 2D? " << f0->getType() << std::endl;
- // std::cout << "f1: type PO 3D? " << f1->getType() << std::endl;
- // std::cout << "f2: type POV 3D? " << f2->getType() << std::endl;
-
- ASSERT_EQ(f0->getType().compare("PO 2D"), 0);
- ASSERT_EQ(f1->getType().compare("PO 3D"), 0);
- ASSERT_EQ(f2->getType().compare("POV 3D"), 0);
-
- // check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (unsigned int) 3);
-
- // check that all frames are linked to Problem
- ASSERT_EQ(f0->getProblem(), P);
- ASSERT_EQ(f1->getProblem(), P);
- ASSERT_EQ(f2->getProblem(), P);
-}
-
-TEST(Problem, StateBlocks)
-{
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- ProblemPtr P = Problem::create("PO 3D");
- Eigen::Vector7s xs;
-
- // 2 state blocks, fixed
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2);
-
- // 3 state blocks, fixed
- SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
-
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = 0.1;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 10;
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
-
- St->addProcessor(pt);
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
-
- // 2 state blocks, estimated
- P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
-
- // P->print(4,1,1,1);
-
- // change some SB properties
- St->unfixExtrinsics();
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
-
- // P->print(4,1,1,1);
-}
-
-TEST(Problem, Covariances)
-{
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- ProblemPtr P = Problem::create("PO 3D");
- Eigen::Vector7s xs;
-
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
- SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
-
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = 0.1;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 10;
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
-
- St->addProcessor(pt);
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
-
- // 4 state blocks, estimated
- St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
-
- // set covariance (they are not computed without a solver)
- P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
- P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity());
- P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero());
-
- // get covariance
- Eigen::MatrixXs Cov;
- ASSERT_TRUE(P->getFrameCovariance(F, Cov));
-
- // FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations)
- // JV: The local parameterization projects the covariance to the state size.
- ASSERT_EQ(Cov.cols() , 7);
- ASSERT_EQ(Cov.rows() , 7);
- ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12);
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
deleted file mode 100644
index 8a966f77b39390e80ad2ad5e68655bbb764ebf85..0000000000000000000000000000000000000000
--- a/test/gtest_processor_IMU.cpp
+++ /dev/null
@@ -1,1064 +0,0 @@
-/**
- * \file gtest_processor_imu.cpp
- *
- * Created on: Nov 23, 2016
- * \author: jsola
- */
-
-#include "base/capture/capture_IMU.h"
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/rotations.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-
-#include <cmath>
-#include <iostream>
-
-using namespace Eigen;
-
-class ProcessorIMUt : public testing::Test
-{
-
- public: //These can be accessed in fixtures
- wolf::ProblemPtr problem;
- wolf::SensorBasePtr sensor_ptr;
- wolf::TimeStamp t;
- wolf::Scalar mti_clock, tmp;
- wolf::Scalar dt;
- Vector6s data;
- Matrix6s data_cov;
- VectorXs x0;
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
-
- //a new of this will be created for each new test
- virtual void SetUp()
- {
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
- sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
-
- // Time and data variables
- data = Vector6s::Zero();
- data_cov = Matrix6s::Identity();
-
- // Set the origin
- x0.resize(10);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-TEST(ProcessorIMU_constructors, ALL)
-{
- using namespace wolf;
-
- //constructor with ProcessorIMUParamsPtr argument only
- ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
- param_ptr->max_time_span = 2.0;
- param_ptr->max_buff_length = 20000;
- param_ptr->dist_traveled = 2.0;
- param_ptr->angle_turned = 2.0;
-
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
- ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
- ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
- ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
- ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
-
- //Factory constructor without yaml
- std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
- ProcessorParamsIMU params_default;
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default.angle_turned);
-
- //Factory constructor with yaml
- processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
-}
-
-TEST(ProcessorIMU, voteForKeyFrame)
-{
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 1;
- prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_imu_params->dist_traveled = 1000000000;
- prc_imu_params->angle_turned = 1000000000;
- prc_imu_params->voting_active = true;
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
-
- //setting origin
- VectorXs x0(10);
- TimeStamp t(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- //data variable and covariance matrix
- // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- Vector6s data;
- data << 1,0,0, 0,0,0;
- Matrix6s data_cov(Matrix6s::Identity());
- data_cov(0,0) = 0.5;
-
- // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
-
- // Time
- // we want more than one data to integrate otherwise covariance will be 0
- Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- /*There should be 3 frames :
- - 1 KeyFrame at origin
- - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
- - 1 frame that would be used by future data
- */
- ASSERT_EQ(problem->getTrajectory()->getFrameList().size(),(unsigned int) 3);
-
- /* if max_time_span == 2, Wolf tree should be
-
- Hardware
- S1
- pm5
- o: C2 - F2
- l: C4 - F3
- Trajectory
- KF1
- Estim, ts=0, x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0)
- C1 -> S1
- KF2
- Estim, ts=2.1, x = ( 2.2 0 -22 0 0 0 1 2.1 0 -21 0 0 0 0 0 0 )
- C2 -> S1
- f1
- m = ( 2.21 0 0 0 0 0 1 2.1 0 0 )
- c1 --> F1
- F3
- Estim, ts=2.1, x = ( . . .)
- C4 -> S1
- */
- //TODO : ASSERT TESTS to make sure the factors are correctly set + check the tree above
-
-}
-
-//replace TEST by TEST_F if SetUp() needed
-TEST_F(ProcessorIMUt, interpolate)
-{
- using namespace wolf;
-
- t.set(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 0, 0, 0, 0; // only acc_x
- cap_imu_ptr->setData(data);
-
- // make one step to depart from origin
- cap_imu_ptr->setTimeStamp(0.05);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotion()->getMotion();
-
- // make two steps, then pretend it's just one
- cap_imu_ptr->setTimeStamp(0.10);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotion()->getMotion();
-
- cap_imu_ptr->setTimeStamp(0.15);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotion()->getMotion();
- mot_final.delta_ = mot_final.delta_integr_;
- Motion mot_sec = mot_final;
-
-// problem->getProcessorMotion()->getBuffer().print(0,1,1,0);
-
-class P : public wolf::ProcessorIMU
-{
- public:
- P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
- Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
- {
- return ProcessorIMU::interpolate(ref, sec, ts);
- }
-} imu;
-
-Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
-
-ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
-//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
-ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
-
-}
-
-TEST_F(ProcessorIMUt, acc_x)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- Vector6s b; b << 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-}
-
-TEST_F(ProcessorIMUt, acc_y)
-{
- // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
- // difference hier is that we integrate over 1ms periods
-
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
- x << 0,10,0, 0,0,0,1, 0,20,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, acc_z)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 50; //how 0.1s
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
- x << 0,0,25, 0,0,0,1, 0,0,10;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, check_Covariance)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- VectorXs delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov();
-
- ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
-// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
-}
-
-TEST_F(ProcessorIMUt, gyro_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
-{
- // time
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
-
- // bias
- wolf::Scalar abx = 0.002;
- Vector6s bias; bias << abx,0,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
- // state
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
-
- // init things
- problem->setPrior(x0, P0, t, 0.01);
-
- problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
- problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
-// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
-
- // data
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x_true(10);
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- VectorXs x_est(10);
- x_est = problem->getCurrentState().head(10);
-
- ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- wolf::Scalar abx(0.002), aby(0.01);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector6s bias; bias << abx,aby,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
-
- problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
- problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
-// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
- data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
-// << "\n estimated state : " << x.transpose();
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
-// "\n expected state is : \n" << x.transpose() << std::endl;
-}
-
-TEST_F(ProcessorIMUt, gyro_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity and bias parts
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- //rotation + translation due to initial velocity
- x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_x_acc_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
- // v = a*dt = 0.001
- x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
- // v = a*dt = 1
- x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_acc_y)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
- // v = a*dt = 0.001
- x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
- // v = a*dt = 1
- x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_acc_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
- // v = a*dt = 0.001
- x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
- // v = a*dt = 1
- x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
deleted file mode 100644
index 1e3b0940cdf2434a059b071b95ca47847ebadd48..0000000000000000000000000000000000000000
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ /dev/null
@@ -1,550 +0,0 @@
-/**
- * \file gtest_imu_preintegration_jacobians.cpp
- *
- * Created on: Nov 29, 2016
- * \author: AtDinesh
- */
-
- //Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "test/processor_IMU_UnitTester.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//google test
-#include "utils_gtest.h"
-
-//#define DEBUG_RESULTS
-//#define WRITE_RESULTS
-
-using namespace wolf;
-
-// A new one of these is created for each test
-class ProcessorIMU_jacobians : public testing::Test
-{
- public:
- TimeStamp t;
- Eigen::Vector6s data_;
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- wolf::Scalar ddelta_bias;
- wolf::Scalar dt;
- Eigen::Matrix<wolf::Scalar,9,1> Delta_noise;
- Eigen::Matrix<wolf::Scalar,9,1> delta_noise;
- struct IMU_jac_bias bias_jac;
- struct IMU_jac_deltas deltas_jac;
-
- void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
-
- new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
- new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
- }
-
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
-
- assert(place < _jac_delta.Delta_noisy_vect_.size());
- new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
- new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
- }
-
- virtual void SetUp()
- {
- //SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
- data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-
- ProcessorIMU_UnitTester processor_imu;
- ddelta_bias = 0.00000001;
- dt = 0.001;
-
- //defining a random Delta to begin with (not to use Origin point)
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
- Delta0.head<3>() = Delta0.head<3>()*100;
- Delta0.tail<3>() = Delta0.tail<3>()*10;
- Eigen::Vector3s ang0, ang;
- ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
-
- Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
- Delta0_quat = v2q(ang0);
- Delta0_quat.normalize();
- ang = q2v(Delta0_quat);
-
- //std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
- //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
-
- //get data to compute jacobians
- struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
- bias_jac.copyfrom(bias_jac_c);
-
- Delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
- delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
-
- struct IMU_jac_deltas deltas_jac_c = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
- deltas_jac = deltas_jac_c;
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-class ProcessorIMU_jacobians_Dq : public testing::Test
-{
- public:
- TimeStamp t;
- Eigen::Vector6s data_;
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- wolf::Scalar ddelta_bias2;
- wolf::Scalar dt;
- struct IMU_jac_bias bias_jac2;
-
- void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
-
- new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
- new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
- }
-
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
-
- assert(place < _jac_delta.Delta_noisy_vect_.size());
- new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
- new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
- }
-
- virtual void SetUp()
- {
- //SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
- data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-
- ProcessorIMU_UnitTester processor_imu;
- ddelta_bias2 = 0.0001;
- dt = 0.001;
-
- //defining a random Delta to begin with (not to use Origin point)
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
- Delta0.head<3>() = Delta0.head<3>()*100;
- Delta0.tail<3>() = Delta0.tail<3>()*10;
- Eigen::Vector3s ang0, ang;
- ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
-
- Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
- Delta0_quat = v2q(ang0);
- Delta0_quat.normalize();
- ang = q2v(Delta0_quat);
-
- //std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
- //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
-
- //get data to compute jacobians
- struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
- bias_jac2.copyfrom(bias_jac_c);
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
- ///BIAS TESTS
-
-/* IMU_jac_deltas struct form :
- contains vectors of size 7 :
- Elements at place 0 are those not affected by the bias noise that we add (da_bx,..., dw_bx,... ).
- place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
- place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
-*/
-
-/* Mathematics
-
- dDp_dab = [dDp_dab_x, dDp_dab_y, dDp_dab_z]
- dDp_dab_x = (dDp(ab_x + dab_x, ab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_x
- dDp_dab_x = (dDp(ab_x, ab_y + dab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_y
- dDp_dab_x = (dDp(ab_x, ab_y, ab_z + dab_z) - dDp(ab_x,ab_y,ab_z)) / dab_z
-
- similar for dDv_dab
- note dDp_dab_x, dDp_dab_y, dDp_dab_z, dDv_dab_x, dDv_dab_y, dDv_dab_z are 3x1 vectors !
-
- dDq_dab = 0_{3x3}
- dDq_dwb = [dDq_dwb_x, dDq_dwb_y, dDq_dwb_z]
- dDq_dwb_x = log( dR(wb).transpose() * dR(wb - dwb_x))/dwb_x
- = log( dR(wb).transpose() * exp((wx - wbx - dwb_x)dt, (wy - wby)dt, (wy - wby)dt))/dwb_x
- dDq_dwb_y = log( dR(wb).transpose() * dR(wb - dwb_y))/dwb_y
- dDq_dwb_z = log( dR(wb).transpose() * dR(wb + dwb_z))/dwb_z
-
- Note : dDq_dwb must be computed recursively ! So comparing the one returned by processor_imu and the numerical
- one will have no sense if we aredoing this from a random Delta. The Delta here should be the origin.
- dDq_dwb_ = dR.tr()*dDq_dwb - Jr(wdt)*dt
- Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt
-*/
-
-TEST_F(ProcessorIMU_jacobians, dDp_dab)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dab;
-
- for(int i=0;i<3;i++)
- dDp_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
-
- ASSERT_TRUE((dDp_dab - bias_jac.dDp_dab_).isMuchSmallerThan(1,0.000001)) << "dDp_dab : \n" << dDp_dab << "\n bias_jac.dDp_dab_ :\n" << bias_jac.dDp_dab_ <<
- "\ndDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dab)
-{
- using namespace wolf;
- Eigen::Matrix3s dDv_dab;
-
- for(int i=0;i<3;i++)
- dDv_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
- ASSERT_TRUE((dDv_dab - bias_jac.dDv_dab_).isMuchSmallerThan(1,0.000001)) << "dDv_dab : \n" << dDv_dab << "\n bias_jac.dDv_dab_ :\n" << bias_jac.dDv_dab_ <<
- "\ndDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dwb)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dwb;
-
- for(int i=0;i<3;i++)
- dDp_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
-
- ASSERT_TRUE((dDp_dwb - bias_jac.dDp_dwb_).isMuchSmallerThan(1,0.000001)) << "dDp_dwb : \n" << dDp_dwb << "\n bias_jac.dDp_dwb_ :\n" << bias_jac.dDp_dwb_ <<
- "\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
-{
- using namespace wolf;
- Eigen::Matrix3s dDv_dwb;
-
- for(int i=0;i<3;i++)
- dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
- ASSERT_TRUE((dDv_dwb - bias_jac.dDv_dwb_).isMuchSmallerThan(1,0.000001)) << "dDv_dwb : \n" << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n" << bias_jac.dDv_dwb_ <<
- "\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDq_dab)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dab;
-
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
- for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
- dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
- }
-
- ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dwb;
-
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
-
- for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
-
- dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
- }
-
- ASSERT_TRUE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1,0.000001))
- << "dDq_dwb : \n" << dDq_dwb
- << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_
- << "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb
- << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix()
- << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dwb;
-
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac2.Delta0_.data() + 3);
- for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac2.Deltas_noisy_vect_(i+3).data() + 3);
- dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias2;
- }
-
- ASSERT_TRUE((dDq_dwb - bias_jac2.dDq_dwb_).isMuchSmallerThan(1,0.001)) << "dDq_dwb : \n" << dDq_dwb << "\n bias_jac2.dDq_dwb_ :\n" << bias_jac2.dDq_dwb_ <<
- "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac2.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
-}
-
- ///Perturbation TESTS
-
-/* reminder :
- jacobian_delta_preint_vect_ jacobian_delta_vect_
- 0: + 0, 0: + 0
- 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz
- 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz
- 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 8: +dvy, 9: +dvz
-*/
-
-/* Numerical method to check jacobians wrt noise
-
- dDp_dP = [dDp_dPx, dDp_dPy, dDp_dPz]
- dDp_dPx = ((P + dPx) - P)/dPx = Id
- dDp_dPy = ((P + dPy) - P)/dPy = Id
- dDp_dPz = ((P + dPz) - P)/dPz = Id
-
- dDp_dV = [dDp_dVx, dDp_dVy, dDp_dVz]
- dDp_dVx = ((V + dVx)*dt - V*dt)/dVx = Id*dt
- dDp_dVy = ((V + dVy)*dt - V*dt)/dVy = Id*dt
- dDp_dVz = ((V + dVz)*dt - V*dt)/dVz = Id*dt
-
- dDp_dO = [dDp_dOx, dDp_dOy, dDp_dOz]
- dDp_dOx = (( dR(Theta + dThetax)*dp ) - ( dR(Theta)*dp ))/dThetax
- = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
- dDp_dOy = (( dR(Theta) * exp(0,dThetay,0)*dp ) - ( dR(Theta)*dp ))/dThetay
- dDp_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dp ) - ( dR(Theta)*dp ))/dThetaz
-
- dDv_dP = [dDv_dPx, dDv_dPy, dDv_dPz] = [0, 0, 0]
-
- dDv_dV = [dDv_dVx, dDv_dVy, dDv_dVz]
- dDv_dVx = ((V + dVx) - V)/dVx = Id
- dDv_dVy = ((V + dVy) - V)/dVy = Id
- dDv_dVz = ((V + dVz) - V)/dVz = Id
-
- dDv_dO = [dDv_dOx, dDv_dOy, dDv_dOz]
- dDv_dOx = (( dR(Theta + dThetax)*dv ) - ( dR(Theta)*dv ))/dThetax
- = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
- dDv_dOx = (( dR(Theta) * exp(0,dThetay,0)*dv ) - ( dR(Theta)*dv ))/dThetay
- dDv_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dv ) - ( dR(Theta)*dv ))/dThetaz
-
- dDp_dp = [dDp_dpx, dDp_dpy, dDp_dpz]
- dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
- dDp_dpy = ( dR*(p + dpy) - dR*(p))/dpy
- dDp_dpz = ( dR*(p + dpz) - dR*(p))/dpy
-
- dDp_dv = [dDp_dvx, dDp_dvy, dDp_dvz] = [0, 0, 0]
-
- dDp_do = [dDp_dox, dDp_doy, dDp_doz] = [0, 0, 0]
-
- dDv_dp = [dDv_dpx, dDv_dpy, dDv_dpz] = [0, 0, 0]
-
- dDv_dv = [dDv_dvx, dDv_dvy, dDv_dvz]
- dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
- dDv_dvy = ( dR*(v + dvy) - dR*(v))/dvy
- dDv_dvz = ( dR*(v + dvz) - dR*(v))/dvz
-
- dDv_do = [dDv_dox, dDv_doy, dDv_doz] = [0, 0, 0]
-
- dDo_dp = dDo_dv = dDo_dP = dDo_dV = [0, 0, 0]
-
- dDo_dO = [dDo_dOx, dDo_dOy, dDo_dOz]
-
- dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta+dThetax) * dr(theta) )/dThetax
- = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
- = log( (_Delta * _delta).transpose() * (_Delta_noisy * _delta))
- dDo_dOy = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,dThetay,0)) * dr(theta) )/dThetay
- dDo_dOz = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,0,dThetaz)) * dr(theta) )/dThetaz
-
- dDo_do = [dDo_dox, dDo_doy, dDo_doz]
-
- dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * dr(theta+dthetax) )/dthetax
- = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
- = log( (_Delta * _delta).transpose() * (_Delta * _delta_noisy))
- dDo_doy = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,dthetay,0)) )/dthetay
- dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
- */
-
-TEST_F(ProcessorIMU_jacobians, dDp_dP)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dP;
-
- //dDp_dPx = ((P + dPx) - P)/dPx
- for(int i=0;i<3;i++)
- dDp_dP.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3))/Delta_noise(i);
-
- ASSERT_TRUE((dDp_dP - deltas_jac.jacobian_delta_preint_.block(0,0,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dP : \n" << dDp_dP << "\n deltas_jac.jacobian_delta_preint_.block(0,0,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) <<
- "\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dV)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dV;
-
- //Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
- for(int i=0;i<3;i++)
- dDp_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3)*dt - deltas_jac.Delta0_.tail(3)*dt)/Delta_noise(i+6);
-
- ASSERT_TRUE((dDp_dV - deltas_jac.jacobian_delta_preint_.block(0,6,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dV : \n" << dDp_dV << "\n deltas_jac.jacobian_delta_preint_.block(0,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) <<
- "\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dO)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDp_dO;
-
- //dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDp_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.head(3)) - (Dq0.matrix()* deltas_jac.delta0_.head(3)))/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDp_dO - deltas_jac.jacobian_delta_preint_.block(0,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dO : \n" << dDp_dO << "\n deltas_jac.jacobian_delta_preint_.block(0,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) <<
- "\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dV)
-{
- using namespace wolf;
- Eigen::Matrix3s dDv_dV;
-
- //dDv_dVx = ((V + dVx) - V)/dVx
- for(int i=0;i<3;i++)
- dDv_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3) - deltas_jac.Delta0_.tail(3))/Delta_noise(i+6);
-
- ASSERT_TRUE((dDv_dV - deltas_jac.jacobian_delta_preint_.block(6,6,3,3)).isMuchSmallerThan(1,0.000001)) << "dDv_dV : \n" << dDv_dV << "\n deltas_jac.jacobian_delta_preint_.block(6,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) <<
- "\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dO)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDv_dO;
-
- //dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDv_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.tail(3)) - (Dq0.matrix()* deltas_jac.delta0_.tail(3)))/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDv_dO - deltas_jac.jacobian_delta_preint_.block(6,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDv_dO : \n" << dDv_dO << "\n deltas_jac.jacobian_delta_preint_.block(6,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) <<
- "\ndDv_dO_a - dDv_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) - dDv_dO << std::endl;
-}
-
-//dDo_dP = dDo_dV = [0, 0, 0]
-
-TEST_F(ProcessorIMU_jacobians, dDo_dO)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDo_dO;
-
- //dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDo_dO.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix()) )/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDo_dO - deltas_jac.jacobian_delta_preint_.block(3,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDo_dO : \n" << dDo_dO << "\n deltas_jac.jacobian_delta_preint_.block(3,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) <<
- "\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dp)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL);
- Eigen::Matrix3s dDp_dp, dDp_dp_a;
-
- dDp_dp_a = deltas_jac.jacobian_delta_.block(0,0,3,3);
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- //dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
- for(int i=0;i<3;i++)
- dDp_dp.block<3,1>(0,i) = ( (Dq0.matrix() * deltas_jac.delta_noisy_vect_(i).head(3)) - (Dq0.matrix() * deltas_jac.delta0_.head(3)) )/delta_noise(i);
-
- ASSERT_TRUE((dDp_dp - dDp_dp_a).isMuchSmallerThan(1,0.000001)) << "dDp_dp : \n" << dDp_dp << "\n dDp_dp_a :\n" << dDp_dp_a <<
- "\ndDp_dp_a - dDp_dp_ : \n" << dDp_dp_a - dDp_dp << std::endl;
-}
-
-//dDv_dp = [0, 0, 0]
-
-TEST_F(ProcessorIMU_jacobians, dDv_dv)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL);
- Eigen::Matrix3s dDv_dv, dDv_dv_a;
-
- dDv_dv_a = deltas_jac.jacobian_delta_.block(6,6,3,3);
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- //dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
- for(int i=0;i<3;i++)
- dDv_dv.block<3,1>(0,i) = ( (Dq0 * deltas_jac.delta_noisy_vect_(i+6).tail(3)) - (Dq0 * deltas_jac.delta0_.tail(3)) )/delta_noise(i+6);
-
- ASSERT_TRUE((dDv_dv - dDv_dv_a).isMuchSmallerThan(1,0.000001)) << "dDv_dv : \n" << dDv_dv << "\n dDv_dv_a :\n" << dDv_dv_a <<
- "\ndDv_dv_a - dDv_dv_ : \n" << dDv_dv_a - dDv_dv << std::endl;
-}
-
-//dDo_dp = dDo_dv = [0, 0, 0]
-
-TEST_F(ProcessorIMU_jacobians, dDo_do)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDo_do, dDo_do_a;
-
- //dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- dDo_do_a = deltas_jac.jacobian_delta_.block(3,3,3,3);
-
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDo_do.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq0.matrix() * dq_noisy.matrix()) )/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDo_do - dDo_do_a).isMuchSmallerThan(1,0.000001)) << "dDo_do : \n" << dDo_do << "\n dDo_do_a :\n" << dDo_do_a <<
- "\ndDo_do_a - dDo_do_ : \n" << dDo_do_a - dDo_do << std::endl;
-}
-
-int main(int argc, char **argv)
-{
- using namespace wolf;
-
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
deleted file mode 100644
index 7479ca984eda6dff7504595c66a959ae1d3aebfa..0000000000000000000000000000000000000000
--- a/test/gtest_processor_base.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * gtest_capture_base.cpp
- *
- * Created on: Feb 15, 2018
- * Author: asantamaria
- */
-
-//Wolf
-#include "utils_gtest.h"
-
-#include "base/processor/processor_odom_2D.h"
-#include "base/sensor/sensor_odom_2D.h"
-
-#include "base/processor/processor_tracker_feature_dummy.h"
-#include "base/capture/capture_void.h"
-
-#include "base/problem.h"
-
-// STL
-#include <iterator>
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(ProcessorBase, KeyFrameCallback)
-{
-
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using Eigen::Vector2s;
-
- Scalar dt = 0.01;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("PO 2D");
-
- // Install tracker (sensor and processor)
- SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = dt/2;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 5;
- shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params);
-
- problem->addSensor(sens_trk);
- sens_trk->addProcessor(proc_trk);
-
- // Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
- ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
- proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odometer", sens_odo, proc_odo_params);
-
- std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
- // Sequence to test KeyFrame creations (callback calls)
-
- // initialize
- TimeStamp t(0.0);
- Vector3s x(0,0,0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
- problem->setPrior(x, P, t, dt/2); // KF1
-
- CaptureOdom2DPtr capt_odo = make_shared<CaptureOdom2D>(t, sens_odo, Vector2s(0.5,0));
-
- // Track
- CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
- proc_trk->process(capt_trk);
-
- for (size_t ii=0; ii<10; ii++ )
- {
- // Move
- t = t+dt;
- WOLF_INFO("----------------------- ts: ", t , " --------------------------");
-
- capt_odo->setTimeStamp(t);
- proc_odo->process(capt_odo);
-
- // Track
- capt_trk = make_shared<CaptureVoid>(t, sens_trk);
- proc_trk->process(capt_trk);
-
-// problem->print(4,1,1,0);
-
- // Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 );
- }
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
deleted file mode 100644
index 425f6a96b9a9af4dcdcf5a491a8fb2ac75f98cf3..0000000000000000000000000000000000000000
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ /dev/null
@@ -1,235 +0,0 @@
-
-/**
- * \file gtest_processor_frame_nearest_neighbor_filter.cpp
- *
- * Created on: Aug 2, 2017
- * \author: tessajohanna
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/sensor/sensor_odom_2D.h"
-#include "base/processor/processor_frame_nearest_neighbor_filter.h"
-
-#include <iostream>
-
-// Dummy class so that it is no pur virtual
-struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter
-{
- DummyLoopCloser(ParamsPtr _params) :
- wolf::ProcessorFrameNearestNeighborFilter(_params) { }
-
- bool confirmLoopClosure() override { return false; }
- bool voteForKeyFrame() override { return false; }
-};
-
-// Declare Wolf problem
-wolf::ProblemPtr problem = wolf::Problem::create("PO 2D");
-
-// Declare Sensor
-Eigen::Vector3s odom_extrinsics = Eigen::Vector3s(0,0,0);
-
-std::shared_ptr<wolf::IntrinsicsOdom2D> odom_intrinsics =
- std::make_shared<wolf::IntrinsicsOdom2D>(wolf::IntrinsicsOdom2D());
-
-wolf::SensorBasePtr sensor_ptr;
-
-// Declare Processors
-wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_point2d;
-wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_point2d =
- std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
-
-wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_ellipse2d;
-wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_ellipse2d =
- std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
-
-// Declare Frame Pointer
-wolf::StateBlockPtr stateblock_ptr1, stateblock_ptr2, stateblock_ptr3, stateblock_ptr4;
-wolf::FrameBasePtr F1, F2, F3, F4;
-wolf::CaptureBasePtr capture_dummy, incomming_dummy;
-
-//##############################################################################
-TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
-{
- // four different states [x, y]
- Eigen::Vector3s state1, state2, state3, state4;
- state1 << 3.0, 5.0, 0.0;
- state2 << 3.0, 6.0, 0.0;
- state3 << 3.0, 7.0, 0.0;
- state4 << 100.0, 100.0, 0.0;
-
- auto stateblock_pptr1 = std::make_shared<wolf::StateBlock>(state1.head<2>());
- auto stateblock_optr1 = std::make_shared<wolf::StateBlock>(state1.tail<1>());
-
- auto stateblock_pptr2 = std::make_shared<wolf::StateBlock>(state2.head<2>());
- auto stateblock_optr2 = std::make_shared<wolf::StateBlock>(state2.tail<1>());
-
- auto stateblock_pptr3 = std::make_shared<wolf::StateBlock>(state3.head<2>());
- auto stateblock_optr3 = std::make_shared<wolf::StateBlock>(state3.tail<1>());
-
- auto stateblock_pptr4 = std::make_shared<wolf::StateBlock>(state4.head<2>());
- auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>());
-
- // create Keyframes
- F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
- 1,
- stateblock_pptr1,
- stateblock_optr1);
- F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
- 1,
- stateblock_pptr2,
- stateblock_optr2);
- F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
- 1,
- stateblock_pptr3,
- stateblock_optr3);
- F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
- 1,
- stateblock_pptr4,
- stateblock_optr4);
-
- // add dummy capture
- capture_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
- 1.0,
- sensor_ptr);
- F1->addCapture(capture_dummy);
- F2->addCapture(capture_dummy);
- F3->addCapture(capture_dummy);
- F4->addCapture(capture_dummy);
-
- // Add first three states to trajectory
- problem->getTrajectory()->addFrame(F1);
- problem->getTrajectory()->addFrame(F2);
- problem->getTrajectory()->addFrame(F3);
- problem->getTrajectory()->addFrame(F4);
-
- // Add same covariances for all states
- Eigen::Matrix2s position_covariance_matrix;
- position_covariance_matrix << 9.0, 0.0,
- 0.0, 5.0;
-
- Eigen::Matrix1s orientation_covariance_matrix;
- orientation_covariance_matrix << 0.01;
-
- Eigen::Vector2s tt_covariance_matrix;
- tt_covariance_matrix << 0.0, 0.0;
-
- problem->addCovarianceBlock( stateblock_pptr1, stateblock_pptr1,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr1, stateblock_optr1,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr1, stateblock_optr1,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr2, stateblock_pptr2,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr2, stateblock_optr2,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr2, stateblock_optr2,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr3, stateblock_pptr3,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr3, stateblock_optr3,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr3, stateblock_optr3,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr4, stateblock_pptr4,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr4, stateblock_optr4,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr4, stateblock_optr4,
- tt_covariance_matrix);
-
- // create dummy capture
- incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
- 1.2,
- sensor_ptr);
- // Make 3rd frame last Keyframe
- problem->getTrajectory()->setLastKeyFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_point2d->process(incomming_dummy);
-
- const std::vector<wolf::FrameBasePtr> &testloops =
- processor_ptr_point2d->getCandidates();
-
- ASSERT_EQ(testloops.size(), (unsigned int) 1);
- ASSERT_EQ(testloops[0]->id(), (unsigned int) 2);
-}
-
-//##############################################################################
-TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
-{
- // set covariance to a -90 degree turned ellipse
- Eigen::Matrix2s position_covariance_matrix;
-// position_covariance_matrix << 9.0, 1.8,
-// 1.8, 5.0;
-
- position_covariance_matrix << 5.0, 0.0,
- 0.0, 9.0;
-
- problem->addCovarianceBlock( F1->getP(), F1->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F2->getP(), F2->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F3->getP(), F3->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F4->getP(), F4->getP(),
- position_covariance_matrix);
-
- // Make 3rd frame last Keyframe
- problem->getTrajectory()->setLastKeyFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_ellipse2d->process(incomming_dummy);
- const std::vector<wolf::FrameBasePtr> &testloops =
- processor_ptr_ellipse2d->getCandidates();
-
- ASSERT_EQ(testloops.size(), (unsigned int) 2);
- ASSERT_EQ(testloops[0]->id(), (unsigned int) 1);
- ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
-
- // Make 4th frame last Keyframe
- problem->getTrajectory()->setLastKeyFrame(F4);
-
- // trigger search for loopclosure again
- processor_ptr_ellipse2d->process(incomming_dummy);
- ASSERT_EQ(testloops.size(), (unsigned int) 0);
-}
-
-//##############################################################################
-int main(int argc, char **argv)
-{
- // SENSOR PARAMETERS
- odom_intrinsics->k_disp_to_disp = 0.2;
- odom_intrinsics->k_rot_to_rot = 0.2;
-
- // install sensor
- sensor_ptr = problem->installSensor("ODOM 2D", "odom",
- odom_extrinsics, odom_intrinsics);
-
- // install the processors
- processor_params_point2d->probability_ = 0.95;
- processor_params_point2d->buffer_size_ = 0; // just exclude identical frameIDs
- processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE;
-
- processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d);
- processor_ptr_point2d->setName("processor2Dpoint");
-
- sensor_ptr->addProcessor(processor_ptr_point2d);
-
- processor_params_ellipse2d->probability_ = 0.95;
- processor_params_ellipse2d->buffer_size_ = 0; // just exclude identical frameIDs
- processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
-
- processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d);
- processor_ptr_ellipse2d->setName("processor2Dellipse");
-
- sensor_ptr->addProcessor(processor_ptr_ellipse2d);
-
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
deleted file mode 100644
index 205c79f9d60a78a4b006e204b6537adea720274c..0000000000000000000000000000000000000000
--- a/test/gtest_processor_motion.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-/*
- * gtest_processor_motion.cpp
- *
- * Created on: Sep 27, 2017
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "base/wolf.h"
-#include "base/logging.h"
-
-#include "base/sensor/sensor_odom_2D.h"
-#include "base/processor/processor_odom_2D.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::static_pointer_cast;
-
-class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
- public:
- Scalar dt;
- ProblemPtr problem;
- SensorOdom2DPtr sensor;
- ProcessorOdom2DPtr processor;
- CaptureMotionPtr capture;
- Vector2s data;
- Matrix2s data_cov;
-
- ProcessorMotion_test() :
- ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
- dt(0)
- { }
-
- virtual void SetUp()
- {
- dt = 1.0;
- problem = Problem::create("PO 2D");
- ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
- params->time_tolerance = 0.25;
- params->dist_traveled = 100;
- params->angle_turned = 6.28;
- params->max_time_span = 2.5*dt; // force KF at every third process()
- params->cov_det = 100;
- params->unmeasured_perturbation_std = 0.001;
- sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0)));
- processor = static_pointer_cast<ProcessorOdom2D>(problem->installProcessor("ODOM 2D", "odom", sensor, params));
- capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
-
- Vector3s x0; x0 << 0, 0, 0;
- Matrix3s P0; P0.setIdentity();
- problem->setPrior(x0, P0, 0.0, 0.01);
- }
-
- Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
- {
- return ProcessorMotion::interpolate(_ref, _second, _ts);
- }
-
- Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
- {
- return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
- }
-
-
- virtual void TearDown(){}
-
-};
-
-TEST_F(ProcessorMotion_test, IntegrateStraight)
-{
- data << 1, 0; // advance straight
- data_cov.setIdentity();
- TimeStamp t(0.0);
-
- for (int i = 0; i<9; i++)
- {
- t += dt;
- capture->setTimeStamp(t);
- capture->setData(data);
- capture->setDataCovariance(data_cov);
- processor->process(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
- }
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<9,0,0).finished(), 1e-8);
-}
-
-TEST_F(ProcessorMotion_test, IntegrateCircle)
-{
- data << 1, 2*M_PI/10; // advance in circle
- data_cov.setIdentity();
- TimeStamp t(0.0);
-
- for (int i = 0; i<10; i++) // one full turn exactly
- {
- t += dt;
- capture->setTimeStamp(t);
- capture->setData(data);
- capture->setDataCovariance(data_cov);
- processor->process(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
- }
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8);
-}
-
-TEST_F(ProcessorMotion_test, Interpolate)
-{
- data << 1, 2*M_PI/10; // advance in turn
- data_cov.setIdentity();
- TimeStamp t(0.0);
- std::vector<Motion> motions;
- motions.push_back(motionZero(t));
-
- for (int i = 0; i<10; i++) // one full turn exactly
- {
- t += dt;
- capture->setTimeStamp(t);
- capture->setData(data);
- capture->setDataCovariance(data_cov);
- processor->process(capture);
- motions.push_back(processor->getMotion(t));
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
- }
-
- TimeStamp tt = 2.2;
- Motion ref = motions[2];
- Motion second = motions[3];
- Motion interp = interpolate(ref, second, tt);
-
- ASSERT_NEAR( interp.ts_.get() , 2.2 , 1e-8);
- ASSERT_MATRIX_APPROX(interp.data_ , VectorXs::Zero(2) , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_ , VectorXs::Zero(3) , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[2].delta_integr_ , 1e-8);
-
- tt = 2.6;
- interp = interpolate(ref, second, tt);
-
- ASSERT_NEAR( interp.ts_.get() , 2.6 , 1e-8);
- ASSERT_MATRIX_APPROX(interp.data_ , motions[3].data_ , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_ , motions[3].delta_ , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-}
-
-TEST_F(ProcessorMotion_test, Interpolate_alternative)
-{
- data << 1, 2*M_PI/10; // advance in turn
- data_cov.setIdentity();
- TimeStamp t(0.0);
- std::vector<Motion> motions;
- motions.push_back(motionZero(t));
-
- for (int i = 0; i<10; i++) // one full turn exactly
- {
- t += dt;
- capture->setTimeStamp(t);
- capture->setData(data);
- capture->setDataCovariance(data_cov);
- processor->process(capture);
- motions.push_back(processor->getMotion(t));
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
- }
-
- TimeStamp tt = 2.2;
- Motion ref1 = motions[2];
- Motion ref2 = motions[3];
- Motion second(0.0, 2, 3, 3, 0);
- Motion interp = interpolate(ref1, ref2, tt, second);
-
- ASSERT_NEAR( interp.ts_.get() , 2.2 , 1e-8);
- ASSERT_MATRIX_APPROX(interp.data_ , VectorXs::Zero(2) , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_ , VectorXs::Zero(3) , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[2].delta_integr_ , 1e-8);
-
- ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8);
- ASSERT_MATRIX_APPROX(second.data_ , motions[3].data_ , 1e-8);
- ASSERT_MATRIX_APPROX(second.delta_ , motions[3].delta_ , 1e-8);
- ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-
- tt = 2.6;
- interp = interpolate(ref1, ref2, tt, second);
-
- ASSERT_NEAR( interp.ts_.get() , 2.6 , 1e-8);
- ASSERT_MATRIX_APPROX(interp.data_ , motions[3].data_ , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_ , motions[3].delta_ , 1e-8);
- ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-
- ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8);
- ASSERT_MATRIX_APPROX(second.data_ , VectorXs::Zero(2) , 1e-8);
- ASSERT_MATRIX_APPROX(second.delta_ , VectorXs::Zero(3) , 1e-8);
- ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_processor_tracker_feature_polyline_2D.cpp b/test/gtest_processor_tracker_feature_polyline_2D.cpp
deleted file mode 100644
index 2c3580aed605ca84a3214b5474178d9b7dbaa384..0000000000000000000000000000000000000000
--- a/test/gtest_processor_tracker_feature_polyline_2D.cpp
+++ /dev/null
@@ -1,1183 +0,0 @@
-/**
- * \file gtest_processor_tracker_feature_polyline_2D.cpp
- *
- * Created on: Apr 3, 2019
- * \author: jvallve
- */
-
-#include "base/processor/processor_tracker_feature_polyline_2D.h"
-#include "base/wolf.h"
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/ceres_wrapper/ceres_manager.h"
-
-#include <cmath>
-#include <iostream>
-
-using namespace Eigen;
-
-WOLF_PTR_TYPEDEFS(ProcessorPolylinePublicMethods);
-class ProcessorPolylinePublicMethods : public ProcessorTrackerFeaturePolyline2D
-{
- public:
- ProcessorPolylinePublicMethods(ProcessorParamsTrackerFeaturePolyline2DPtr _params) :
- ProcessorTrackerFeaturePolyline2D(_params)
- {
-
- }
-
- unsigned int trackFeatures(const FeatureBaseList& _features_last_in,
- FeatureBaseList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
- {
- return ProcessorTrackerFeaturePolyline2D::trackFeatures(_features_last_in,_features_incoming_out,_feature_correspondences);
- }
-
- bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
- {
- return ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(_origin_feature,
- _last_feature,
- _incoming_feature);
- }
-
- bool voteForKeyFrame()
- {
- return ProcessorTrackerFeaturePolyline2D::voteForKeyFrame();
- }
-
- unsigned int processNew(const unsigned int& _max_features)
- {
- return ProcessorTrackerFeaturePolyline2D::processNew(_max_features);
- }
-
- unsigned int detectNewFeatures(const unsigned int& _max_new_features,
- FeatureBaseList& _features_last_out)
- {
- return ProcessorTrackerFeaturePolyline2D::detectNewFeatures(_max_new_features, _features_last_out);
- }
-
- void establishConstraints()
- {
- ProcessorTrackerFeaturePolyline2D::establishConstraints();
- }
-
- void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id)
- {
- ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(_polyline_feature,
- _polyline_landmark,
- _ftr_point_id,
- _lmk_point_id,
- _lmk_prev_point_id);
- }
-
- void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id)
- {
- ProcessorTrackerFeaturePolyline2D::emplaceConstraintPoint(_polyline_feature,
- _polyline_landmark,
- _ftr_point_id,
- _lmk_point_id);
- }
-
- LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr)
- {
- return ProcessorTrackerFeaturePolyline2D::createLandmark(_feature_ptr);
- }
-
- bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
- {
- return ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch( lmk_match, pl_ftr);
- }
-
- void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
- const FeaturePolyline2DPtr& _ftr_last,
- const FeaturePolyline2DPtr& _ftr_incoming,
- const Eigen::Matrix3s& _T_last_incoming_prior) const
- {
- ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature( ftr_matches,
- _ftr_last,
- _ftr_incoming,
- _T_last_incoming_prior);
- }
-
- void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const
- {
- ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(lmk_matches,
- _lmk_ptr,
- _feat_ptr,
- _T_feature_landmark_prior);
- }
-
- bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const
- {
- return ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(_lmk_match, _lmk_ptr, _feat_ptr);
- }
-
- void computeTransformations()
- {
- ProcessorTrackerFeaturePolyline2D::computeTransformations();
- }
-
- LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
- FeatureMatchPolyline2DPtr ftr_match_incoming_last,
- LandmarkMatchPolyline2DPtr lmk_match_last) const
- {
- return ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(pl_incoming,
- ftr_match_incoming_last,
- lmk_match_last);
- }
-};
-
-class ProcessorPolyline2Dt : public testing::Test
-{
- public: //These can be accessed in fixtures
- ProcessorPolylinePublicMethodsPtr processor_pl;
- wolf::SensorBasePtr sensor_ptr;
- wolf::TimeStamp t;
- wolf::Scalar mti_clock, tmp;
- wolf::Scalar dt;
- Vector6s data;
- Matrix6s data_cov;
- VectorXs x0;
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
-
- //a new of this will be created for each new test
- virtual void SetUp()
- {
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
- sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
-
- // Time and data variables
- data = Vector6s::Zero();
- data_cov = Matrix6s::Identity();
-
- // Set the origin
- x0.resize(10);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-TEST(ProcessorIMU_constructors, ALL)
-{
- using namespace wolf;
-
- //constructor with ProcessorIMUParamsPtr argument only
- ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
- param_ptr->max_time_span = 2.0;
- param_ptr->max_buff_length = 20000;
- param_ptr->dist_traveled = 2.0;
- param_ptr->angle_turned = 2.0;
-
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
- ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
- ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
- ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
- ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
-
- //Factory constructor without yaml
- std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
- ProcessorParamsIMU params_default;
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default.angle_turned);
-
- //Factory constructor with yaml
- processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
-}
-
-TEST(ProcessorIMU, voteForKeyFrame)
-{
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 1;
- prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_imu_params->dist_traveled = 1000000000;
- prc_imu_params->angle_turned = 1000000000;
- prc_imu_params->voting_active = true;
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
-
- //setting origin
- VectorXs x0(10);
- TimeStamp t(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- //data variable and covariance matrix
- // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- Vector6s data;
- data << 1,0,0, 0,0,0;
- Matrix6s data_cov(Matrix6s::Identity());
- data_cov(0,0) = 0.5;
-
- // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
-
- // Time
- // we want more than one data to integrate otherwise covariance will be 0
- Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- /*There should be 3 frames :
- - 1 KeyFrame at origin
- - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
- - 1 frame that would be used by future data
- */
- ASSERT_EQ(problem->getTrajectoryPtr()->getFrameList().size(),(unsigned int) 3);
-
- /* if max_time_span == 2, Wolf tree should be
-
- Hardware
- S1
- pm5
- o: C2 - F2
- l: C4 - F3
- Trajectory
- KF1
- Estim, ts=0, x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0)
- C1 -> S1
- KF2
- Estim, ts=2.1, x = ( 2.2 0 -22 0 0 0 1 2.1 0 -21 0 0 0 0 0 0 )
- C2 -> S1
- f1
- m = ( 2.21 0 0 0 0 0 1 2.1 0 0 )
- c1 --> F1
- F3
- Estim, ts=2.1, x = ( . . .)
- C4 -> S1
- */
- //TODO : ASSERT TESTS to make sure the constraints are correctly set + check the tree above
-
-}
-
-//replace TEST by TEST_F if SetUp() needed
-TEST_F(ProcessorIMUt, interpolate)
-{
- using namespace wolf;
-
- t.set(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 0, 0, 0, 0; // only acc_x
- cap_imu_ptr->setData(data);
-
- // make one step to depart from origin
- cap_imu_ptr->setTimeStamp(0.05);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
-
- // make two steps, then pretend it's just one
- cap_imu_ptr->setTimeStamp(0.10);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
-
- cap_imu_ptr->setTimeStamp(0.15);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
- mot_final.delta_ = mot_final.delta_integr_;
- Motion mot_sec = mot_final;
-
-// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
-
-class P : public wolf::ProcessorIMU
-{
- public:
- P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
- Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
- {
- return ProcessorIMU::interpolate(ref, sec, ts);
- }
-} imu;
-
-Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
-
-ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
-//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
-ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
-
-}
-
-TEST_F(ProcessorIMUt, acc_x)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- Vector6s b; b << 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-}
-
-TEST_F(ProcessorIMUt, acc_y)
-{
- // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
- // difference hier is that we integrate over 1ms periods
-
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
- x << 0,10,0, 0,0,0,1, 0,20,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, acc_z)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 50; //how 0.1s
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
- x << 0,0,25, 0,0,0,1, 0,0,10;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, check_Covariance)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
-
- ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
-// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
-}
-
-TEST_F(ProcessorIMUt, gyro_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
-{
- // time
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
-
- // bias
- wolf::Scalar abx = 0.002;
- Vector6s bias; bias << abx,0,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
- // state
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
-
- // init things
- problem->setPrior(x0, P0, t, 0.01);
-
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
-// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
-
- // data
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x_true(10);
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- VectorXs x_est(10);
- x_est = problem->getCurrentState().head(10);
-
- ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- wolf::Scalar abx(0.002), aby(0.01);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector6s bias; bias << abx,aby,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
-
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
-// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
- data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
-// << "\n estimated state : " << x.transpose();
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
-// "\n expected state is : \n" << x.transpose() << std::endl;
-}
-
-TEST_F(ProcessorIMUt, gyro_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity and bias parts
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- //rotation + translation due to initial velocity
- x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_x_acc_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
- // v = a*dt = 0.001
- x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
- // v = a*dt = 1
- x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_acc_y)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
- // v = a*dt = 0.001
- x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
- // v = a*dt = 1
- x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_acc_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
- // v = a*dt = 0.001
- x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
- // v = a*dt = 1
- x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
deleted file mode 100644
index 5d656eda2fe23cde71e16b1ca15c389e11d65940..0000000000000000000000000000000000000000
--- a/test/gtest_rotation.cpp
+++ /dev/null
@@ -1,773 +0,0 @@
-/**
- * \file gtest_rotation.cpp
- *
- * Created on: Oct 13, 2016
- * \author: AtDinesh
- */
-
-//Eigen
-#include <Eigen/Geometry>
-
-//Wolf
-#include "base/wolf.h"
-#include "base/rotations.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-#include "utils_gtest.h"
-
-//#define write_results
-
-// THESE ARE UNITARY TESTS FOR METHODS IN ROTATION.H
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(exp_q, unit_norm)
-{
- Vector3s v0 = Vector3s::Random();
- Scalar scale = 1.0;
- for (int i = 0; i < 20; i++)
- {
- Vector3s v = v0 * scale;
- Quaternions q = exp_q(v);
- ASSERT_NEAR(q.norm(), 1.0, 1e-10) << "Failed at scale 1e-" << i << " with angle = " << 2.0*q.vec().norm();
- scale /= 10;
- }
-}
-
-TEST(rotations, pi2pi)
-{
- ASSERT_NEAR(M_PI_2, pi2pi((Scalar)M_PI_2), 1e-10);
- ASSERT_NEAR(-M_PI_2, pi2pi(3.0*M_PI_2), 1e-10);
- ASSERT_NEAR(-M_PI_2, pi2pi(-M_PI_2), 1e-10);
- ASSERT_NEAR(M_PI_2, pi2pi(-3.0*M_PI_2), 1e-10);
- // ASSERT_NEAR(M_PI, pi2pi(M_PI), 1e-10); // Exact PI is not safely testable because of numeric issues.
- ASSERT_NEAR(M_PI-.01, pi2pi(M_PI-.01), 1e-10);
- ASSERT_NEAR(-M_PI+.01, pi2pi(M_PI+.01), 1e-10);
-}
-
-TEST(skew, Skew_vee)
-{
- using namespace wolf;
- Vector3s vec3 = Vector3s::Random();
- Matrix3s skew_mat;
- skew_mat = skew(vec3);
-
- // vee
- Vector3s vec3_bis;
- vec3_bis = vee(skew_mat);
-
- ASSERT_TRUE(vec3_bis == vec3);
-}
-
-TEST(exp_q, v2q_q2v)
-{
- using namespace wolf;
- //defines scalars
- wolf::Scalar deg_to_rad = M_PI/180.0;
-
- Vector4s vec0, vec1;
-
- //v2q
- Vector3s rot_vector0, rot_vector1;
- rot_vector0 = Vector3s::Random();
- rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
- rot_vector0 = rot_vector0*deg_to_rad;
-
- Quaternions quat0, quat1;
- quat0 = v2q(rot_vector0);
- quat1 = v2q(rot_vector1);
-
- //q2v
- Vector3s quat_to_v0, quat_to_v1;
- VectorXs quat_to_v0x, quat_to_v1x;
-
- quat_to_v0 = q2v(quat0);
- quat_to_v1 = q2v(quat1);
- quat_to_v0x = q2v(quat0);
- quat_to_v1x = q2v(quat1);
-
- ASSERT_MATRIX_APPROX(rot_vector0, quat_to_v0, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(rot_vector1, quat_to_v1, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(rot_vector0, quat_to_v0x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(rot_vector1, quat_to_v1x, wolf::Constants::EPS);
-}
-
-TEST(exp_R, v2R_R2v)
-{
- using namespace wolf;
- //First test is to verify we get the good result with v -> v2R -> R2v -> v
- //test 2 : how small can angles in rotation vector be ?
-
- //definition
- wolf::Scalar deg_to_rad = M_PI/180.0;
- Vector3s rot_vector0, rot_vector1;
-
- rot_vector0 = Vector3s::Random();
- rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
- rot_vector0 = rot_vector0*deg_to_rad;
-
- Matrix3s rot0, rot1;
- rot0 = v2R(rot_vector0);
- rot1 = v2R(rot_vector1);
-
- //R2v
- Vector3s rot0_vec, rot1_vec;
- rot0_vec = R2v(rot0);
- rot1_vec = R2v(rot1);
-
- //check now
- ASSERT_MATRIX_APPROX(rot0_vec, rot_vector0, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(rot1_vec, rot_vector1, wolf::Constants::EPS);
-}
-
-TEST(log_R, R2v_v2R_limits)
-{
- using namespace wolf;
- //test 2 : how small can angles in rotation vector be ?
- wolf::Scalar scale = 1;
- Matrix3s v_to_R, initial_matrix;
- Vector3s R_to_v;
-
- //Vector3s rv;
- for(int i = 0; i<8; i++){
- initial_matrix = v2R(Vector3s::Random().eval() * scale);
-
- R_to_v = R2v(initial_matrix);
- v_to_R = v2R(R_to_v);
-
- ASSERT_MATRIX_APPROX(v_to_R, initial_matrix, wolf::Constants::EPS);
- scale = scale*0.1;
- }
-}
-
-TEST(log_R, R2v_v2R_AAlimits)
-{
- using namespace wolf;
- //let's see how small the angles can be here : limit reached at scale/10 = 1e-16
- wolf::Scalar scale = 1;
- Matrix3s rotation_mat;
- Vector3s rv;
-
- for(int i = 0; i<8; i++){
- rotation_mat = v2R(Vector3s::Random().eval() * scale);
-
- //rv = R2v(rotation_mat); //decomposing R2v below
- AngleAxis<wolf::Scalar> aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
- rv = aa0.axis() * aa0.angle();
- //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl;
-
- ASSERT_FALSE(rv == Vector3s::Zero());
- scale = scale*0.1;
- }
-}
-
-TEST(exp_q, v2q2R2v)
-{
- using namespace wolf;
- wolf::Scalar scale = 1;
- // testing new path : vector -> quaternion -> matrix -> vector
-
- for(int i = 0; i< 8; i++){
- Vector3s vector_ = Vector3s::Random()*scale;
- Quaternions quat_ = v2q(vector_);
- Matrix3s mat_ = quat_.matrix();
- Vector3s vector_bis = R2v(mat_);
-
- ASSERT_MATRIX_APPROX(vector_, vector_bis, wolf::Constants::EPS);
- scale = scale*0.1;
- }
-}
-
-TEST(rotations, AngleAxis_limits)
-{
- using namespace wolf;
- //Hypothesis : problem with construction of AngleAxis objects.
- // Example : if R = I + [delta t]_x (happenning in the IMU case with delta t = 0.001). Then angle mays be evaluated as 0 (due to cosinus definition ?)
- // Here we try to get the AngleAxis object from a random rotation matrix, then get back to the rotation matrix using AngleAxis::toRotationMatrix()
-
- wolf::Scalar scale = 1;
- Matrix3s res, res_i, rotation_mati, rotation_mat;
- Vector3s rv;
-
- for(int i = 0; i<8; i++){ //FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all.
- rotation_mat = v2R(Vector3s::Random().eval() * scale);
- rotation_mati = rotation_mat;
-
- //rv = R2v(rotation_mat); //decomposing R2v below
- AngleAxis<wolf::Scalar> aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
- rv = aa0.axis() * aa0.angle();
- //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl;
- res = aa0.toRotationMatrix();
-
- // now we set the diagonal to identity
- AngleAxis<wolf::Scalar> aa1 = AngleAxis<wolf::Scalar>(rotation_mat);
- rv = aa1.axis() * aa1.angle();
- //std::cout << "aa1.axis : " << aa0.axis().transpose() << ",\t aa1.angles :" << aa0.angle() <<std::endl;
- res_i = aa1.toRotationMatrix();
-
- ASSERT_MATRIX_APPROX(res, rotation_mat, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(res_i, rotation_mati, wolf::Constants::EPS);
- scale = scale*0.1;
- }
-}
-
-TEST(compose, Quat_compos_const_rateOfTurn)
-{
- using namespace wolf;
-
- // ********* constant rate of turn *********
-
- /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3
- (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
- But this is not OK, we cannot expect those 2 rotation integration to be equal.
- The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3
-
- more specifically :
- - At a constant velocity, because we keep a constant rotation axis, the integral is the same.
- - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3.
-
- We change the idea :
- define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz.
- Then compare the final orientation from rotation matrix composition and quaternion composition
- */
-
- wolf::Scalar deg_to_rad = M_PI/180.0;
- Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity());
- Eigen::Quaternions q0, qRot;
- q0.setIdentity();
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
-
- const unsigned int x_rot_vel = 5; // deg/s
- const unsigned int y_rot_vel = 2; // deg/s
- const unsigned int z_rot_vel = 10; // deg/s
-
- wolf::Scalar tmpx, tmpy, tmpz;
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = x_rot_vel * t; %express angle in rad before using sinus
- oy = y_rot_vel * t;
- oz = z_rot_vel * t;
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = x_rot_vel;
- wy = y_rot_vel;
- wz = z_rot_vel;
- */
-
- //there is no need to compute the rate of turn at each time because it is constant here :
- tmpx = deg_to_rad * x_rot_vel; // rad/s
- tmpy = deg_to_rad * y_rot_vel;
- tmpz = deg_to_rad * z_rot_vel;
- tmp_vec << tmpx, tmpy, tmpz;
- const wolf::Scalar dt = 0.1;
-
- for(unsigned int data_iter = 0; data_iter < 100; data_iter ++)
- {
- rot0 = rot0 * v2R(tmp_vec*dt);
- q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)
-
- }
-
- // Compare results from rotation matrix composition and quaternion composition
- qRot = (v2q(R2v(rot0)));
-
- Eigen::Vector3s final_orientation(q2v(qRot));
- ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
-}
-
-TEST(compose, Quat_compos_var_rateOfTurn)
-{
- using namespace wolf;
-
- //********* changing rate of turn - same freq for all axis *********
-
- /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3
- (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
- But this is not OK, we cannot expect those 2 rotation integration to be equal.
- The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3
-
- more specifically :
- - At a constant velocity, because we keep a constant rotation axis, the integral is the same.
- - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3.
-
- We change the idea :
- define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz.
- Then compare the final orientation from ox, oy, oz and quaternion we get by data integration
-
- ******* RESULT : *******
- The error in this test is due to discretization. The smaller is dt and the better is the integration !
- with dt = 0.001, the error is in 1e-5
- */
-
- wolf::Scalar deg_to_rad = M_PI/180.0;
- Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity());
- Eigen::Quaternions q0, qRot;
- q0.setIdentity();
-
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 15; // deg/s
- const unsigned int y_rot_vel = 15; // deg/s
- const unsigned int z_rot_vel = 15; // deg/s
-
- wolf::Scalar tmpx, tmpy, tmpz;
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(x_rot_vel * t * pi/180); %express angle in rad before using sinus
- oy = pi*sin(y_rot_vel * t * pi/180);
- oz = pi*sin(z_rot_vel * t * pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi * x_rot_vel * cos(x_rot_vel * t * pi/180) * pi/180;
- wy = pi * y_rot_vel * cos(y_rot_vel * t * pi/180) * pi/180;
- wz = pi * z_rot_vel * cos(z_rot_vel * t * pi/180) * pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter <= 10000; data_iter ++)
- {
- tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad;
- tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad;
- tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad;
- tmp_vec << tmpx, tmpy, tmpz;
-
- rot0 = rot0 * v2R(tmp_vec*dt);
- q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)
-
- time += dt;
- }
-
- // Compare results from rotation matrix composition and quaternion composition
- qRot = (v2q(R2v(rot0)));
-
- Eigen::Vector3s final_orientation(q2v(qRot));
-
- EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
- ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.0001)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
-
-}
-
-TEST(compose, Quat_compos_var_rateOfTurn_diff)
-{
- using namespace wolf;
-
- // ********* changing rate of turn - different freq for 1 axis *********
-
- /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3
- (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
- But this is not OK, we cannot expect those 2 rotation integration to be equal.
- The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3
-
- more specifically :
- - At a constant velocity, because we keep a constant rotation axis, the integral is the same.
- - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3.
-
- We change the idea :
- define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz.
- Then compare the final orientation from ox, oy, oz and quaternion we get by data integration
-
- ******* RESULT : *******
- Things are more tricky here. The errors go growing with time.
- with dt = 0.001, the error is in 1e-4 for 1 s integration ! But this may also depend on the frequency given to the rotation on each of the axis.
- */
-
- wolf::Scalar deg_to_rad = M_PI/180.0;
- Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity());
- Eigen::Quaternions q0, qRot;
- q0.setIdentity();
-
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 1; // deg/s
- const unsigned int y_rot_vel = 3; // deg/s
- const unsigned int z_rot_vel = 6; // deg/s
-
- wolf::Scalar tmpx, tmpy, tmpz;
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(x_rot_vel * t * pi/180); %express angle in rad before using sinus
- oy = pi*sin(y_rot_vel * t * pi/180);
- oz = pi*sin(z_rot_vel * t * pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi * x_rot_vel * cos(x_rot_vel * t * pi/180) * pi/180;
- wy = pi * y_rot_vel * cos(y_rot_vel * t * pi/180) * pi/180;
- wz = pi * z_rot_vel * cos(z_rot_vel * t * pi/180) * pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
- {
- tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad;
- tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad;
- tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad;
- tmp_vec << tmpx, tmpy, tmpz;
-
- rot0 = rot0 * v2R(tmp_vec*dt);
- q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)
-
- time += dt;
- }
-
- // Compare results from rotation matrix composition and quaternion composition
- qRot = (v2q(R2v(rot0)));
-
- Eigen::Vector3s final_orientation(q2v(qRot));
-
- EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
-
- ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.001)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
-}
-
-TEST(Plus, Random)
-{
- Quaternions q;
- q .coeffs().setRandom().normalize();
-
- Vector3s v;
- v .setRandom();
-
- Quaternions q2 = q * exp_q(v);
- Quaternions q3 = exp_q(v) * q;
-
- ASSERT_QUATERNION_APPROX(plus(q,v) , q2, 1e-12);
- ASSERT_QUATERNION_APPROX(plus_right(q,v), q2, 1e-12);
- ASSERT_QUATERNION_APPROX(plus_left(v,q) , q3, 1e-12);
-
-}
-
-TEST(Plus, Identity_plus_small)
-{
- Quaternions q;
- q .setIdentity();
-
- Vector3s v;
- v .setRandom();
- v *= 1e-6;
-
- Quaternions q2;
- q2.w() = 1;
- q2.vec() = 0.5*v;
-
- ASSERT_QUATERNION_APPROX(plus(q,v), q2, 1e-12);
-}
-
-TEST(Minus_and_diff, Random)
-{
- Quaternions q1, q2, qo;
- q1 .coeffs().setRandom().normalize();
- q2 .coeffs().setRandom().normalize();
-
- Vector3s vr = log_q(q1.conjugate() * q2);
- Vector3s vl = log_q(q2 * q1.conjugate());
-
- ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12);
- ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12);
- ASSERT_MATRIX_APPROX(minus_left(q1, q2), vl, 1e-12);
-
- qo = plus(q1, minus(q1, q2));
- if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
- ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
-
- qo = plus(q1, diff(q1, q2));
- if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
- ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
-
- qo = plus_left(minus_left(q1, q2), q1);
- if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
- ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
-}
-
-TEST(Jacobians, Jr)
-{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
-
- // Check the main Jr property for q and R
- // exp( theta + d_theta ) \approx exp(theta) * exp(Jr * d_theta)
- Matrix3s Jr = jac_SO3_right(theta);
- ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(theta) * exp_q(Jr*dtheta), 1e-8);
- ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(theta) * exp_R(Jr*dtheta)), 1e-8);
-}
-
-TEST(Jacobians, Jl)
-{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
-
- // Check the main Jl property for q and R
- // exp( theta + d_theta ) \approx exp(Jl * d_theta) * exp(theta)
- Matrix3s Jl = jac_SO3_left(theta);
- ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(Jl*dtheta) * exp_q(theta), 1e-8);
- ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(Jl*dtheta) * exp_R(theta)), 1e-8);
-
- // Jl = Jr.tr
- ASSERT_MATRIX_APPROX(Jl, jac_SO3_right(theta).transpose(), 1e-8);
-
- // Jl = R*Jr
- ASSERT_MATRIX_APPROX(Jl, exp_R(theta)*jac_SO3_right(theta), 1e-8);
-}
-
-TEST(Jacobians, Jr_inv)
-{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
- Quaternions q = v2q(theta);
- Matrix3s R = v2R(theta);
-
- // Check the main Jr_inv property for q and R
- // log( R * exp(d_theta) ) \approx log( R ) + Jrinv * d_theta
- Matrix3s Jr_inv = jac_SO3_right_inv(theta);
- ASSERT_MATRIX_APPROX(log_q(q * exp_q(dtheta)), log_q(q) + Jr_inv*dtheta, 1e-8);
- ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv*dtheta, 1e-8);
-}
-
-TEST(Jacobians, Jl_inv)
-{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
- Quaternions q = v2q(theta);
- Matrix3s R = v2R(theta);
-
- // Check the main Jl_inv property for q and R
- // log( exp(d_theta) * R ) \approx log( R ) + Jlinv * d_theta
- Matrix3s Jl_inv = jac_SO3_left_inv(theta);
- ASSERT_MATRIX_APPROX(log_q(exp_q(dtheta) * q), log_q(q) + Jl_inv*dtheta, 1e-8);
- ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv*dtheta, 1e-8);
-}
-
-TEST(Jacobians, compose)
-{
-
- Vector3s th1(.1,.2,.3), th2(.3,.1,.2);
- Quaternions q1(exp_q(th1));
- Quaternions q2(exp_q(th2));
- Quaternions qc;
- Matrix3s J1a, J2a, J1n, J2n;
-
- // composition and analytic Jacobians
- wolf::compose(q1, q2, qc, J1a, J2a);
-
- // Numeric Jacobians
- Scalar dx = 1e-6;
- Vector3s pert;
- Quaternions q1_pert, q2_pert, qc_pert;
- for (int i = 0; i<3; i++)
- {
- pert.setZero();
- pert(i) = dx;
-
- // Jac wrt q1
- q1_pert = q1*exp_q(pert);
- qc_pert = q1_pert * q2;
- J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
-
- // Jac wrt q2
- q2_pert = q2*exp_q(pert);
- qc_pert = q1 * q2_pert;
- J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
- }
-
- ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5);
- ASSERT_MATRIX_APPROX(J2a, J2n, 1e-5);
-}
-
-TEST(Jacobians, between)
-{
-
- Vector3s th1(.1,.2,.3), th2(.3,.1,.2);
- Quaternions q1(exp_q(th1));
- Quaternions q2(exp_q(th2));
- Quaternions qc;
- Matrix3s J1a, J2a, J1n, J2n;
-
- // composition and analytic Jacobians
- wolf::between(q1, q2, qc, J1a, J2a);
-
- // Numeric Jacobians
- Scalar dx = 1e-6;
- Vector3s pert;
- Quaternions q1_pert, q2_pert, qc_pert;
- for (int i = 0; i<3; i++)
- {
- pert.setZero();
- pert(i) = dx;
-
- // Jac wrt q1
- q1_pert = q1*exp_q(pert);
- qc_pert = q1_pert.conjugate() * q2;
- J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
-
- // Jac wrt q2
- q2_pert = q2*exp_q(pert);
- qc_pert = q1.conjugate() * q2_pert;
- J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
- }
-
- ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5);
- ASSERT_MATRIX_APPROX(J2a, J2n, 1e-5);
-}
-
-TEST(exp_q, small)
-{
- Vector3s u; u.setRandom().normalize();
- Vector3s v;
- Quaternions q;
- Scalar scale = 1.0;
- for (int i = 0; i<20; i++)
- {
- v = u*scale;
- q = exp_q(v);
- Vector3s ratio = q.vec().array() / v.array();
-
- WOLF_TRACE("scale = ", scale, "; ratio = ", ratio.transpose());
-
- scale /= 10;
- }
- ASSERT_MATRIX_APPROX(q.vec()/(10*scale), u/2, 1e-12);
-}
-
-TEST(log_q, double_cover)
-{
- Quaternions qp; qp.coeffs().setRandom().normalize();
- Quaternions qn; qn.coeffs() = - qp.coeffs();
- ASSERT_MATRIX_APPROX(log_q(qp), log_q(qn), 1e-16);
-}
-
-TEST(log_q, small)
-{
- Vector3s u; u.setRandom().normalize();
- Scalar scale = 1.0;
- for (int i = 0; i<20; i++)
- {
- Vector3s v = u*scale;
- Quaternions q = exp_q(v);
- Vector3s l = log_q(q);
-
- ASSERT_MATRIX_APPROX(v, l, 1e-10);
-
- scale /= 10;
- }
-}
-
-TEST(Conversions, q2R_R2q)
-{
- Vector3s v; v.setRandom();
- Quaternions q = v2q(v);
- Matrix3s R = v2R(v);
-
- Quaternions q_R = R2q(R);
- Quaternions qq_R(R);
-
- ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS);
- ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS);
- ASSERT_NEAR(qq_R.norm(), 1, wolf::Constants::EPS);
-
- ASSERT_MATRIX_APPROX(q.coeffs(), R2q(R).coeffs(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(R, q2R(q), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS);
-}
-
-TEST(Conversions, e2q_q2e)
-{
- Vector3s e, eo;
- Quaternions q;
-
- e << 0.1, .2, .3;
-
- q = e2q(e);
-
- eo = q2e(q);
- ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-
- eo = q2e(q.coeffs());
- ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-
-}
-
-TEST(Conversions, e2q_q2R_R2e)
-{
- Vector3s e, eo;
- Quaternions q;
- Matrix3s R;
-
- e << 0.1, .2, .3;
- q = e2q(e);
- R = q2R(q);
-
- eo = R2e(R);
-
- ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-}
-
-TEST(Conversions, e2R_R2e)
-{
- Vector3s e, eo;
- Matrix3s R;
-
- e << 0.1, 0.2, 0.3;
-
- R = e2R(e);
- eo = R2e(R);
- ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-}
-
-TEST(Conversions, e2R_R2q_q2e)
-{
- Vector3s e, eo;
- Quaternions q;
- Matrix3s R;
-
- e << 0.1, 0.2, 0.3;
- R = e2R(e);
- q = R2q(R);
-
- eo = q2e(q.coeffs());
-
- ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-}
-
-int main(int argc, char **argv)
-{
- using namespace wolf;
-
- /*
- LIST OF FUNCTIONS :
- - pi2pi
- - skew -> Skew_vee OK
- - vee -> Skew_vee OK
- - v2q v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
- - Matrix<T, 3, 1> q2v(const Quaternion<T>& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
- - VectorXs q2v(const Quaternions& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
- - v2R
- - R2v
- - jac_SO3_right
- - jac_SO3_right_inv
- - jac_SO3_left
- - jac_SO3_left_inv
- - quaternion composition
- */
-
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
deleted file mode 100644
index cd809e75cd0a414fdac833de89bcb1d768baf84f..0000000000000000000000000000000000000000
--- a/test/gtest_sensor_base.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/**
- * \file gtest_sensor_base.cpp
- *
- * Created on: Mar 27, 2018
- * \author: jsola
- */
-
-#include "base/sensor/sensor_base.h"
-
-#include "utils_gtest.h"
-
-using namespace wolf;
-
-TEST(SensorBase, setNoiseStd)
-{
- SensorBasePtr S (std::make_shared<SensorBase>("BASE", nullptr, nullptr, nullptr, 2)); // noise size 2
-
- Eigen::Vector2s noise_std = Eigen::Vector2s::Ones() * 0.1;
- Eigen::Matrix2s noise_cov = Eigen::Matrix2s::Identity() * 0.01;
-
- S->setNoiseStd(noise_std);
-
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
deleted file mode 100644
index 9ca4d657144b0ed1ae1568a4495f674e3c130dc7..0000000000000000000000000000000000000000
--- a/test/gtest_sensor_camera.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-/**
- * \file gtest_sensor_camera.cpp
- *
- * Created on: Feb 7, 2019
- * \author: jsola
- */
-
-
-#include "utils_gtest.h"
-
-#include "src/sensor/sensor_camera.cpp"
-#include "base/sensor/sensor_factory.h"
-
-using namespace wolf;
-
-TEST(SensorCamera, Img_size)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.width = 640;
- params.height = 480;
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- ASSERT_EQ(cam->getImgWidth() , 640);
- ASSERT_EQ(cam->getImgHeight(), 480);
-
- cam->setImgWidth(100);
- ASSERT_EQ(cam->getImgWidth() , 100);
-
- cam->setImgHeight(100);
- ASSERT_EQ(cam->getImgHeight(), 100);
-}
-
-TEST(SensorCamera, Intrinsics_Raw_Rectified)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.pinhole_model_raw << 321, 241, 321, 321;
- params.pinhole_model_rectified << 320, 240, 320, 320;
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- Eigen::Matrix3s K_raw, K_rectified;
- K_raw << 321, 0, 321, 0, 321, 241, 0, 0, 1;
- K_rectified << 320, 0, 320, 0, 320, 240, 0, 0, 1;
- Eigen::Vector4s k_raw(321,241,321,321);
- Eigen::Vector4s k_rectified(320,240,320,320);
-
- // default is raw
- ASSERT_TRUE(cam->isUsingRawImages());
- ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
-
- cam->useRectifiedImages();
- ASSERT_FALSE(cam->isUsingRawImages());
- ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsic()->getState(), 1e-8);
-
- cam->useRawImages();
- ASSERT_TRUE(cam->isUsingRawImages());
- ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
-}
-
-TEST(SensorCamera, Distortion)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.width = 640;
- params.height = 480;
- params.pinhole_model_raw << 321, 241, 321, 321;
- params.pinhole_model_rectified << 320, 240, 320, 320;
- params.distortion = Eigen::Vector3s( -0.3, 0.096, 0 );
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- Eigen::Vector3s d(-0.3, 0.096, 0);
-
- ASSERT_MATRIX_APPROX(d, cam->getDistortionVector(), 1e-8);
-}
-
-TEST(SensorCamera, Correction_zero)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.width = 640;
- params.height = 480;
- params.pinhole_model_raw << 321, 241, 321, 321;
- params.pinhole_model_rectified << 320, 240, 320, 320;
- params.distortion = Eigen::Vector3s( 0, 0, 0 );
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- Eigen::MatrixXs c(cam->getCorrectionVector().size(), 1);
- c.setZero();
-
- ASSERT_GE(cam->getCorrectionVector().size(), cam->getDistortionVector().size());
- ASSERT_MATRIX_APPROX(c, cam->getCorrectionVector(), 1e-8);
-}
-
-TEST(SensorCamera, create)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
- params->width = 640;
- params->height = 480;
- params->pinhole_model_raw << 321, 241, 321, 321;
- params->pinhole_model_rectified << 320, 240, 320, 320;
- params->distortion = Eigen::Vector3s( 0, 0, 0 );
-
- SensorBasePtr sen = SensorCamera::create("camera", extrinsics, params);
-
- ASSERT_NE(sen, nullptr);
-
- SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
-
- ASSERT_NE(cam, nullptr);
- ASSERT_EQ(cam->getImgWidth(), 640);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
deleted file mode 100644
index cebdce1f8da148e95c39422c422d9a44422c3dbe..0000000000000000000000000000000000000000
--- a/test/gtest_shared_from_this.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-#include "utils_gtest.h"
-#include "base/node_base.h"
-
-class CChildBase;
-
-class CParentBase : public wolf::NodeBase
-{
- public:
-
- std::list<std::shared_ptr<CChildBase> > child_list_;
-
- CParentBase() :
- NodeBase("")
- {};
-
- virtual ~CParentBase(){};
-
- virtual void addChild(std::shared_ptr<CChildBase> _child_ptr) final
- {
- child_list_.push_back(_child_ptr);
- }
-};
-
-class CParentDerived : public CParentBase
-{
- public:
-
- CParentDerived(){};
-};
-
-class CChildBase : public wolf::NodeBase, public std::enable_shared_from_this<CChildBase>
-{
- public:
- std::shared_ptr<CParentBase> parent_ptr_;
-
- CChildBase(std::shared_ptr<CParentBase> _parent_ptr) :
- NodeBase(""),
- parent_ptr_(_parent_ptr)
- {
- auto wptr = std::shared_ptr<CChildBase>( this, [](CChildBase*){} );
-
- parent_ptr_->addChild(shared_from_this());
- };
-};
-
-class CChildDerived : public CChildBase
-{
- public:
-
- CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){};
-};
-
-TEST(TestTest, SingleTest)
-{
- std::shared_ptr<CParentDerived> parent_derived_ptr = std::make_shared<CParentDerived>();
-
- std::shared_ptr<CChildDerived> child_derived_ptr = std::make_shared<CChildDerived>(parent_derived_ptr);
-
- ASSERT_EQ(child_derived_ptr, parent_derived_ptr->child_list_.front());
- ASSERT_EQ(parent_derived_ptr, child_derived_ptr->parent_ptr_);
-
- std::string parent_name("my booooring father...");
- std::string child_name("my intelligent son!");
- parent_derived_ptr->setName(parent_name);
- child_derived_ptr->setName(child_name);
-
- ASSERT_EQ(child_derived_ptr->getName(), child_name);
- ASSERT_EQ(parent_derived_ptr->getName(), parent_name);
- ASSERT_EQ(child_derived_ptr->getName(), parent_derived_ptr->child_list_.front()->getName());
- ASSERT_EQ(parent_derived_ptr->getName(), child_derived_ptr->parent_ptr_->getName());
-
- //std::cout << "parent_derived_ptr->getName() " << parent_derived_ptr->getName() << std::endl;
- //std::cout << "child_derived_ptr->getName() " << child_derived_ptr->getName() << std::endl;
- //std::cout << "child_derived_ptr->parent_ptr_->getName() " << child_derived_ptr->parent_ptr_->getName() << std::endl;
- //std::cout << "parent_derived_ptr->child_list_.front()->getName() " << parent_derived_ptr->child_list_.front()->getName() << std::endl;
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
deleted file mode 100644
index 04c7e6d7a06b2d76224c9b4c1df6e5da3a6f7f03..0000000000000000000000000000000000000000
--- a/test/gtest_solver_manager.cpp
+++ /dev/null
@@ -1,616 +0,0 @@
-/*
- * gtest_solver_manager.cpp
- *
- * Created on: Jun, 2018
- * Author: jvallve
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/problem.h"
-#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
-#include "base/capture/capture_void.h"
-#include "base/factor/factor_pose_2D.h"
-#include "base/solver/solver_manager.h"
-#include "base/local_parametrization_base.h"
-#include "base/local_parametrization_angle.h"
-
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-WOLF_PTR_TYPEDEFS(SolverManagerWrapper);
-class SolverManagerWrapper : public SolverManager
-{
- public:
- std::list<FactorBasePtr> factors_;
- std::map<StateBlockPtr,bool> state_block_fixed_;
- std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
-
- SolverManagerWrapper(const ProblemPtr& wolf_problem) :
- SolverManager(wolf_problem)
- {
- };
-
- bool isStateBlockRegistered(const StateBlockPtr& st) const
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st) const
- {
- return state_block_fixed_.at(st);
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
- {
- return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
- {
- return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
- };
-
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_block_local_param_.find(st) != state_block_local_param_.end();
- };
-
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const StateBlockPtrList& st_list){};
-
- // The following are dummy implementations
- bool hasConverged() { return true; }
- SizeStd iterations() { return 1; }
- Scalar initialCost() { return Scalar(1); }
- Scalar finalCost() { return Scalar(0); }
-
-
-
- protected:
-
- virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr)
- {
- factors_.push_back(fac_ptr);
- };
- virtual void removeFactor(const FactorBasePtr& fac_ptr)
- {
- factors_.remove(fac_ptr);
- };
- virtual void addStateBlock(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
- virtual void removeStateBlock(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_.erase(state_ptr);
- state_block_local_param_.erase(state_ptr);
- };
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- };
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
- {
- if (state_ptr->getLocalParametrization() == nullptr)
- state_block_local_param_.erase(state_ptr);
- else
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
-};
-
-TEST(SolverManager, Create)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // check double pointers to branches
- ASSERT_EQ(P, solver_manager_ptr->getProblem());
-}
-
-TEST(SolverManager, AddStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- // check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-}
-
-TEST(SolverManager, DoubleAddStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- // add stateblock again
- P->addStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- // check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-}
-
-TEST(SolverManager, UpdateStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // update solver
- solver_manager_ptr->update();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // check stateblock unfixed
- ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
-
- // Fix frame
- sb_ptr->fix();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // update solver manager
- solver_manager_ptr->update();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
-}
-
-TEST(SolverManager, AddUpdateStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // Fix state block
- sb_ptr->fix();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // update solver manager
- solver_manager_ptr->update();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
-}
-
-TEST(SolverManager, AddUpdateLocalParamStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_ptr);
-
- // Fix state block
- sb_ptr->fix();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
-
- // update solver manager
- solver_manager_ptr->update();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
-}
-
-TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_ptr);
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver manager
- solver_manager_ptr->update();
-
- // check stateblock localparam
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
-
- // Remove local param
- sb_ptr->removeLocalParametrization();
-
- // check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
-
- // update solver manager
- solver_manager_ptr->update();
-
- // check stateblock localparam
- ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
-}
-
-TEST(SolverManager, RemoveStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- // check stateblock
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-}
-
-TEST(SolverManager, AddRemoveStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- // check state block
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-}
-
-TEST(SolverManager, RemoveUpdateStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add state_block
- P->addStateBlock(sb_ptr);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-}
-
-TEST(SolverManager, DoubleRemoveStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // add stateblock
- P->addStateBlock(sb_ptr);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- // update solver manager
- solver_manager_ptr->update();
-}
-
-TEST(SolverManager, AddUpdatedStateBlock)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Fix
- sb_ptr->fix();
-
- // Set State
- Vector2s state_2 = 2*state;
- sb_ptr->setState(state_2);
-
- // Check flags have been set true
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->stateUpdated());
-
- // == When an ADD is notified, all previous notifications should be cleared (if not consumption of notifs) ==
-
- // add state_block
- P->addStateBlock(sb_ptr);
-
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
- ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second,ADD);
-
- // == Insert OTHER notifications ==
-
- // Set State --> FLAG
- state_2 = 2*state;
- sb_ptr->setState(state_2);
-
- // Fix --> FLAG
- sb_ptr->unfix();
-
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); // only ADD notification (fix and state are flags in sb)
-
- // == REMOVE should clear the previous notification (ADD) in the stack ==
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),0);// ADD + REMOVE = EMPTY
-
- // == UPDATES + REMOVE should clear the list of notifications ==
-
- // add state_block
- P->addStateBlock(sb_ptr);
-
- // update solver
- solver_manager_ptr->update();
-
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),0); // After solver_manager->update, notifications should be empty
-
- // Fix
- sb_ptr->fix();
-
- // Set State
- state_2 = 2*state;
- sb_ptr->setState(state_2);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
- ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second, REMOVE);
-}
-
-TEST(SolverManager, AddFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- // update solver
- solver_manager_ptr->update();
-
- // check factor
- ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
-}
-
-TEST(SolverManager, RemoveFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- // update solver
- solver_manager_ptr->update();
-
- // add factor
- P->removeFactor(c);
-
- // update solver
- solver_manager_ptr->update();
-
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
-}
-
-TEST(SolverManager, AddRemoveFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
- // add factor
- P->removeFactor(c);
-
- ASSERT_TRUE(P->getFactorNotificationMap().empty());
-
- // update solver
- solver_manager_ptr->update();
-
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
-}
-
-TEST(SolverManager, DoubleRemoveFactor)
-{
- ProblemPtr P = Problem::create("PO 2D");
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
-
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
- CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
- FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
-
- // update solver
- solver_manager_ptr->update();
-
- // remove factor
- P->removeFactor(c);
-
- // update solver
- solver_manager_ptr->update();
-
- // remove factor
- P->removeFactor(c);
-
- // update solver
- solver_manager_ptr->update();
-
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
deleted file mode 100644
index aad321781d7682c8c60dd83c9e0970e17118b164..0000000000000000000000000000000000000000
--- a/test/gtest_time_stamp.cpp
+++ /dev/null
@@ -1,219 +0,0 @@
-#include "utils_gtest.h"
-#include "base/time_stamp.h"
-
-#include <thread>
-
-TEST(WolfTestTimeStamp, TimeStampInitNow)
-{
- wolf::TimeStamp start;
-
- // If we don't sleep, start == time_stamp sometimes.
- // And sometimes start <= time_stamp ...
- std::this_thread::sleep_for(std::chrono::microseconds(1));
-
- ASSERT_NE(start.get(), 0);
-
- wolf::TimeStamp time_stamp;
-
-// std::cout << std::fixed;
-// std::cout << std::setprecision(15);
-// std::cout << start.get() << " | " << time_stamp.get() << std::endl;
-
- ASSERT_NE(time_stamp.get(), start.get());
-
- ASSERT_LT(start.get(), time_stamp.get());
-
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInitNow !\n");
-}
-
-TEST(WolfTestTimeStamp, TimeStampInitScalar)
-{
- wolf::Scalar val(101010);
-
- wolf::TimeStamp start(val);
-
- ASSERT_EQ(start.get(), val);
- ASSERT_EQ(start.getSeconds(), val);
- ASSERT_EQ(start.getNanoSeconds(), (unsigned int) 0);
-
- std::stringstream ss;
- start.print(ss);
-
- ASSERT_STREQ("101010.000000000", ss.str().c_str());
-
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n");
-}
-
-TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano)
-{
- wolf::Scalar sec(101010);
- wolf::Scalar nano(202020);
- wolf::Scalar val(101010.000202020);
-
- wolf::TimeStamp start(sec, nano);
-
- // start.get -> 101010.000202020004508
-
- ASSERT_EQ(start.get(), val);
- ASSERT_EQ(start.getSeconds(), sec);
- ASSERT_EQ(start.getNanoSeconds(), nano);
-
- std::stringstream ss;
- start.print(ss);
-
- ASSERT_STREQ("101010.000202020", ss.str().c_str());
-
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n");
-}
-
-TEST(WolfTestTimeStamp, TimeStampSetNow)
-{
- wolf::TimeStamp t1;
- wolf::TimeStamp t2(t1);
-
- ASSERT_EQ(t1,t2);
-
- // If we don't sleep, start == time_stamp sometimes.
- // And sometimes start <= time_stamp ...
- std::this_thread::sleep_for(std::chrono::microseconds(1));
-
- t2.setToNow();
-
- ASSERT_LT(t1,t2);
-}
-
-TEST(WolfTestTimeStamp, TimeStampSetScalar)
-{
- wolf::Scalar val(101010.000202020);
-
- wolf::TimeStamp start;
- start.set(val);
-
- ASSERT_EQ(start.get(), val);
- ASSERT_EQ(start.getSeconds(), 101010);
- ASSERT_EQ(start.getNanoSeconds(), 202020);
-
- std::stringstream ss;
- start.print(ss);
-
- ASSERT_STREQ("101010.000202020", ss.str().c_str());
-}
-
-TEST(WolfTestTimeStamp, TimeStampSetSecNano)
-{
- unsigned long int sec(101010);
- unsigned long int nano(202020);
-
- wolf::TimeStamp start;
- start.set(sec,nano);
-
- // start.get -> 101010.000202020004508
-
- ASSERT_EQ(start.getSeconds(), sec);
- ASSERT_EQ(start.getNanoSeconds(), nano);
-}
-
-TEST(WolfTestTimeStamp, TimeStampEquality)
-{
- wolf::TimeStamp start;
-
- wolf::TimeStamp time_stamp(start);
-
- ASSERT_EQ(time_stamp, start);
-
- ASSERT_EQ(time_stamp.get(), start.get());
-
- std::this_thread::sleep_for(std::chrono::microseconds(1));
- time_stamp.setToNow();
-
- ASSERT_NE(time_stamp.get(), start.get());
-
- time_stamp = start;
-
- ASSERT_EQ(time_stamp.get(), start.get());
-
-// PRINTF("All good at WolfTestTimeStamp::TimeStampEquality !\n");
-}
-
-TEST(WolfTestTimeStamp, TimeStampInequality)
-{
- wolf::TimeStamp start;
-
- std::this_thread::sleep_for(std::chrono::microseconds(1));
-
- wolf::TimeStamp time_stamp;
-
- // error: no match for ‘operator!=’
- //ASSERT_NE(time_stamp, start);
-
- ASSERT_LT(start, time_stamp);
- ASSERT_LE(start, time_stamp);
- ASSERT_LE(start, start);
-
- // error: no match for ‘operator>’
- ASSERT_GT(time_stamp, start);
- ASSERT_GE(time_stamp, start);
- ASSERT_GE(start, start);
-
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n");
-}
-
-TEST(WolfTestTimeStamp, TimeStampSubstraction)
-{
- wolf::TimeStamp t1;
- wolf::TimeStamp t2(t1);
- wolf::Scalar dt(1e-5);
-
- t2+=dt;
-
- ASSERT_LT(t1, t2);
- ASSERT_EQ(t2-t1, dt);
- ASSERT_EQ(t1-t2, -dt);
-}
-
-TEST(WolfTestTimeStamp, TimeStampAdding)
-{
- wolf::TimeStamp t1,t3;
- wolf::TimeStamp t2(t1);
- wolf::Scalar dt(1e-5);
-
- t2 +=dt;
- t3 = t1+dt;
-
- ASSERT_EQ(t2, t3);
-}
-
-TEST(WolfTestTimeStamp, TimeStampOperatorOstream)
-{
- wolf::TimeStamp t(5);
- wolf::Scalar dt = 1e-4;
- t+=dt;
-
- std::ostringstream ss1, ss2;
- t.print(ss1);
-
- ss2 << t;
-
- ASSERT_EQ(ss1.str(), ss2.str());
-
-// PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n");
-}
-
-TEST(WolfTestTimeStamp, TimeStampSecNanoSec)
-{
- unsigned long int sec = 5;
- unsigned long int nano = 1e5;
- wolf::TimeStamp t1(wolf::Scalar(sec)+wolf::Scalar(nano)*1e-9);
- wolf::TimeStamp t2(sec,nano);
-
- ASSERT_EQ(t1.getSeconds(),sec);
- ASSERT_EQ(t2.getSeconds(),sec);
- ASSERT_EQ(t1.getNanoSeconds(),nano);
- ASSERT_EQ(t2.getNanoSeconds(),nano);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
deleted file mode 100644
index e93c7c2701d1774c6c1e19dfb49c1917ec91d416..0000000000000000000000000000000000000000
--- a/test/gtest_track_matrix.cpp
+++ /dev/null
@@ -1,378 +0,0 @@
-/**
- * \file gtest_track_matrix.cpp
- *
- * Created on: Mar 24, 2018
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "base/track_matrix.h"
-
-#include "base/time_stamp.h"
-
-using namespace wolf;
-
-class TrackMatrixTest : public testing::Test
-{
- public:
- TrackMatrix track_matrix;
-
- Eigen::Vector2s m;
- Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01;
-
- CaptureBasePtr C0, C1, C2, C3, C4;
- FeatureBasePtr f0, f1, f2, f3, f4, f5;
-
- virtual void SetUp()
- {
- C0 = std::make_shared<CaptureBase>("BASE", 0.0);
- C1 = std::make_shared<CaptureBase>("BASE", 1.0);
- C2 = std::make_shared<CaptureBase>("BASE", 2.0);
- C3 = std::make_shared<CaptureBase>("BASE", 3.0);
- C4 = std::make_shared<CaptureBase>("BASE", 4.0);
-
- f0 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f1 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f2 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f3 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f4 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f5 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- }
-};
-
-TEST_F(TrackMatrixTest, newTrack)
-{
- track_matrix.newTrack(C0, f0);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
-
- track_matrix.newTrack(C0, f1);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
-}
-
-TEST_F(TrackMatrixTest, add)
-{
- track_matrix.newTrack(C0, f0);
-
- track_matrix.add(f0->trackId(), C1, f1);
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- */
- ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
- ASSERT_EQ(f1->trackId(), f0->trackId());
-
- track_matrix.add(f0->trackId(), C2, f2);
- /* C0 C1 C2 snapshots
- *
- * f0---f1---f2 trk 0
- */
- ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
- ASSERT_EQ(f2->trackId(), f0->trackId());
-}
-
-TEST_F(TrackMatrixTest, numTracks)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- */
-
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
-
- track_matrix.add(f0->trackId(), C0, f2);
-
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
-}
-
-TEST_F(TrackMatrixTest, trackSize)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- ASSERT_EQ(track_matrix.trackSize(f0->trackId()), (unsigned int) 2);
- ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 1);
-}
-
-TEST_F(TrackMatrixTest, first_last_Feature)
-{
- /* C0 C1 C2 snapshots
- *
- * f0--f1 trk 0
- * |
- * f2 trk 1
- */
-
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C1, f2);
-
- ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f0);
- ASSERT_EQ(track_matrix.lastFeature (f0->trackId()), f1);
- ASSERT_EQ(track_matrix.feature (f0->trackId(), C0 ), f0);
- ASSERT_EQ(track_matrix.feature (f0->trackId(), C1 ), f1);
- ASSERT_EQ(track_matrix.feature (f2->trackId(), C1 ), f2);
-}
-
-TEST_F(TrackMatrixTest, feature_time)
-{
- /* C0 C1 C2 snapshots
- *
- * f0--f1--f2 trk 0
- * |
- * f3--f4 trk 1
- * |
- * f5 trk 2
- */
-
- track_matrix.newTrack(C0,f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.add(f0->trackId(), C2, f2);
- track_matrix.newTrack(C1,f3);
- track_matrix.add(f3->trackId(), C2, f4);
- track_matrix.newTrack(C1,f5);
-
- // query f1 by exact, lacking and past time stamps
- ASSERT_EQ(track_matrix.feature(f0->trackId(), wolf::TimeStamp(1.0)), f1);
- ASSERT_EQ(track_matrix.feature(f0->trackId(), wolf::TimeStamp(0.9)), f1);
- ASSERT_NE(track_matrix.feature(f0->trackId(), wolf::TimeStamp(1.1)), f1);
-
- // query f3 by exact, lacking and past time stamps
- ASSERT_EQ(track_matrix.feature(f3->trackId(), wolf::TimeStamp(1.0)), f3);
- ASSERT_EQ(track_matrix.feature(f3->trackId(), wolf::TimeStamp(0.9)), f3);
- ASSERT_NE(track_matrix.feature(f3->trackId(), wolf::TimeStamp(1.1)), f3);
-
- // query f4 by exact, lacking and past time stamps
- ASSERT_EQ(track_matrix.feature(f3->trackId(), wolf::TimeStamp(2.0)), f4);
- ASSERT_EQ(track_matrix.feature(f3->trackId(), wolf::TimeStamp(1.9)), f4);
- ASSERT_NE(track_matrix.feature(f3->trackId(), wolf::TimeStamp(2.1)), f4);
-
- // query f5 by exact, lacking and past time stamps
- ASSERT_EQ(track_matrix.feature(f5->trackId(), wolf::TimeStamp(1.0)), f5);
- ASSERT_EQ(track_matrix.feature(f5->trackId(), wolf::TimeStamp(0.9)), f5);
- ASSERT_NE(track_matrix.feature(f5->trackId(), wolf::TimeStamp(1.1)), f5);
-
- // query a feature after the track's last time stamp --> should be empty
- ASSERT_TRUE(track_matrix.feature(f0->trackId(), wolf::TimeStamp(3.1)) == nullptr);
-
- // query for a feature in a non-existing track --> should be empty
- ASSERT_TRUE(track_matrix.feature(99, wolf::TimeStamp(1.0)) == nullptr);
-}
-
-
-TEST_F(TrackMatrixTest, remove_ftr)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
-
- track_matrix.remove(f0);
- /* C0 C1 C2 snapshots
- *
- * f1 trk 0
- *
- * f2 trk 1
- */
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
- ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f1);
- ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
- ASSERT_EQ(track_matrix.snapshot(C0).size(), (unsigned int) 1);
- ASSERT_EQ(track_matrix.snapshot(C0).at(f2->trackId()), f2);
- ASSERT_EQ(track_matrix.snapshot(C1).size(), (unsigned int) 1);
- ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1);
-
- track_matrix.remove(f1);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
-}
-
-TEST_F(TrackMatrixTest, remove_trk)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
-
- track_matrix.remove(f0->trackId());
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
-
- track_matrix.remove(f2->trackId());
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
-}
-
-TEST_F(TrackMatrixTest, remove_snapshot)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
-
- track_matrix.remove(C0);
- /* C1 C2 snapshots
- *
- * f1 trk 0
- */
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- ASSERT_EQ(track_matrix.firstFeature(f1->trackId()), f1);
-
- track_matrix.remove(C1);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
-}
-
-TEST_F(TrackMatrixTest, track)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- Track t0 = track_matrix.track(f0->trackId());
- Track t2 = track_matrix.track(f2->trackId());
-
- ASSERT_EQ(t0.size(), (unsigned int) 2);
- ASSERT_EQ(t0.at(C0->getTimeStamp()), f0);
- ASSERT_EQ(t0.at(C1->getTimeStamp()), f1);
-
- ASSERT_EQ(t2.size(), (unsigned int) 1);
- ASSERT_EQ(t2.at(C0->getTimeStamp()), f2);
-}
-
-TEST_F(TrackMatrixTest, snapshot)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- Snapshot s0 = track_matrix.snapshot(C0);
- Snapshot s1 = track_matrix.snapshot(C1);
-
- ASSERT_EQ(s0.size(), (unsigned int) 2);
- ASSERT_EQ(s0.at(f0->trackId()), f0);
- ASSERT_EQ(s0.at(f2->trackId()), f2);
-
- ASSERT_EQ(s1.size(), (unsigned int) 1);
- ASSERT_EQ(s1.at(f1->trackId()), f1);
-}
-
-TEST_F(TrackMatrixTest, trackAsVector)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- std::vector<FeatureBasePtr> vt0 = track_matrix.trackAsVector(f0->trackId()); // get track 0 as vector
-
- ASSERT_EQ(vt0.size(), (unsigned int) 2);
- ASSERT_EQ(vt0[0], f0);
- ASSERT_EQ(vt0.at(1), f1);
-}
-
-TEST_F(TrackMatrixTest, snapshotAsList)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
-
- /* C0 C1 C2 snapshots
- *
- * f0---f1 trk 0
- * |
- * f2 trk 1
- */
-
- std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector
-
- ASSERT_EQ(lt0.size() , (unsigned int) 2);
- ASSERT_EQ(lt0.front(), f0);
- ASSERT_EQ(lt0.back() , f2);
-}
-
-TEST_F(TrackMatrixTest, matches)
-{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.add(f0->trackId(), C2, f2);
- track_matrix.newTrack(C0, f3);
- track_matrix.add(f3->trackId(), C1, f4);
-
- /* C0 C1 C2 C3 snapshots
- *
- * f0---f1---f2 trk 0
- * | |
- * f3---f4 trk 1
- */
-
- TrackMatches pairs = track_matrix.matches(C0, C2);
-
- ASSERT_EQ(pairs.size(), (unsigned int) 1);
- ASSERT_EQ(pairs.at(f0->trackId()).first, f0);
- ASSERT_EQ(pairs.at(f0->trackId()).second, f2);
-
- pairs = track_matrix.matches(C2, C3);
-
- ASSERT_EQ(pairs.size(), (unsigned int) 0);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
deleted file mode 100644
index d77defcbc697d87cf827a465d1ef99e9221293a6..0000000000000000000000000000000000000000
--- a/test/gtest_trajectory.cpp
+++ /dev/null
@@ -1,260 +0,0 @@
-/*
- * gtest_trajectory.cpp
- *
- * Created on: Nov 13, 2016
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-#include "base/logging.h"
-
-#include "base/problem.h"
-#include "base/trajectory_base.h"
-#include "base/frame_base.h"
-
-#include <iostream>
-
-using namespace wolf;
-
-struct DummyNotificationProcessor
-{
- DummyNotificationProcessor(ProblemPtr _problem)
- : problem_(_problem)
- {
- //
- }
-
- void update()
- {
- if (problem_ == nullptr)
- {
- FAIL() << "problem_ is nullptr !";
- }
-
- auto sb_noti_pair = problem_->getStateBlockNotificationMap().begin();
- while (sb_noti_pair != problem_->getStateBlockNotificationMap().end())
- {
- switch (sb_noti_pair->second)
- {
- case ADD:
- {
- break;
- }
- case REMOVE:
- {
- break;
- }
- default:
- throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE.");
- }
- sb_noti_pair = problem_->getStateBlockNotificationMap().erase(sb_noti_pair);
- }
- ASSERT_TRUE(problem_->getStateBlockNotificationMap().empty());
- }
-
- ProblemPtr problem_;
-};
-
-/// Set to true if you want debug info
-bool debug = false;
-
-TEST(TrajectoryBase, ClosestKeyFrame)
-{
-
- ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectory();
-
- // Trajectory status:
- // kf1 kf2 f3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, nullptr, nullptr);
- FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, nullptr, nullptr);
- FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr);
- T->addFrame(f1);
- T->addFrame(f2);
- T->addFrame(f3);
-
- FrameBasePtr kf; // closest key-frame queried
-
- kf = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0
- ASSERT_EQ(kf->id(), f1->id()); // same id!
-
- kf = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return f1
- ASSERT_EQ(kf->id(), f1->id()); // same id!
-
- kf = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return f2
- ASSERT_EQ(kf->id(), f2->id()); // same id!
-
- kf = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return f2
- ASSERT_EQ(kf->id(), f2->id()); // same id!
-
- kf = T->closestKeyFrameToTimeStamp(3.2); // after the last frame --> return f2
- ASSERT_EQ(kf->id(), f2->id()); // same id!
-}
-
-TEST(TrajectoryBase, Add_Remove_Frame)
-{
- using std::make_shared;
-
- ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectory();
-
- DummyNotificationProcessor N(P);
-
- // Trajectory status:
- // kf1 kf2 f3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-
- std::cout << __LINE__ << std::endl;
-
- // add frames and keyframes
- T->addFrame(f1); // KF
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
- std::cout << __LINE__ << std::endl;
-
- T->addFrame(f2); // KF
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
- std::cout << __LINE__ << std::endl;
-
- T->addFrame(f3); // F
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
- std::cout << __LINE__ << std::endl;
-
- ASSERT_EQ(T->getLastFrame()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
- std::cout << __LINE__ << std::endl;
-
- N.update();
- std::cout << __LINE__ << std::endl;
-
- // remove frames and keyframes
- f2->remove(); // KF
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
- std::cout << __LINE__ << std::endl;
-
- ASSERT_EQ(T->getLastFrame()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
- std::cout << __LINE__ << std::endl;
-
- f3->remove(); // F
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
- std::cout << __LINE__ << std::endl;
-
- ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
-
- f1->remove(); // KF
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
- std::cout << __LINE__ << std::endl;
-
- N.update();
-
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 0);
- std::cout << __LINE__ << std::endl;
-}
-
-TEST(TrajectoryBase, KeyFramesAreSorted)
-{
- using std::make_shared;
-
- ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectory();
-
- // Trajectory status:
- // kf1 kf2 f3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-
- // add frames and keyframes in random order --> keyframes must be sorted after that
- T->addFrame(f2); // KF2
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
-
- T->addFrame(f3); // F3
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
-
- T->addFrame(f1); // KF1
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
-
- f3->setKey(); // set KF3
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
-
- FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
- T->addFrame(f4);
- // Trajectory status:
- // kf1 kf2 kf3 f4 frames
- // 1 2 3 1.5 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f4->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
-
- f4->setKey();
- // Trajectory status:
- // kf1 kf4 kf2 kf3 frames
- // 1 1.5 2 3 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
-
- f2->setTimeStamp(4);
- // Trajectory status:
- // kf1 kf4 kf3 kf2 frames
- // 1 1.5 3 4 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
-
- f4->setTimeStamp(0.5);
- // Trajectory status:
- // kf4 kf2 kf3 kf2 frames
- // 0.5 1 3 4 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getFrameList().front()->id(), f4->id());
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/processor_IMU_UnitTester.cpp b/test/processor_IMU_UnitTester.cpp
deleted file mode 100644
index aa4f46db66546395e9b4d07920407e2c0b52a597..0000000000000000000000000000000000000000
--- a/test/processor_IMU_UnitTester.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "processor_IMU_UnitTester.h"
-
-namespace wolf {
-
-ProcessorIMU_UnitTester::ProcessorIMU_UnitTester() :
- ProcessorIMU(std::make_shared<ProcessorParamsIMU>()),
- Dq_out_(nullptr)
-{}
-
-ProcessorIMU_UnitTester::~ProcessorIMU_UnitTester(){}
-
-} // namespace wolf
-
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h
deleted file mode 100644
index c114086a16cae9cb28859270b3784461d7bb7773..0000000000000000000000000000000000000000
--- a/test/processor_IMU_UnitTester.h
+++ /dev/null
@@ -1,379 +0,0 @@
-
-#ifndef PROCESSOR_IMU_UNITTESTER_H
-#define PROCESSOR_IMU_UNITTESTER_H
-
-// Wolf
-#include "base/processor/processor_IMU.h"
-#include "base/processor/processor_motion.h"
-
-namespace wolf {
- struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
-
- IMU_jac_bias(Eigen::Matrix<Eigen::VectorXs,6,1> _Deltas_noisy_vect,
- Eigen::VectorXs _Delta0 ,
- Eigen::Matrix3s _dDp_dab,
- Eigen::Matrix3s _dDv_dab,
- Eigen::Matrix3s _dDp_dwb,
- Eigen::Matrix3s _dDv_dwb,
- Eigen::Matrix3s _dDq_dwb) :
- Deltas_noisy_vect_(_Deltas_noisy_vect),
- Delta0_(_Delta0) ,
- dDp_dab_(_dDp_dab),
- dDv_dab_(_dDv_dab),
- dDp_dwb_(_dDp_dwb),
- dDv_dwb_(_dDv_dwb),
- dDq_dwb_(_dDq_dwb)
- {
- //
- }
-
- IMU_jac_bias(){
-
- for (int i=0; i<6; i++){
- Deltas_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
- }
-
- Delta0_ = Eigen::VectorXs::Zero(1,1);
- dDp_dab_ = Eigen::Matrix3s::Zero();
- dDv_dab_ = Eigen::Matrix3s::Zero();
- dDp_dwb_ = Eigen::Matrix3s::Zero();
- dDv_dwb_ = Eigen::Matrix3s::Zero();
- dDq_dwb_ = Eigen::Matrix3s::Zero();
- }
-
- IMU_jac_bias(IMU_jac_bias const & toCopy){
-
- Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
- Delta0_ = toCopy.Delta0_;
- dDp_dab_ = toCopy.dDp_dab_;
- dDv_dab_ = toCopy.dDv_dab_;
- dDp_dwb_ = toCopy.dDp_dwb_;
- dDv_dwb_ = toCopy.dDv_dwb_;
- dDq_dwb_ = toCopy.dDq_dwb_;
- }
-
- public:
- /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
- place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
- place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
- */
- Eigen::Matrix<Eigen::VectorXs,6,1> Deltas_noisy_vect_;
- Eigen::VectorXs Delta0_;
- Eigen::Matrix3s dDp_dab_;
- Eigen::Matrix3s dDv_dab_;
- Eigen::Matrix3s dDp_dwb_;
- Eigen::Matrix3s dDv_dwb_;
- Eigen::Matrix3s dDq_dwb_;
-
- public:
- void copyfrom(IMU_jac_bias const& right){
-
- Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
- Delta0_ = right.Delta0_;
- dDp_dab_ = right.dDp_dab_;
- dDv_dab_ = right.dDv_dab_;
- dDp_dwb_ = right.dDp_dwb_;
- dDv_dwb_ = right.dDv_dwb_;
- dDq_dwb_ = right.dDq_dwb_;
- }
- };
-
- struct IMU_jac_deltas{
-
- IMU_jac_deltas(Eigen::VectorXs _Delta0,
- Eigen::VectorXs _delta0,
- Eigen::Matrix<Eigen::VectorXs,9,1> _Delta_noisy_vect,
- Eigen::Matrix<Eigen::VectorXs,9,1> _delta_noisy_vect,
- Eigen::MatrixXs _jacobian_delta_preint,
- Eigen::MatrixXs _jacobian_delta ) :
- Delta0_(_Delta0),
- delta0_(_delta0),
- Delta_noisy_vect_(_Delta_noisy_vect),
- delta_noisy_vect_(_delta_noisy_vect),
- jacobian_delta_preint_(_jacobian_delta_preint),
- jacobian_delta_(_jacobian_delta)
- {
- //
- }
-
- IMU_jac_deltas(){
- for (int i=0; i<9; i++){
- Delta_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
- delta_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
- }
-
- Delta0_ = Eigen::VectorXs::Zero(1,1);
- delta0_ = Eigen::VectorXs::Zero(1,1);
- jacobian_delta_preint_ = Eigen::MatrixXs::Zero(9,9);
- jacobian_delta_ = Eigen::MatrixXs::Zero(9,9);
- }
-
- IMU_jac_deltas(IMU_jac_deltas const & toCopy){
-
- Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
- delta_noisy_vect_ = toCopy.delta_noisy_vect_;
-
- Delta0_ = toCopy.Delta0_;
- delta0_ = toCopy.delta0_;
- jacobian_delta_preint_ = toCopy.jacobian_delta_preint_;
- jacobian_delta_ = toCopy.jacobian_delta_;
- }
-
- public:
- /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
- Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx, dVx, dvx,..., dOz,doz).
- Delta_noisy_vect_ delta_noisy_vect_
- 0: + 0, 0: + 0
- 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz
- 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz
- 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 9: +dvy, +: +dvz
- */
- Eigen::VectorXs Delta0_; //this will contain the Delta not affected by noise
- Eigen::VectorXs delta0_; //this will contain the delta not affected by noise
- Eigen::Matrix<Eigen::VectorXs,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises
- Eigen::Matrix<Eigen::VectorXs,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises
- Eigen::MatrixXs jacobian_delta_preint_;
- Eigen::MatrixXs jacobian_delta_;
-
- public:
- void copyfrom(IMU_jac_deltas const& right){
-
- Delta_noisy_vect_ = right.Delta_noisy_vect_;
- delta_noisy_vect_ = right.delta_noisy_vect_;
- Delta0_ = right.Delta0_;
- delta0_ = right.delta0_;
- jacobian_delta_preint_ = right.jacobian_delta_preint_;
- jacobian_delta_ = right.jacobian_delta_;
- }
- };
-
- class ProcessorIMU_UnitTester : public ProcessorIMU{
-
- public:
-
- ProcessorIMU_UnitTester();
- virtual ~ProcessorIMU_UnitTester();
-
- //Functions to test jacobians with finite difference method
-
- /* params :
- _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
- _dt : time interval between 2 IMU measurements
- da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one.
- */
- IMU_jac_bias finite_diff_ab(const Scalar _dt,
- Eigen::Vector6s& _data,
- const wolf::Scalar& da_b,
- const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0);
-
- /* params :
- _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
- _dt : time interval between 2 IMU measurements
- _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
- _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
- */
- IMU_jac_deltas finite_diff_noise(const Scalar& _dt,
- Eigen::Vector6s& _data,
- const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise,
- const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise,
- const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr = nullptr);
-
- public:
- // Maps quat, to be used as temporary
- Eigen::Map<Eigen::Quaternions> Dq_out_;
-
- };
-
-}
-
-/////////////////////////////////////////////////////////
-// IMPLEMENTATION. Put your implementation includes here
-/////////////////////////////////////////////////////////
-
-// Wolf
-#include "base/state_block.h"
-#include "base/rotations.h"
-
-namespace wolf{
-
- //Functions to test jacobians with finite difference method
-inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt,
- Eigen::Vector6s& _data,
- const wolf::Scalar& da_b,
- const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0)
-{
- //TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
- ///Define all the needed variables
- Eigen::VectorXs Delta0;
- Eigen::Matrix<Eigen::VectorXs,6,1> Deltas_noisy_vect;
- Eigen::Vector6s data0;
- data0 = _data;
-
- Eigen::MatrixXs data_cov;
- Eigen::MatrixXs jacobian_delta_preint;
- Eigen::MatrixXs jacobian_delta;
- Eigen::VectorXs delta_preint_plus_delta0;
- data_cov.resize(6,6);
- jacobian_delta_preint.resize(9,9);
- jacobian_delta.resize(9,9);
- delta_preint_plus_delta0.resize(10);
-
- //set all variables to 0
- data_cov = Eigen::MatrixXs::Zero(6,6);
- jacobian_delta_preint = Eigen::MatrixXs::Zero(9,9);
- jacobian_delta = Eigen::MatrixXs::Zero(9,9);
- delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
-
- Vector6s bias = Vector6s::Zero();
-
- /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
- place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
- place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
- */
-
- Eigen::Matrix3s dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb;
-
- //Deltas without use of da_b
- computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_);
- deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
- MatrixXs jacobian_bias = jacobian_delta * jacobian_delta_calib_;
- Delta0 = delta_preint_plus_delta0; //this is the first preintegrated delta, not affected by any added bias noise
- dDp_dab = jacobian_bias.block(0,0,3,3);
- dDp_dwb = jacobian_bias.block(0,3,3,3);
- dDq_dwb = jacobian_bias.block(3,3,3,3);
- dDv_dab = jacobian_bias.block(6,0,3,3);
- dDv_dwb = jacobian_bias.block(6,3,3,3);
-
-
- // propagate bias noise
- for(int i=0; i<6; i++){
- //need to reset stuff here
- delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
- data_cov = Eigen::MatrixXs::Zero(6,6);
-
- // add da_b
- _data = data0;
- _data(i) = _data(i) - da_b; //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
- //data2delta
- computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
- deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
- Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
- }
-
- IMU_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
- return bias_jacobians;
-}
-
-inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _dt, Eigen::Vector6s& _data, const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise, const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise, const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0)
-{
- //we do not propagate any noise from data vector
- Eigen::VectorXs Delta_; //will contain the preintegrated Delta affected by added noise
- Eigen::VectorXs delta0; //will contain the delta not affected by added noise
- // delta_ that /will contain the delta affected by added noise is declared in processor_motion.h
- Eigen::VectorXs delta_preint_plus_delta;
- delta0.resize(10);
- delta_preint_plus_delta.resize(10);
- delta_preint_plus_delta << 0,0,0 ,0,0,0,1 ,0,0,0;
-
- Eigen::MatrixXs jacobian_delta_preint; //will be used as input for deltaPlusDelta
- Eigen::MatrixXs jacobian_delta; //will be used as input for deltaPlusDelta
- jacobian_delta_preint.resize(9,9);
- jacobian_delta.resize(9,9);
- jacobian_delta_preint.setIdentity(9,9);
- jacobian_delta.setIdentity(9,9);
- Eigen::MatrixXs jacobian_delta_preint0; //will contain the jacobian we want to check
- Eigen::MatrixXs jacobian_delta0; //will contain the jacobian we want to check
- jacobian_delta_preint0.resize(9,9);
- jacobian_delta0.resize(9,9);
- jacobian_delta_preint0.setIdentity(9,9);
- jacobian_delta0.setIdentity(9,9);
-
- Eigen::MatrixXs data_cov; //will be used filled with Zeros as input for data2delta
- data_cov.resize(6,6);
- data_cov = Eigen::MatrixXs::Zero(6,6);
-
- Eigen::Matrix<Eigen::VectorXs,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises
- Eigen::Matrix<Eigen::VectorXs,9,1> delta_noisy_vect; //this will contain the deltas affected by noises
-
- Vector6s bias = Vector6s::Zero();
-
- computeCurrentDelta(_data, data_cov, bias,_dt,delta_,delta_cov_,jacobian_delta_calib_); //Affects dp_out, dv_out and dq_out
- delta0 = delta_; //save the delta that is not affected by noise
- deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta);
- jacobian_delta_preint0 = jacobian_delta_preint;
- jacobian_delta0 = jacobian_delta;
-
- //We compute all the jacobians wrt deltas and noisy deltas
- for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
- {
- //PQV formulation
- //Add perturbation in position
- delta_ = delta0;
- delta_(i) = delta_(i) + _delta_noise(i); //noise has been added
- delta_noisy_vect(i) = delta_;
-
- //Add perturbation in velocity
- /*
- delta_ is size 10 (P Q V), _delta_noise is size 9 (P O V)
- */
- delta_ = delta0;
- delta_(i+7) = delta_(i+7) + _delta_noise(i+6); //noise has been added
- delta_noisy_vect(i+6) = delta_;
- }
-
- for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
- {
- //PQV formulation
- //fist we need to reset some stuff
- Eigen::Vector3s dtheta = Eigen::Vector3s::Zero();
-
- delta_ = delta0;
- new (&Dq_out_) Map<Quaternions>(delta_.data() + 3); //not sure that we need this
- dtheta(i) += _delta_noise(i+3); //introduce perturbation
- Dq_out_ = Dq_out_ * v2q(dtheta);
- delta_noisy_vect(i+3) = delta_;
- }
-
- //We compute all the jacobians wrt Deltas and noisy Deltas
- for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
- {
- //PQV formulation
- //Add perturbation in position
- Delta_ = _Delta0;
- Delta_(i) = Delta_(i) + _Delta_noise(i); //noise has been added
- Delta_noisy_vect(i) = Delta_;
-
- //Add perturbation in velocity
- /*
- Delta_ is size 10 (P Q V), _Delta_noise is size 9 (P O V)
- */
- Delta_ = _Delta0;
- Delta_(i+7) = Delta_(i+7) + _Delta_noise(i+6); //noise has been added
- Delta_noisy_vect(i+6) = Delta_;
- }
-
- for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
- {
- //fist we need to reset some stuff
- Eigen::Vector3s dtheta = Eigen::Vector3s::Zero();
-
- Delta_ = _Delta0;
- new (&Dq_out_) Map<Quaternions>(Delta_.data() + 3);
- dtheta(i) += _Delta_noise(i+3); //introduce perturbation
- Dq_out_ = Dq_out_ * v2q(dtheta);
- Delta_noisy_vect(i+3) = Delta_;
- }
-
- IMU_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
- return jac_deltas;
-
-}
-
-} // namespace wolf
-
-#endif // PROCESSOR_IMU_UNITTESTER_H
diff --git a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
index 8532cc9f2d32246ed536a340574251a6b34ce379..ae4a43a3bdd7733931da9b42fb87e279f529a7a2 100644
--- a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
+++ b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
@@ -5,7 +5,7 @@
* Author: Jeremie Deray
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "../../utils_gtest.h"
#include "../../../serialization/cereal/serialization_eigen_geometry.h"
diff --git a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
index 5ca60e7f111f5b9e56392bcf59af3123c29637b6..c7f7249a5747d2e848bf07ea892b8486ee1f2eaa 100644
--- a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
+++ b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
@@ -5,7 +5,7 @@
* Author: Jeremie Deray
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "../../utils_gtest.h"
#include "../../../serialization/cereal/serialization_eigen_sparse.h"