diff --git a/.gitignore b/.gitignore
index fae25dce1facaccabf8aa15085cf2491d5959212..77906100d4ae252720e456622103b13d8c6371e5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -31,5 +31,8 @@ src/examples/map_apriltag_save.yaml
\.vscode/
build_release/
-
-wolf.found
+.clangd
+wolfcore.found
+/wolf.found
+.ccls*
+compile_commands.json
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 93de8a4bc9455e83e2f10e181cfb681ff40457e8..050de2e95581cfa91b0ad4569c3508592ef96e36 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,70 +1,78 @@
-image: segaleran/ceres
-
-before_script:
- - ls
- - apt-get update
- - apt-get install -y build-essential cmake
-
-# SPDLOG
-# - apt-get install -y libspdlog-dev
- - if [ -d spdlog ]; then
- - echo "directory exists"
- - if [ "$(ls -A ./spdlog)" ]; then
- - echo "directory not empty"
- - cd spdlog
- - git pull
- - else
- - echo "directory empty"
- - git clone https://github.com/gabime/spdlog.git
- - cd spdlog
- - fi
- - else
- - echo "directory inexistent"
- - git clone https://github.com/gabime/spdlog.git
- - cd spdlog
- - fi
- - git fetch
- - git checkout v0.17.0
+.build_and_test_template: &build_and_test_definition
- mkdir -pv build
- cd build
- - ls
- - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF ..
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
+ - make -j$WOLF_N_PROC
+ - ctest -j$WOLF_N_PROC
+ # run demos
+ - ../bin/hello_wolf
+ - ../bin/hello_wolf_autoconf
- make install
- - cd ../..
-# YAML
-# - apt-get install -y libyaml-cpp-dev
- - if [ -d yaml-cpp ]; then
- - echo "directory exists"
- - if [ "$(ls -A ./yaml-cpp)" ]; then
- - echo "directory not empty"
- - cd yaml-cpp
- - git pull
- - else
- - echo "directory empty"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
- - fi
- - else
- - echo "directory inexistent"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
- - fi
- - mkdir -pv build
- - cd build
- - ls
- - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
- - make install
- - cd ../..
+stages:
+ - build_and_test
+ - deploy
-wolf_build_and_test:
- stage: build
+build_and_test:xenial:
+ stage: build_and_test
+ image: labrobotica/wolf_deps:16.04
except:
- master
script:
- - mkdir -pv build
- - cd build
- - ls # we can check whether the directory was already full
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
- - make -j$(nproc)
- - ctest
+ - *build_and_test_definition
+
+build_and_test:bionic:
+ stage: build_and_test
+ image: labrobotica/wolf_deps:18.04
+ except:
+ - master
+ script:
+ - *build_and_test_definition
+
+deploy_imu:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
+ strategy: depend
+
+deploy_gnss:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
+ strategy: depend
+
+deploy_vision:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
+ strategy: depend
+
+deploy_laser:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
+ strategy: depend
+
+deploy_apriltag:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
+ strategy: depend
+
+deploy_bodydynamics:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
+ strategy: depend
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cd81990329ddee7e5b82b04ce5cab1a28b7a3b29..1789e63307e8231e2d27547204b4fa710ef33733 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,40 +1,29 @@
+#Start WOLF build
+MESSAGE("Starting WOLF CMakeLists ...")
+
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
+
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
# The project name
-PROJECT(wolf)
-
-#string(COMPARE EQUAL "${CMAKE_BINARY_DIR}" "" result)
-#IF(result)
-# SET(CMAKE_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR})
-#ENDIF()
-#message(STATUS "Binary path : " ${CMAKE_BINARY_DIR})
-#
-
-#SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-#SET(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-#
-# We'll set the install prefix only is it's empty
-# which shouldn't be the case ...
-#string(COMPARE EQUAL "${CMAKE_INSTALL_PREFIX}" "" result)
-#IF(result)
- # This path is actually default on linux
-# SET(CMAKE_INSTALL_PREFIX /usr/local)
-#ENDIF()
-#message(STATUS "Installation path : " ${CMAKE_INSTALL_PREFIX})
+PROJECT(core)
+set(PLUGIN_NAME "wolf${PROJECT_NAME}")
+# Paths
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
+SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE)
+# Build type
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "DEBUG")
ENDIF (NOT CMAKE_BUILD_TYPE)
@@ -64,13 +53,24 @@ if(UNIX)
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
+# Options
+IF(NOT BUILD_TESTS)
+ OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(NOT BUILD_DEMOS)
+ OPTION(BUILD_DEMOS "Build Demos" ON)
+ENDIF(NOT BUILD_DEMOS)
+
+IF(NOT BUILD_DOC)
+ OPTION(BUILD_DOC "Build Documentation" OFF)
+ENDIF(NOT BUILD_DOC)
+
+IF(PROFILING)
+ add_definitions(-DPROFILING)
+ message("Compiling with profiling options...")
+ENDIF(PROFILING)
-#OPTION(BUILD_DOC "Build Documentation" OFF)
-OPTION(BUILD_TESTS "Build Unit tests" ON)
-#############
-## Testing ##
-#############
-#
if(BUILD_TESTS)
# Enables testing for this directory and below.
# Note that ctest expects to find a test file in the build directory root.
@@ -79,93 +79,41 @@ if(BUILD_TESTS)
enable_testing()
endif()
-#+START_SRC --------------------------------------------------------------------------------------------------------------------------------
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
set(_WOLF_DEBUG true)
ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-# option(BUILD_EXAMPLES "Build examples" OFF)
-set(BUILD_TESTS true)
-set(BUILD_EXAMPLES false)
-
# Does this has any other interest
# but for the examples ?
# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
+IF(BUILD_DEMOS OR BUILD_TESTS)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
+ENDIF(BUILD_DEMOS OR BUILD_TESTS)
-#find dependencies.
+#START_SRC --------------------------------------------------------------------------------------------------------------------------------
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
+#CMAKE modules
-FIND_PACKAGE(Threads REQUIRED)
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+MESSAGE(STATUS "Cmake modules at: " ${CMAKE_MODULE_PATH})
-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)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
+#find dependencies.
+FIND_PACKAGE(Threads REQUIRED)
+
+FIND_PACKAGE(Ceres REQUIRED) #Ceres is required
+
+FIND_PACKAGE(Eigen3 3.3 REQUIRED)
+if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
+ message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
+endif()
# 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)
+FIND_PACKAGE(YamlCpp REQUIRED)
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal)
@@ -181,68 +129,54 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+message(STATUS "WOLF CONFIG DIRECTORY ${WOLF_CONFIG_DIR}")
+message(STATUS "WOLF CONFIG FILE ${WOLF_CONFIG_DIR}/config.h")
include_directories("${PROJECT_BINARY_DIR}/conf")
+
# include spdlog (logging library)
FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
IF (SPDLOG_INCLUDE_DIR)
- INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
+ # 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})
-include_directories("include")
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_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)
+# Includes
+INCLUDE_DIRECTORIES("include") # In this same project
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
#HEADERS
-
SET(HDRS_COMMON
include/core/common/factory.h
include/core/common/node_base.h
include/core/common/time_stamp.h
include/core/common/wolf.h
+ include/core/common/params_base.h
)
SET(HDRS_MATH
+ include/core/math/SE2.h
include/core/math/SE3.h
include/core/math/rotations.h
+ include/core/math/covariance.h
)
SET(HDRS_UTILS
+ include/core/utils/check_log.h
+ include/core/utils/converter.h
include/core/utils/eigen_assert.h
include/core/utils/eigen_predicates.h
+ include/core/utils/graph_search.h
+ include/core/utils/loader.h
include/core/utils/logging.h
include/core/utils/make_unique.h
+ include/core/utils/params_server.h
include/core/utils/singleton.h
- include/core/utils/params_server.hpp
- include/core/utils/converter.h
- include/core/utils/loader.hpp
+ include/core/utils/utils_gtest.h
+ include/core/utils/converter_utils.h
)
SET(HDRS_PROBLEM
include/core/problem/problem.h
@@ -260,47 +194,50 @@ SET(HDRS_FRAME
include/core/frame/frame_base.h
)
SET(HDRS_STATE_BLOCK
+ include/core/state_block/factory_state_block.h
+ include/core/state_block/has_state_blocks.h
include/core/state_block/local_parametrization_angle.h
include/core/state_block/local_parametrization_base.h
include/core/state_block/local_parametrization_homogeneous.h
include/core/state_block/local_parametrization_quaternion.h
include/core/state_block/state_angle.h
include/core/state_block/state_block.h
- include/core/state_block/state_homogeneous_3D.h
+ include/core/state_block/state_composite.h
+ include/core/state_block/state_homogeneous_3d.h
include/core/state_block/state_quaternion.h
)
SET(HDRS_CAPTURE
include/core/capture/capture_base.h
- include/core/capture/capture_buffer.h
include/core/capture/capture_motion.h
- include/core/capture/capture_odom_2D.h
- include/core/capture/capture_odom_3D.h
+ include/core/capture/capture_odom_2d.h
+ include/core/capture/capture_odom_3d.h
include/core/capture/capture_pose.h
- include/core/capture/capture_velocity.h
include/core/capture/capture_void.h
- include/core/capture/capture_wheel_joint_position.h
+ include/core/capture/capture_diff_drive.h
)
SET(HDRS_FACTOR
include/core/factor/factor_analytic.h
include/core/factor/factor_autodiff.h
- include/core/factor/factor_autodiff_distance_3D.h
include/core/factor/factor_base.h
include/core/factor/factor_block_absolute.h
+ include/core/factor/factor_block_difference.h
include/core/factor/factor_diff_drive.h
- include/core/factor/factor_odom_2D.h
- include/core/factor/factor_odom_2D_analytic.h
- include/core/factor/factor_odom_3D.h
- include/core/factor/factor_pose_2D.h
- include/core/factor/factor_pose_3D.h
+ include/core/factor/factor_distance_3d.h
+ include/core/factor/factor_pose_2d.h
+ include/core/factor/factor_pose_3d.h
include/core/factor/factor_quaternion_absolute.h
- include/core/factor/factor_relative_2D_analytic.h
+ include/core/factor/factor_relative_pose_2d.h
+ include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+ include/core/factor/factor_relative_pose_3d.h
+ include/core/factor/factor_pose_3d_with_extrinsics.h
+ include/core/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
- include/core/feature/feature_diff_drive.h
include/core/feature/feature_match.h
- include/core/feature/feature_odom_2D.h
+ include/core/feature/feature_motion.h
+ include/core/feature/feature_odom_2d.h
include/core/feature/feature_pose.h
)
SET(HDRS_LANDMARK
@@ -308,49 +245,45 @@ SET(HDRS_LANDMARK
include/core/landmark/landmark_match.h
)
SET(HDRS_PROCESSOR
- include/core/processor/autoconf_processor_factory.h
- include/core/processor/diff_drive_tools.h
- include/core/processor/diff_drive_tools.hpp
+ include/core/processor/is_motion.h
include/core/processor/motion_buffer.h
include/core/processor/processor_base.h
- include/core/processor/processor_capture_holder.h
include/core/processor/processor_diff_drive.h
- include/core/processor/processor_factory.h
- include/core/processor/processor_frame_nearest_neighbor_filter.h
+ include/core/processor/processor_fix_wing_model.h
+ include/core/processor/factory_processor.h
include/core/processor/processor_logging.h
- include/core/processor/processor_loopclosure_base.h
+ include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
- include/core/processor/processor_odom_2D.h
- include/core/processor/processor_odom_3D.h
+ include/core/processor/processor_odom_2d.h
+ include/core/processor/processor_odom_3d.h
+ include/core/processor/processor_pose.h
include/core/processor/processor_tracker.h
include/core/processor/processor_tracker_feature.h
- include/core/processor/processor_tracker_feature_dummy.h
include/core/processor/processor_tracker_landmark.h
- # include/core/processor/processor_tracker_landmark_dummy.h
include/core/processor/track_matrix.h
)
SET(HDRS_SENSOR
- include/core/sensor/autoconf_sensor_factory.h
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
- include/core/sensor/sensor_factory.h
- include/core/sensor/sensor_odom_2D.h
- include/core/sensor/sensor_odom_3D.h
+ include/core/sensor/factory_sensor.h
+ include/core/sensor/sensor_model.h
+ include/core/sensor/sensor_odom_2d.h
+ include/core/sensor/sensor_odom_3d.h
+ include/core/sensor/sensor_pose.h
)
SET(HDRS_SOLVER
include/core/solver/solver_manager.h
+ include/core/solver/factory_solver.h
)
-
-SET(HDRS_DTASSC
- include/core/association/association_node.h
- include/core/association/association_solver.h
- include/core/association/association_tree.h
- include/core/association/matrix.h
- include/core/association/association_nnls.h
+SET(HDRS_TREE_MANAGER
+ include/core/tree_manager/factory_tree_manager.h
+ include/core/tree_manager/tree_manager_base.h
+ include/core/tree_manager/tree_manager_sliding_window.h
+ include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
)
-
SET(HDRS_YAML
- include/core/yaml/parser_yaml.hpp
+ include/core/yaml/parser_yaml.h
+ include/core/yaml/yaml_conversion.h
)
#SOURCES
SET(SRCS_PROBLEM
@@ -369,10 +302,12 @@ SET(SRCS_FRAME
src/frame/frame_base.cpp
)
SET(SRCS_STATE_BLOCK
+ src/state_block/has_state_blocks.cpp
src/state_block/local_parametrization_base.cpp
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp
+ src/state_block/state_composite.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
@@ -381,17 +316,21 @@ SET(SRCS_COMMON
SET(SRCS_MATH
)
SET(SRCS_UTILS
+ src/utils/check_log.cpp
+ src/utils/converter_utils.cpp
+ src/utils/graph_search.cpp
+ src/utils/loader.cpp
+ src/utils/params_server.cpp
)
SET(SRCS_CAPTURE
src/capture/capture_base.cpp
src/capture/capture_motion.cpp
- src/capture/capture_odom_2D.cpp
- src/capture/capture_odom_3D.cpp
+ src/capture/capture_odom_2d.cpp
+ src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
- src/capture/capture_velocity.cpp
src/capture/capture_void.cpp
- src/capture/capture_wheel_joint_position.cpp
+ src/capture/capture_diff_drive.cpp
)
SET(SRCS_FACTOR
src/factor/factor_analytic.cpp
@@ -399,62 +338,67 @@ SET(SRCS_FACTOR
)
SET(SRCS_FEATURE
src/feature/feature_base.cpp
- src/feature/feature_diff_drive.cpp
- src/feature/feature_odom_2D.cpp
+ src/feature/feature_motion.cpp
+ src/feature/feature_odom_2d.cpp
src/feature/feature_pose.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
)
SET(SRCS_PROCESSOR
+ src/processor/is_motion.cpp
src/processor/motion_buffer.cpp
src/processor/processor_base.cpp
- src/processor/processor_capture_holder.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_frame_nearest_neighbor_filter.cpp
- src/processor/processor_loopclosure_base.cpp
+ src/processor/processor_fix_wing_model.cpp
+ src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
- src/processor/processor_odom_2D.cpp
- src/processor/processor_odom_3D.cpp
+ src/processor/processor_odom_2d.cpp
+ src/processor/processor_odom_3d.cpp
+ src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
- src/processor/processor_tracker_feature_dummy.cpp
src/processor/processor_tracker_landmark.cpp
- # src/processor/processor_tracker_landmark_dummy.cpp
src/processor/track_matrix.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
- src/sensor/sensor_odom_2D.cpp
- src/sensor/sensor_odom_3D.cpp
- )
-SET(SRCS_DTASSC
- src/association/association_nnls.cpp
- src/association/association_node.cpp
- src/association/association_solver.cpp
- src/association/association_tree.cpp
+ src/sensor/sensor_model.cpp
+ src/sensor/sensor_odom_2d.cpp
+ src/sensor/sensor_odom_3d.cpp
+ src/sensor/sensor_pose.cpp
)
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
+SET(SRCS_TREE_MANAGER
+ src/tree_manager/tree_manager_sliding_window.cpp
+ src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+ )
SET(SRCS_YAML
+ src/yaml/parser_yaml.cpp
+ src/yaml/processor_odom_3d_yaml.cpp
+ src/yaml/sensor_odom_2d_yaml.cpp
+ src/yaml/sensor_odom_3d_yaml.cpp
+ src/yaml/sensor_pose_yaml.cpp
)
#OPTIONALS
#optional HDRS and SRCS
IF (Ceres_FOUND)
SET(HDRS_WRAPPER
#ceres_wrapper/qr_manager.h
- include/core/ceres_wrapper/ceres_manager.h
include/core/ceres_wrapper/cost_function_wrapper.h
include/core/ceres_wrapper/create_numeric_diff_cost_function.h
include/core/ceres_wrapper/local_parametrization_wrapper.h
+ include/core/ceres_wrapper/iteration_update_callback.h
+ include/core/ceres_wrapper/solver_ceres.h
include/core/solver/solver_manager.h
include/core/solver_suitesparse/sparse_utils.h
)
SET(SRCS_WRAPPER
#ceres_wrapper/qr_manager.cpp
- src/ceres_wrapper/ceres_manager.cpp
+ src/ceres_wrapper/solver_ceres.cpp
src/ceres_wrapper/local_parametrization_wrapper.cpp
src/solver/solver_manager.cpp
)
@@ -463,9 +407,6 @@ ELSE(Ceres_FOUND)
SET(SRCS_WRAPPER)
ENDIF(Ceres_FOUND)
-#SUBDIRECTORIES
-add_subdirectory(hello_wolf)
-add_subdirectory(hello_plugin)
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
@@ -474,27 +415,14 @@ 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_YAML ${HDRS_YAML}
- include/core/yaml/yaml_conversion.h
- )
- # sources
- SET(SRCS_YAML ${SRCS_YAML}
- src/yaml/processor_odom_3D_yaml.cpp
- src/yaml/sensor_odom_3D_yaml.cpp
- )
-ENDIF(YAMLCPP_FOUND)
# create the shared library
-ADD_LIBRARY(${PROJECT_NAME}
+ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS}
${SRCS_BASE}
${SRCS_CAPTURE}
${SRCS_COMMON}
- ${SRCS_DTASSC}
${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_FRAME}
@@ -508,114 +436,120 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
${SRCS_TRAJECTORY}
+ ${SRCS_TREE_MANAGER}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML}
)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
+
+# Set compiler options
+# ====================
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ message(STATUS "Using C++ compiler clang")
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
+ # using Clang
+elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+ message(STATUS "Using C++ compiler gnu")
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
+ # using GCC
+endif()
#Link the created libraries
#=============================================================
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
ENDIF(Ceres_FOUND)
+IF(BUILD_TESTS)
+ MESSAGE(STATUS "Will build tests.")
+ add_subdirectory(test)
+ENDIF(BUILD_TESTS)
-
-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)
-#check if this is done correctly
-
-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 demos.")
+IF(BUILD_DEMOS)
+ #Build demos
+ MESSAGE(STATUS "Will build demos.")
ADD_SUBDIRECTORY(demos)
-ENDIF(BUILD_EXAMPLES)
+ENDIF(BUILD_DEMOS)
#install library
#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
+INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iri-algorithms
ARCHIVE DESTINATION lib/iri-algorithms)
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
-INSTALL(FILES ${HDRS_MATH}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
-INSTALL(FILES ${HDRS_UTILS}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
-INSTALL(FILES ${HDRS_PROBLEM}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
-INSTALL(FILES ${HDRS_HARDWARE}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
-INSTALL(FILES ${HDRS_TRAJECTORY}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
-INSTALL(FILES ${HDRS_MAP}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
-INSTALL(FILES ${HDRS_FRAME}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
-INSTALL(FILES ${HDRS_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
-INSTALL(FILES ${HDRS_COMMON}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
-INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
+INSTALL(FILES ${HDRS_COMMON}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+INSTALL(FILES ${HDRS_FRAME}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
+INSTALL(FILES ${HDRS_HARDWARE}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+INSTALL(FILES ${HDRS_MAP}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
+INSTALL(FILES ${HDRS_MATH}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
+INSTALL(FILES ${HDRS_PROBLEM}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
+INSTALL(FILES ${HDRS_PROCESSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+INSTALL(FILES ${HDRS_SENSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
INSTALL(FILES ${HDRS_SERIALIZATION}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/serialization)
+INSTALL(FILES ${HDRS_SOLVER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
+INSTALL(FILES ${HDRS_TRAJECTORY}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
+INSTALL(FILES ${HDRS_TREE_MANAGER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
+INSTALL(FILES ${HDRS_UTILS}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
+INSTALL(FILES ${HDRS_WRAPPER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
+FILE(WRITE ${PLUGIN_NAME}.found "")
+INSTALL(FILES ${PLUGIN_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_core)
#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
+configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake"
+ "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" @ONLY)
+
+configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
+ "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY)
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
+INSTALL(FILES "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
+INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-export(PACKAGE ${PROJECT_NAME})
+export(PACKAGE ${PLUGIN_NAME})
#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
+
FIND_PACKAGE(Doxygen)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
@@ -658,17 +592,4 @@ ELSE(UNIX)
)
ENDIF(UNIX)
-IF (UNIX)
- SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
- SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
- SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
- SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
- SET(CPACK_GENERATOR "DEB")
- SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu")
- SET(CPACK_SET_DESTDIR "ON") # Necessary because of the absolute install paths
- INCLUDE(CPack)
-ELSE(UNIX)
- ADD_CUSTOM_COMMAND(
- COMMENT "packaging only implemented in unix"
- TARGET uninstall)
-ENDIF(UNIX)
+
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..281d399f13dfd23087acc56303dd38d68162587c
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,619 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+ The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works. By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users. We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors. You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+ To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights. Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received. You must make sure that they, too, receive
+or can get the source code. And you must show them these terms so they
+know their rights.
+
+ Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+ For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software. For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+ Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so. This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software. The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable. Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products. If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+ Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary. To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ TERMS AND CONDITIONS
+
+ 0. Definitions.
+
+ "This License" refers to version 3 of the GNU General Public License.
+
+ "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+ "The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+ To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy. The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+ A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+ To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+ To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+ An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+ 1. Source Code.
+
+ The "source code" for a work means the preferred form of the work
+for making modifications to it. "Object code" means any non-source
+form of a work.
+
+ A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+ The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+ The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+ The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+ The Corresponding Source for a work in source code form is that
+same work.
+
+ 2. Basic Permissions.
+
+ All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+ You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force. You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+ Conveying under any other circumstances is permitted solely under
+the conditions stated below. Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+ No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+ When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+ 4. Conveying Verbatim Copies.
+
+ You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+ You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+ 5. Conveying Modified Source Versions.
+
+ You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified
+ it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is
+ released under this License and any conditions added under section
+ 7. This requirement modifies the requirement in section 4 to
+ "keep intact all notices".
+
+ c) You must license the entire work, as a whole, under this
+ License to anyone who comes into possession of a copy. This
+ License will therefore apply, along with any applicable section 7
+ additional terms, to the whole of the work, and all its parts,
+ regardless of how they are packaged. This License gives no
+ permission to license the work in any other way, but it does not
+ invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display
+ Appropriate Legal Notices; however, if the Program has interactive
+ interfaces that do not display Appropriate Legal Notices, your
+ work need not make them do so.
+
+ A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit. Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+ 6. Conveying Non-Source Forms.
+
+ You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by the
+ Corresponding Source fixed on a durable physical medium
+ customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by a
+ written offer, valid for at least three years and valid for as
+ long as you offer spare parts or customer support for that product
+ model, to give anyone who possesses the object code either (1) a
+ copy of the Corresponding Source for all the software in the
+ product that is covered by this License, on a durable physical
+ medium customarily used for software interchange, for a price no
+ more than your reasonable cost of physically performing this
+ conveying of source, or (2) access to copy the
+ Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the
+ written offer to provide the Corresponding Source. This
+ alternative is allowed only occasionally and noncommercially, and
+ only if you received the object code with such an offer, in accord
+ with subsection 6b.
+
+ d) Convey the object code by offering access from a designated
+ place (gratis or for a charge), and offer equivalent access to the
+ Corresponding Source in the same way through the same place at no
+ further charge. You need not require recipients to copy the
+ Corresponding Source along with the object code. If the place to
+ copy the object code is a network server, the Corresponding Source
+ may be on a different server (operated by you or a third party)
+ that supports equivalent copying facilities, provided you maintain
+ clear directions next to the object code saying where to find the
+ Corresponding Source. Regardless of what server hosts the
+ Corresponding Source, you remain obligated to ensure that it is
+ available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided
+ you inform other peers where the object code and Corresponding
+ Source of the work are being offered to the general public at no
+ charge under subsection 6d.
+
+ A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+ A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling. In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage. For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product. A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+ "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source. The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+ If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+ The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed. Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+ Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+ 7. Additional Terms.
+
+ "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law. If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+ When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+ Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the
+ terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or
+ author attributions in that material or in the Appropriate Legal
+ Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or
+ requiring that modified versions of such material be marked in
+ reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or
+ authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some
+ trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that
+ material by anyone who conveys the material (or modified versions of
+ it) with contractual assumptions of liability to the recipient, for
+ any liability that these contractual assumptions directly impose on
+ those licensors and authors.
+
+ All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10. If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term. If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+ If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+ Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+ 8. Termination.
+
+ You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+ However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+ Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+ Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+ 9. Acceptance Not Required for Having Copies.
+
+ You are not required to accept this License in order to receive or
+run a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+ 10. Automatic Licensing of Downstream Recipients.
+
+ Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+ An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+ You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+ 11. Patents.
+
+ A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+ A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+ Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+ In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+ If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+ If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+ A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License. You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+ Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+ 12. No Surrender of Others' Freedom.
+
+ If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all. For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+ 13. Use with the GNU Affero General Public License.
+
+ Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work. The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+ 14. Revised Versions of this License.
+
+ The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation. If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+ If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+ Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+ 15. Disclaimer of Warranty.
+
+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. Limitation of Liability.
+
+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+ 17. Interpretation of Sections 15 and 16.
+
+ If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
diff --git a/PluginsInfo.md b/PluginsInfo.md
index 822109172503bf13012fb4a5b333459f7a1b8e78..21134bbce0d5b9b06d218b7176131165bc89a356 100644
--- a/PluginsInfo.md
+++ b/PluginsInfo.md
@@ -5,8 +5,8 @@
## Installing wolf(core)
```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/core.git
-cd core
+git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+cd wolf
mkdir build & cd build
cmake ..
make
@@ -32,8 +32,8 @@ If you want to use the core, you just need to have it installed and in your CMak
`find_package(wolf REQUIRED)`.
If wolf is indeed installed, this will define two variables
-`${wolf_INCLUDE_DIR}` which will contain the path to the wolf include directory
-and `${wolf_LIBRARY}` which will contain the path to the wolf library.
+`${wolf_INCLUDE_DIRS}` which will contain the path to the wolf include directory
+and `${wolf_LIBRARIES}` which will contain the path to the wolf library.
## Using wolf plugins
@@ -41,13 +41,13 @@ If you also want to use some wolf plugin, you just follow the same procedure, ch
`find_package(wolf<plugin name> REQUIRED)`.
If the pluging is indeed installed, this will define two variables
-`${wolf<plugin name>_INCLUDE_DIR}` which will contain the path to the plugin's include directory
-and `${wolf<plugin name>_LIBRARY}` which will contain the path to the plugin's library.
+`${wolf<plugin name>_INCLUDE_DIRS}` which will contain the path to the plugin's include directory
+and `${wolf<plugin name>_LIBRARIES}` which will contain the path to the plugin's library.
-As an example, suppose that we want to use the _vision_ plugin. First, we will clone and install it
+As an example, suppose that we want to use the _laser_ plugin. First, we will clone and install it
```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git
-cd vision
+git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/laser.git
+cd laser
mkdir build && cd build
cmake ..
make
@@ -56,15 +56,12 @@ sudo make install
Then, in the CMakeLists.txt of the application we are developing we will put the following line
```
-find_package(wolfvision REQUIRED)
+find_package(wolflaser REQUIRED)
```
if the plugin has been correctly installed, and thus find_package succeeds, then it will define two variables
-- ${wolfvision_INCLUDE_DIR} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_vision`
-- ${wolfvision_LIBRARY} which is the path to the compiled (& linked) library. It should have the value `/usr/local/lib/iri-algorithms/libwolfvision.so`
-
-**IMPORTANT NOTE** For the time being, each plugin is only responsible for finding their own includes and library. This means that for instance wolfvision, which obviously requires
-the _core_ plugin, will not find its own dependencies. It is the responsibility of the programmer to do `find_package(wolf REQUIRED)` to get the includes and library. In the future this will change and each plugin will be responsible for finding all the necessary dependencies.
+- ${wolflaser_INCLUDE_DIRS} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_laser;/usr/local/include/iri-algorithms`
+- ${wolfvision_LIBRARIES} which is the path to the required libraries. It should have the value `/usr/local/lib/iri-algorithms/libwolflaser.so;/usr/local/lib/iri-algorithms/liblaser_scan_utils.so`
# Creating your plugin
@@ -79,6 +76,17 @@ git init
or directly fork*(?)* the repository.
# Contributing
+
## Contributing your plugin to wolf
+Will we have some kind of repository keeping track of wolf plugins?
+
## Contributing to existing plugins
-## Contributing to wolf core
\ No newline at end of file
+???
+
+## Contributing to wolf core
+Steps:
+
+* Let us know what is it that you want to contribute. It is very important that you communicate with us. Obviously noone is stopping you
+from directly forking the repository, but you run the risk of having your pull request declined if we haven't discussed your contribution beforehand.
+* If it is something we are already working on we can maybe integrate you into the working group.
+* Otherwise, fork the repository and pull request when your contribution is ready.
\ No newline at end of file
diff --git a/README.md b/README.md
index 69055e1fda60d7623404761607c6bc5f531c7b80..c1e960469ccdcaf2b9918905b7e10536c9e9af59 100644
--- a/README.md
+++ b/README.md
@@ -1,381 +1,4 @@
WOLF - Windowed Localization Frames
===================================
-Overview
---------
-
-Wolf is a library to solve localization problems in mobile robotics, such as SLAM, map-based localization, or visual odometry. The approach contemplates the coexistence of multiple sensors of different kind, be them synchronized or not. It is thought to build state vectors formed by a set of key-frames (window), defining the robot trajectory, plus other states such as landmarks or sensor parameters for calibration, and compute error vectors given the available measurements in that window.
-
-Wolf is mainly a structure for having the data accessible and organized, plus some functionality for managing this data. It requires, on one side, several front-ends (one per sensor, or per sensor type), and a back-end constituted by the solver.
-
-Wolf may be interfaced with many kinds of solvers, including filters and nonlinear optimizers. It can be used with EKF, error-state KF, iterative EKF, and other kinds of filters such as information filters. It can also be used with nonlinear optimizers, most especially -- though not necessarily -- in their sparse flavors, and in particular the incremental ones.
-
-The task of interfacing Wolf with these solvers is relegated to wrappers, which are coded out of Wolf. We provide a wrapper to Google CERES. We also provide an experimental wrapper-less solver for incremental, sparse, nonlinear optimization based on the QR decomposition (implementing the iSAM algorithm).
-
-The basic Wolf structure is a tree of base classes reproducing the elements of the robotic problem. This is called the Wolf Tree. It has a robot, with sensors, with a trajectory formed by keyframes, and the map with its landmarks. These base classes can be derived to build the particularizations you want. You have the basic functionality in the base classes, and you add what you want on top. The Wolf Tree connectivity is augmented with the constraints linking different parts of it, becoming a real network of relations. This network is equivalent to the factor graph that would be solved by graphical models and nonlinear optimization. Wrappers are the ones transferring the Wolf structure into a factor graph that can be provided to the solver. See the documentation for a proper rationale and all the details.
-
-Wolf can be used within ROS for an easy integration. We provide examples of ROS nodes using Wolf. Wolf can also be used in other robotics frameworks.
-
-#### Features
-
-- Keyframe based
-- Multi-sensor
-- Pose-SLAM, landmark-based SLAM, or visual odometry
-- Different state types -- the state blocks
-- Different measurement models -- the constraint blocks
-- Built with polymorphic classes using virtual inheritance and templates.
-- Solver agnostic: choose your solver and build your wrapper to Wolf.
-
-#### Some preliminary documentation
-
-- You can visit this [Wolf inspiring document](https://docs.google.com/document/d/1_kBtvCIo33pdP59M3Ib4iEBleDDLcN6yCbmwJDBLtcA). Contact [Joan](mailto:jsola@iri.upc.edu) if you need permissions for the link.
-- You can also have a look at the [Wolf tree](https://docs.google.com/drawings/d/1jj5VVjQThddswpTPMLG2xv87vtT3o1jiMJo3Mk1Utjg), showing the organization of the main elements in the Wolf project. Contact [Andreu](mailto:acorominas@iri.upc.edu) if you need permissions for the link.
-- You can finally visit this [other inspiring document](https://docs.google.com/document/d/18XQlgdfTwplakYKKsfw2YAoaVuSyUEQoaU6bYHQDNpU) providing the initial motivation for the Wolf project. Contact [Joan](mailto:jsola@iri.upc.edu) if you need permissions for the link.
-
-Dependencies
-------------
-
-! Please notice that we are detailing two installation procedures below. If you are familiar with `ROS` and more especially the [`catkin_tools`](https://catkin-tools.readthedocs.io/en/latest/index.html) package then you may jump directly to the 'Using the `catkin_tools` package' section.
-
-#### CMake
-Building tool used by Wolf and by some of its dependencies. In order to install *cmake* please follow the instructions at [cmake site](https://cmake.org/install/)
-
-#### Autoreconf
-
- $ sudo apt install dh-autoreconf
-
-#### Eigen
-
-[Eigen](http://eigen.tuxfamily.org). Linear algebra, header library. Eigen 3.2 is also a depencency of ROS-Hydro. In case you don't have ROS in your machine, you can install Eigen by typing:
-
- $ sudo apt-get install libeigen3-dev
-
-#### Ceres (5 steps)
-
-[Ceres](http://www.ceres-solver.org/) is an optimization library. Currently, this dependency is optional, so the build procedure of Wolf skips part of compilation in case this dependency is not found on the system. **Installation** is described at [Ceres site](http://www.ceres-solver.org/building.html). However we report here an alternative step by step procedure to install Ceres.
-
-**(1)** Skip this step if Cmake 2.8.0+ and Eigen3.0+ are already installed. Otherwise install them with *apt-get*.
-
-**(2) GFLAGS**
-
-- Git clone the source:
-
- $ git clone https://github.com/gflags/gflags.git
-
-- Build and install with:
-
- $ cd gflags
- $ mkdir build
- $ cd build
- $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DGFLAGS_NAMESPACE="google" ..
- $ make
- $ sudo make install
-
-libgflags.a will be installed at **/usr/local/lib**
-
-**(3) GLOG**
-
-- Git clone the source:
-
- $ git clone https://github.com/google/glog.git
-
-- Build and install with:
-
- $ cd glog
- $ ./autogen.sh
- $ ./configure --with-gflags=/usr/local/
- $ make
- $ sudo make install
-
-libglog.so will be installed at **/usr/local/lib**
-
-- Troubleshooting:
-
- * If ./autogen.sh fails with './autogen.sh: autoreconf: not found'
-
- In a fresh installation you will probably need to install autoreconf running
-
- $ sudo apt-get install dh-autoreconf
-
- * If `make` command fails with the error: `/bin/bash: aclocal-1.14: command not found`
-
- Install Glog with the following commands:
-
- $ cd glog
- $ sudo apt-get install autoconf
- $ autoreconf --force --install
- $ ./configure --with-gflags=/usr/local/
- $ make
- $ sudo make install
-
-**(4) SUITESPARSE**
-
-- Easy way!:
-
- $ sudo apt-get install libsuitesparse-dev
-
-**(5) CERES**
-
-- Git clone the source:
-
- $ git clone https://ceres-solver.googlesource.com/ceres-solver
-
-- Build and install with:
-
- $ cd ceres-solver
- $ mkdir build
- $ cd build
- $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..
- $ make
- $ sudo make install
-
-libceres.a will be installed at **/usr/local/lib** and headers at **/usr/local/include/ceres**
-
-#### Yaml-cpp
-
-Wolf uses YAML files for configuration and for saving and loading workspaces. To obtain it run:
-
-- Ubuntu:
-
- $ sudo apt-get install libyaml-cpp-dev
-
-- Mac:
-
- $ brew install yaml-cpp
-
-We are shipping the CMAKE file `FindYamlCpp.cmake` together with Wolf. Find it at `[wolf]/cmake_modules/FindYamlCpp.cmake`
-
-#### spdlog
-
-Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0.17.0. To install it:
-
-- Git clone the source:
-
- $ git clone https://github.com/gabime/spdlog.git
-
-- Build and install with:
-
- $ cd spdlog
- $ git checkout v0.17.0
- $ mkdir build
- $ cd build
- $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..
- $ make
- $ sudo make install
-
-#### Optional: Vision Utils (Install only if you want to use IRI's vision utils)
-
-This library requires OpenCV. If it is not installed in your system or you are unsure, please follow the installation steps at https://gitlab.iri.upc.edu/mobile_robotics/vision_utils
-
-**(1)** Git clone the source:
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/vision_utils.git
-
-**(2)** Build and install:
-
- $ cd vision_utils/build
- $ cmake ..
- $ make
- $ sudo make install
-
-**(3)** Optionally run tests
-
- $ ctest
-
-#### Optional: Laser Scan Utils (Install only if you want to use IRI's laser scan utils)
-
-**(1)** Git clone the source:
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/laser_scan_utils.git
-
-**(2)** Build and install:
-
- $ cd laser_scan_utils
- $ mkdir build && cd build
- $ cmake ..
- $ make
- $ sudo make install
-
-#### Optional: Raw GPS Utils (Install only if you want to use IRI's raw gps utils)
-
-**(1)** Git clone the source:
-
- $ git clone https://github.com/pt07/raw_gps_utils.git
-
-**(2)** Build and install:
-
- $ cd raw_gps_utils
- $ mkdir build && cd build
- $ cmake ..
- $ make
- $ sudo make install
-
-Download and build
-------------------
-
-#### Wolf C++ Library
-
-**Clone:**
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git
-
-**Build:**
-
- $ cd wolf
- $ mkdir build
- $ cd build
- $ cmake ..
- $ make
- $ sudo make install //optional in case you want to install wolf library
-
-
-#### Wolf ROS Node
-
-- Git clone the source (inside your ROS workspace):
-
- $ git clone https://github.com/IRI-MobileRobotics/wolf_ros.git
-
-Using the `catkin_tools` package
---------------------------------
-
-**(1)** Install `catkin_tools` :
-
-[`installation webpage.`](https://catkin-tools.readthedocs.io/en/latest/installing.html)
-
-- Installing on Ubuntu with `apt-get`
-
- $ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list'
- $ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- $ sudo apt-get update
- $ sudo apt-get install python-catkin-tools
-
-- Installing with [`pip`](https://pip.pypa.io/en/stable/installing/)
-
- $ sudo pip install -U catkin_tools
-
-**(2)** Create a `catkin workspace` :
-
- $ cd
- $ mkdir -p wolf_ws/src
- $ cd wolf_ws/src
- $ catkin_init_workspace
- $ cd ..
- $ catkin_make
-
-**(3)** Setup your `bash_rc`:
-
-Add at the end of the ~/.bashrc file with the following command:
-
- $ echo "source ~/wolf_ws/devel/setup.bash" >> ~/.bashrc
-
-Source your bash:
-
- source ~/.bashrc
-
-**(3)** Download `Ceres` :
-
-In the previously created directory `~/my_workspace_directory/wolf_ws/src/` clone `Ceres` & `wolf`.
-
- $ git clone https://github.com/artivis/ceres_solver.git
-
-**(4)** Download `wolf` :
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf.git
-
-At this point you might need to switch to the `catkin_build` branch of the wolf project.
-
- $ cd wolf
- $ git checkout catkin_build
-
-(optional) Download `wolf_ros` :
-
- $ git clone https://github.com/IRI-MobileRobotics/Wolf_ros.git
-
-**(5)** Let's Compile !
-
- The command below can be launch from any sub-directory in `~/my_workspace_directory/wolf_ws/`.
-
- $ catkin build
-
-**(6)** Run tests:
-
- $ catkin run_tests
-
-Troubleshooting
----------------
-
-#### Boost
-
-We have made our best to keep being boost-independent.
-However, in case you run into a boost installation issue at execution time, check that you have boost installed.
-If needed, install it with:
-
-[Boost](http://www.boost.org/). Free peer-reviewed portable C++ source libraries.
-
- $ sudo apt-get install libboost-all-dev
-
-
-
-Inspiring Links
----------------
-
-- [Basics on code optimization](http://www.eventhelix.com/realtimemantra/basics/optimizingcandcppcode.htm)
-
-- [Headers, Includes, Forward declarations, ...](http://www.cplusplus.com/forum/articles/10627/)
-
-- Using Eigen quaternion and CERES: [explanation](http://www.lloydhughes.co.za/index.php/using-eigen-quaternions-and-ceres-solver/) & [GitHub CERES extension](https://github.com/system123/ceres_extensions)
-
-Useful tools
-------------
-
-#### Profiling with Valgrind and Kcachegrind
-
-Kcachegrind is a graphical frontend for profiling your program and optimizing your code.
-
-- Ubuntu:
-
-Get the programs with
-
- $ sudo apt-get install valgrind kcachegrind
-
-- Mac OSX
-
-In Mac, you can use qcachegrind instead. To get it through Homebrew, type
-
- $ brew install valgrind qcachegrind
-
-I don't know if these packages are available through MacPorts. Try
-
- $ ports search --name valgrind
- $ ports search --name qcachegrind
-
-If they are available, just do
-
- $ sudo port install valgrind qcachegrind
-
-#### Do the profiling and watch the reports
-
-_Remember:_ For a proper profiling, compile Wolf and related libraries
-in RELEASE mode. Profiling code compiled in DEBUG mode is not going to take you
-anywhere, and in the reports you will mostly see the overhead of the DEBUG mode.
-
-Type in your `wolf/bin/` directory:
-
- $ cd bin/
- $ valgrind --tool=callgrind ./my_program <my_prg_params>
-
-this produces a log report called `callgrind.out.XXXX`, where XXXX is a number. Then type
-
-- Ubuntu
-
- ```shell
- $ kcachegrind callgrind.out.XXXX
- ```
-
-- Mac OSX
-
- ```shell
- $ qcachegrind callgrind.out.XXXX
- ```
-
-and enjoy.
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
\ No newline at end of file
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
index d44ea8903d4c5a72af468d603dc1d8e5a6bbf542..9e969786089ca8ea3801be8b084c51a5782f09b5 100644
--- a/cmake_modules/FindEigen3.cmake
+++ b/cmake_modules/FindEigen3.cmake
@@ -1,263 +1,97 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
+# - Try to find Eigen3 lib
#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
+# This module supports requiring a minimum version, e.g. you can do
+# find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
#
-# * Redistributions of source code must retain the above copyright notice,
-# this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-# this list of conditions and the following disclaimer in the documentation
-# and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-# used to endorse or promote products derived from this software without
-# specific prior written permission.
+# Once done this will define
#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
+# EIGEN3_FOUND - system has eigen lib with correct version
+# EIGEN3_INCLUDE_DIR - the eigen include directory
+# EIGEN3_VERSION - eigen version
#
-# Author: alexs.mac@gmail.com (Alex Stewart)
+# This module reads hints about search locations from
+# the following enviroment variables:
#
+# EIGEN3_ROOT
+# EIGEN3_ROOT_DIR
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+ if(NOT Eigen3_FIND_VERSION_MAJOR)
+ set(Eigen3_FIND_VERSION_MAJOR 2)
+ endif(NOT Eigen3_FIND_VERSION_MAJOR)
+ if(NOT Eigen3_FIND_VERSION_MINOR)
+ set(Eigen3_FIND_VERSION_MINOR 91)
+ endif(NOT Eigen3_FIND_VERSION_MINOR)
+ if(NOT Eigen3_FIND_VERSION_PATCH)
+ set(Eigen3_FIND_VERSION_PATCH 0)
+ endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+ set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+ file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+ set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+ if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK FALSE)
+ else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK TRUE)
+ endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+ if(NOT EIGEN3_VERSION_OK)
+
+ message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+ "but at least version ${Eigen3_FIND_VERSION} is required")
+ endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+ # in cache already
+ _eigen3_check_version()
+ set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+ # search first if an Eigen3Config.cmake is available in the system,
+ # if successful this would set EIGEN3_INCLUDE_DIR and the rest of
+ # the script will work as usual
+ find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)
+
+ if(NOT EIGEN3_INCLUDE_DIR)
+ find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+ HINTS
+ ENV EIGEN3_ROOT
+ ENV EIGEN3_ROOT_DIR
+ PATHS
+ ${CMAKE_INSTALL_PREFIX}/include
+ ${KDE4_INCLUDE_DIR}
+ PATH_SUFFIXES eigen3 eigen
+ )
+ endif(NOT EIGEN3_INCLUDE_DIR)
+
+ if(EIGEN3_INCLUDE_DIR)
+ _eigen3_check_version()
+ endif(EIGEN3_INCLUDE_DIR)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+ mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
-# FindEigen.cmake - Find Eigen library, version >= 3.
-#
-# This module defines the following variables:
-#
-# EIGEN_FOUND: TRUE iff Eigen is found.
-# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
-# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
-# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
-# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
-# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
-# FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION: True iff the version of Eigen
-# found was built & installed /
-# exported as a CMake package.
-#
-# The following variables control the behaviour of this module:
-#
-# EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then
-# then prefer using an exported CMake configuration
-# generated by Eigen over searching for the
-# Eigen components manually. Otherwise (FALSE)
-# ignore any exported Eigen CMake configurations and
-# always perform a manual search for the components.
-# Default: TRUE iff user does not define this variable
-# before we are called, and does NOT specify
-# EIGEN_INCLUDE_DIR_HINTS, otherwise FALSE.
-# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
-# search for eigen includes, e.g: /timbuktu/eigen3.
-#
-# The following variables are also defined by this module, but in line with
-# CMake recommended FindPackage() module style should NOT be referenced directly
-# by callers (use the plural variables detailed above instead). These variables
-# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
-# are NOT re-called (i.e. search for library is not repeated) if these variables
-# are set with valid values _in the CMake cache_. This means that if these
-# variables are set directly in the cache, either by the user in the CMake GUI,
-# or by the user passing -DVAR=VALUE directives to CMake when called (which
-# explicitly defines a cache variable), then they will be used verbatim,
-# bypassing the HINTS variables and other hard-coded search locations.
-#
-# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
-# include directory of any dependencies.
-
-# Called if we failed to find Eigen or any of it's required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
-macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
- unset(EIGEN_FOUND)
- unset(EIGEN_INCLUDE_DIRS)
- unset(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION)
- # Make results of search visible in the CMake GUI if Eigen has not
- # been found so that user does not have to toggle to advanced view.
- mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
- # use the camelcase library name, not uppercase.
- if (Eigen_FIND_QUIETLY)
- message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
- elseif (Eigen_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use no priority which emits a message
- # but continues configuration and allows generation.
- message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(EIGEN_REPORT_NOT_FOUND)
-
-# Protect against any alternative find_package scripts for this library having
-# been called previously (in a client project) which set EIGEN_FOUND, but not
-# the other variables we require / set here which could cause the search logic
-# here to fail.
-unset(EIGEN_FOUND)
-
-# -----------------------------------------------------------------
-# By default, if the user has expressed no preference for using an exported
-# Eigen CMake configuration over performing a search for the installed
-# components, and has not specified any hints for the search locations, then
-# prefer an exported configuration if available.
-if (NOT DEFINED EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION
- AND NOT EIGEN_INCLUDE_DIR_HINTS)
- message(STATUS "No preference for use of exported Eigen CMake configuration "
- "set, and no hints for include directory provided. "
- "Defaulting to preferring an installed/exported Eigen CMake configuration "
- "if available.")
- set(EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION TRUE)
-endif()
-
-if (EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION)
- # Try to find an exported CMake configuration for Eigen.
- #
- # We search twice, s/t we can invert the ordering of precedence used by
- # find_package() for exported package build directories, and installed
- # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7)
- # respectively in [1].
- #
- # By default, exported build directories are (in theory) detected first, and
- # this is usually the case on Windows. However, on OS X & Linux, the install
- # path (/usr/local) is typically present in the PATH environment variable
- # which is checked in item 4) in [1] (i.e. before both of the above, unless
- # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed
- # packages are usually detected in preference to exported package build
- # directories.
- #
- # To ensure a more consistent response across all OSs, and as users usually
- # want to prefer an installed version of a package over a locally built one
- # where both exist (esp. as the exported build directory might be removed
- # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which
- # means any build directories exported by the user are ignored, and thus
- # installed directories are preferred. If this fails to find the package
- # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any
- # exported build directories will now be detected.
- #
- # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which
- # is item 5) in [1]), to not preferentially use projects that were built
- # recently with the CMake GUI to ensure that we always prefer an installed
- # version if available.
- #
- # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package
- find_package(Eigen3 QUIET
- NO_MODULE
- NO_CMAKE_PACKAGE_REGISTRY
- NO_CMAKE_BUILDS_PATH)
- if (EIGEN3_FOUND)
- message(STATUS "Found installed version of Eigen: ${Eigen3_DIR}")
- else()
- # Failed to find an installed version of Eigen, repeat search allowing
- # exported build directories.
- message(STATUS "Failed to find installed Eigen CMake configuration, "
- "searching for Eigen build directories exported with CMake.")
- # Again pass NO_CMAKE_BUILDS_PATH, as we know that Eigen is exported and
- # do not want to treat projects built with the CMake GUI preferentially.
- find_package(Eigen3 QUIET
- NO_MODULE
- NO_CMAKE_BUILDS_PATH)
- if (EIGEN3_FOUND)
- message(STATUS "Found exported Eigen build directory: ${Eigen3_DIR}")
- endif()
- endif()
- if (EIGEN3_FOUND)
- set(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION TRUE)
- set(EIGEN_FOUND ${EIGEN3_FOUND})
- set(EIGEN_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}" CACHE STRING
- "Eigen include directory" FORCE)
- else()
- message(STATUS "Failed to find an installed/exported CMake configuration "
- "for Eigen, will perform search for installed Eigen components.")
- endif()
-endif()
-
-if (NOT EIGEN_FOUND)
- # Search user-installed locations first, so that we prefer user installs
- # to system installs where both exist.
- list(APPEND EIGEN_CHECK_INCLUDE_DIRS
- /usr/local/include
- /usr/local/homebrew/include # Mac OS X
- /opt/local/var/macports/software # Mac OS X.
- /opt/local/include
- /usr/include)
- # Additional suffixes to try appending to each search path.
- list(APPEND EIGEN_CHECK_PATH_SUFFIXES
- eigen3 # Default root directory for Eigen.
- Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3
- Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3
-
- # Search supplied hint directories first if supplied.
- find_path(EIGEN_INCLUDE_DIR
- NAMES Eigen/Core
- HINTS ${EIGEN_INCLUDE_DIR_HINTS}
- PATHS ${EIGEN_CHECK_INCLUDE_DIRS}
- PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
-
- if (NOT EIGEN_INCLUDE_DIR OR
- NOT EXISTS ${EIGEN_INCLUDE_DIR})
- eigen_report_not_found(
- "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
- "path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
- endif (NOT EIGEN_INCLUDE_DIR OR
- NOT EXISTS ${EIGEN_INCLUDE_DIR})
-
- # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
- # if called.
- set(EIGEN_FOUND TRUE)
-endif()
-
-# Extract Eigen version from Eigen/src/Core/util/Macros.h
-if (EIGEN_INCLUDE_DIR)
- set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
- if (NOT EXISTS ${EIGEN_VERSION_FILE})
- eigen_report_not_found(
- "Could not find file: ${EIGEN_VERSION_FILE} "
- "containing version information in Eigen install located at: "
- "${EIGEN_INCLUDE_DIR}.")
- else (NOT EXISTS ${EIGEN_VERSION_FILE})
- file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
-
- string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
- EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
- string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
- EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
-
- string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
- EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
- string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
- EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
-
- string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
- EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
- string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
- EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
-
- # This is on a single line s/t CMake does not interpret it as a list of
- # elements and insert ';' separators which would result in 3.;2.;0 nonsense.
- set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
- endif (NOT EXISTS ${EIGEN_VERSION_FILE})
-endif (EIGEN_INCLUDE_DIR)
-
-# Set standard CMake FindPackage variables if found.
-if (EIGEN_FOUND)
- set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
-endif (EIGEN_FOUND)
-
-# Handle REQUIRED / QUIET optional arguments and version.
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(Eigen
- REQUIRED_VARS EIGEN_INCLUDE_DIRS
- VERSION_VAR EIGEN_VERSION)
-
-# Only mark internal variables as advanced if we found Eigen, otherwise
-# leave it visible in the standard GUI for the user to set manually.
-if (EIGEN_FOUND)
- mark_as_advanced(FORCE EIGEN_INCLUDE_DIR
- Eigen3_DIR) # Autogenerated by find_package(Eigen3)
-endif (EIGEN_FOUND)
\ No newline at end of file
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
deleted file mode 100644
index 54a4b4edf5f01dd03e237eeb97d30178ccb14645..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfConfig.cmake
+++ /dev/null
@@ -1,65 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolf_INCLUDE_DIR
- NAMES wolf.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
-IF(wolf_INCLUDE_DIR)
- MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIR}")
-ELSE(wolf_INCLUDE_DIR)
- MESSAGE("Couldn't find wolf include dirs")
-ENDIF(wolf_INCLUDE_DIR)
-
-FIND_LIBRARY(
- wolf_LIBRARY
- NAMES libwolf.so
- PATHS /usr/local/lib/iri-algorithms)
-IF(wolf_LIBRARY)
- MESSAGE("Found wolf lib: ${wolf_LIBRARY}")
-ELSE(wolf_LIBRARY)
- MESSAGE("Couldn't find wolf lib")
-ENDIF(wolf_LIBRARY)
-
-IF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
- SET(wolf_FOUND TRUE)
- ELSE(wolf_INCLUDE_DIR AND wolf_LIBRARY)
- set(wolf_FOUND FALSE)
-ENDIF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
-
-IF (wolf_FOUND)
- IF (NOT wolf_FIND_QUIETLY)
- MESSAGE(STATUS "Found wolf: ${wolf_LIBRARY}")
- ENDIF (NOT wolf_FIND_QUIETLY)
-ELSE (wolf_FOUND)
- IF (wolf_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find wolf")
- ENDIF (wolf_FIND_REQUIRED)
-ENDIF (wolf_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
- set(wolf_FOUND FALSE)
- unset(wolf_INCLUDE_DIR)
- unset(wolf_LIBRARIES)
-
- # Reset the CMake module path to its state when this script was called.
- set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
- # FindPackage() use the camelcase library name, not uppercase.
- if (wolf_FIND_QUIETLY)
- message(STATUS "Failed to find wolf- " ${REASON_MSG} ${ARGN})
- else (wolf_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
- # that prevents generation, but continues configuration.
- message(SEND_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolf_FOUND)
- wolf_report_not_found("Something went wrong while setting up wolf.")
-endif(NOT wolf_FOUND)
-# Set the include directories for wolf (itself).
-set(wolf_FOUND TRUE)
\ No newline at end of file
diff --git a/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..b4df7e15bab20bc8d1090ffbd5428ac7a3ab5cd6
--- /dev/null
+++ b/cmake_modules/wolfcoreConfig.cmake
@@ -0,0 +1,99 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(
+ wolfcore_INCLUDE_DIRS
+ NAMES wolfcore.found
+ PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
+IF(wolfcore_INCLUDE_DIRS)
+ MESSAGE("Found wolf core include dirs: ${wolfcore_INCLUDE_DIRS}")
+ELSE(wolfcore_INCLUDE_DIRS)
+ MESSAGE("Couldn't find wolf core include dirs")
+ENDIF(wolfcore_INCLUDE_DIRS)
+
+FIND_LIBRARY(
+ wolfcore_LIBRARIES
+ NAMES libwolfcore.so libwolfcore.dylib
+ PATHS /usr/local/lib/iri-algorithms)
+IF(wolfcore_LIBRARIES)
+ MESSAGE("Found wolf core lib: ${wolfcore_LIBRARIES}")
+ELSE(wolfcore_LIBRARIES)
+ MESSAGE("Couldn't find wolf core lib")
+ENDIF(wolfcore_LIBRARIES)
+
+IF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+ SET(wolfcore_FOUND TRUE)
+ ELSE(wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+ set(wolfcore_FOUND FALSE)
+ENDIF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+
+IF (wolfcore_FOUND)
+ IF (NOT wolfcore_FIND_QUIETLY)
+ MESSAGE(STATUS "Found wolf core: ${wolfcore_LIBRARIES}")
+ ENDIF (NOT wolfcore_FIND_QUIETLY)
+ELSE (wolfcore_FOUND)
+ IF (wolfcore_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find wolf core")
+ ENDIF (wolfcore_FIND_REQUIRED)
+ENDIF (wolfcore_FOUND)
+
+
+macro(wolfcore_report_not_found REASON_MSG)
+ set(wolfcore_FOUND FALSE)
+ unset(wolfcore_INCLUDE_DIRS)
+ unset(wolfcore_LIBRARIES)
+
+ # Reset the CMake module path to its state when this script was called.
+ set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+ # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+ # FindPackage() use the camelcase library name, not uppercase.
+ if (wolfcore_FIND_QUIETLY)
+ message(STATUS "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
+ elseif (wolfcore_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
+ else()
+ # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+ # that prevents generation, but continues configuration.
+ message(SEND_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
+ endif ()
+ return()
+endmacro(wolfcore_report_not_found)
+
+if(NOT wolfcore_FOUND)
+ wolfcore_report_not_found("Something went wrong while setting up wolf.")
+else (NOT wolfcore_FOUND)
+ # Set the include directories for wolf core (itself).
+ set(wolfcore_FOUND TRUE)
+
+ # Now we gather all the required dependencies for Wolf
+
+ get_filename_component(WOLFCORE_CURRENT_CONFIG_DIR
+ "${CMAKE_CURRENT_LIST_FILE}" PATH)
+
+ SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH})
+ LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLFCORE_CURRENT_CONFIG_DIR})
+
+ FIND_PACKAGE(Threads REQUIRED)
+ list(APPEND wolfcore_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
+
+ FIND_PACKAGE(Ceres REQUIRED)
+ list(APPEND wolfcore_INCLUDE_DIRS ${CERES_INCLUDE_DIRS})
+ list(APPEND wolfcore_LIBRARIES ${CERES_LIBRARIES})
+
+ # YAML with yaml-cpp
+ FIND_PACKAGE(YamlCpp REQUIRED)
+ list(APPEND wolfcore_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS})
+ list(APPEND wolfcore_LIBRARIES ${YAMLCPP_LIBRARY})
+
+ #Eigen
+ # FIND_PACKAGE(Eigen3 REQUIRED)
+ # list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+
+ if(NOT Eigen3_FOUND)
+ FIND_PACKAGE(Eigen3 REQUIRED)
+ endif()
+ if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
+ message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3")
+ endif()
+ list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+endif(NOT wolfcore_FOUND)
+SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
diff --git a/codetemplates eclipse.xml b/codetemplates eclipse.xml
index a83cc56bd43bb272eff012dd89a401c33283610f..8fb5a247ead13c2795a1a7d70da79187d010ad7b 100644
--- a/codetemplates eclipse.xml
+++ b/codetemplates eclipse.xml
@@ -23,7 +23,7 @@ ${declarations}
${namespace_end}</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ test files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cpptestfile" name="Default C++ test template">${filecomment}
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
${includes}
diff --git a/demos/ACTIVESEARCH.yaml b/demos/ACTIVESEARCH.yaml
deleted file mode 100644
index 028f69ba6321802009765d346f2e735c6a018990..0000000000000000000000000000000000000000
--- a/demos/ACTIVESEARCH.yaml
+++ /dev/null
@@ -1,42 +0,0 @@
-sensor:
- type: "USB_CAM"
-
-detector:
- type: "ORB"
- nfeatures: 100
- scale factor: 1.2
- nlevels: 8
- edge threshold: 8 # 16
- first level: 0
- WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
- score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
-descriptor:
- type: "ORB"
- nfeatures: 100
- scale factor: 1.2
- nlevels: 8
- edge threshold: 8 # 16
- first level: 0
- WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
- score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
-matcher:
- type: "FLANNBASED"
- match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
- roi:
- width: 20
- height: 20
- min normalized score: 0.85
-
-algorithm:
- type: "ACTIVESEARCH"
- draw results: false
- grid horiz cells: 12
- grid vert cells: 8
- separation: 10
- min features to track: 5
- max new features: 40
- min response new features: 80
\ No newline at end of file
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 6ee86ed12165a0cc0bc78c93822a0b5a259eddb0..e6b2bb11cbe6b95dceba4095af5c57b2c76f16c0 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -1,228 +1,2 @@
-# Dynamically remove objects from list
-# add_executable(test_list_remove test_list_remove.cpp)
-
-# Matrix product test
-#ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp)
-
-#ADD_EXECUTABLE(test_eigen_template test_eigen_template.cpp)
-
-ADD_EXECUTABLE(test_fcn_ptr test_fcn_ptr.cpp)
-
-ADD_EXECUTABLE(test_kf_callback test_kf_callback.cpp)
-TARGET_LINK_LIBRARIES(test_kf_callback ${PROJECT_NAME})
-
-ADD_EXECUTABLE(test_wolf_logging test_wolf_logging.cpp)
-TARGET_LINK_LIBRARIES(test_wolf_logging ${PROJECT_NAME})
-
-IF(Ceres_FOUND)
- # test_processor_odom_3D
- ADD_EXECUTABLE(test_processor_odom_3D test_processor_odom_3D.cpp)
- TARGET_LINK_LIBRARIES(test_processor_odom_3D ${PROJECT_NAME})
-
-# ADD_EXECUTABLE(test_motion_2d test_motion_2d.cpp)
-# TARGET_LINK_LIBRARIES(test_motion_2d ${PROJECT_NAME})
-ENDIF(Ceres_FOUND)
-
-
-
-
-# State blocks with local parametrizations test
-#ADD_EXECUTABLE(test_state_quaternion test_state_quaternion.cpp)
-#TARGET_LINK_LIBRARIES(test_state_quaternion ${PROJECT_NAME})
-
-# Testing Eigen Permutations
-#ADD_EXECUTABLE(test_permutations solver/test_permutations.cpp)
-#TARGET_LINK_LIBRARIES(test_permutations ${PROJECT_NAME})
-
-# Enable Yaml config files
-IF(YAMLCPP_FOUND)
-# ADD_EXECUTABLE(test_yaml test_yaml.cpp)
-# TARGET_LINK_LIBRARIES(test_yaml ${PROJECT_NAME})
-
-# ADD_EXECUTABLE(test_yaml_conversions test_yaml_conversions.cpp)
-# TARGET_LINK_LIBRARIES(test_yaml_conversions ${PROJECT_NAME})
-
- # SensorFactory classes test
-# ADD_EXECUTABLE(test_wolf_factories test_wolf_factories.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_factories ${PROJECT_NAME})
-
- # Map load and save test
-# ADD_EXECUTABLE(test_map_yaml test_map_yaml.cpp)
-# TARGET_LINK_LIBRARIES(test_map_yaml ${PROJECT_NAME})
-ENDIF(YAMLCPP_FOUND)
-
-IF(Suitesparse_FOUND)
-IF (0) # These cannot compile on MacOSX -- we'll fix it some day.
- # Testing a ccolamd test
- ADD_EXECUTABLE(test_ccolamd solver/test_ccolamd.cpp)
- TARGET_LINK_LIBRARIES(test_ccolamd ${PROJECT_NAME})
-
- # Testing a blocks ccolamd test
- ADD_EXECUTABLE(test_ccolamd_blocks solver/test_ccolamd_blocks.cpp)
- TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${PROJECT_NAME})
-
- # Testing an incremental blocks ccolamd test
- ADD_EXECUTABLE(test_incremental_ccolamd_blocks solver/test_incremental_ccolamd_blocks.cpp)
- TARGET_LINK_LIBRARIES(test_incremental_ccolamd_blocks ${PROJECT_NAME})
-
- # Testing an incremental QR with block ccolamd test
- ADD_EXECUTABLE(test_iQR solver/test_iQR.cpp)
- TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME})
-
- # Testing QR solver test tending to wolf
- ADD_EXECUTABLE(test_iQR_wolf solver/test_iQR_wolf.cpp)
- TARGET_LINK_LIBRARIES(test_iQR_wolf ${PROJECT_NAME})
-
- # Testing SPQR simple test
- #ADD_EXECUTABLE(test_SPQR solver/test_SPQR.cpp)
- #TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME})
-ENDIF(0)
-
-ENDIF(Suitesparse_FOUND)
-
-# Building and populating the wolf tree
-# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
-
-# Building and populating the wolf tree from .graph file (TORO)
-#ADD_EXECUTABLE(test_wolf_imported_graph test_wolf_imported_graph.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_imported_graph ${PROJECT_NAME})
-
-# Comparing performance of wolf auto_diff and ceres auto_diff
-#ADD_EXECUTABLE(test_wolf_autodiffwrapper test_wolf_autodiffwrapper.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_autodiffwrapper ${PROJECT_NAME})
-
-# Prunning wolf tree from .graph file (TORO)
-#ADD_EXECUTABLE(test_wolf_prunning test_wolf_prunning.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_prunning ${PROJECT_NAME})
-
-# Sparsification from wolf tree from .graph file (TORO)
-ADD_EXECUTABLE(test_sparsification test_sparsification.cpp)
-TARGET_LINK_LIBRARIES(test_sparsification ${PROJECT_NAME})
-
-# Comparing analytic and autodiff odometry factors
-#ADD_EXECUTABLE(test_analytic_odom_factor test_analytic_odom_factor.cpp)
-#TARGET_LINK_LIBRARIES(test_analytic_odom_factor ${PROJECT_NAME})
-
-# Vision
-IF(vision_utils_FOUND)
-
- IF(Ceres_FOUND)
- # Testing many things for the 3D image odometry
- ADD_EXECUTABLE(test_image test_image.cpp)
- TARGET_LINK_LIBRARIES(test_image ${PROJECT_NAME})
-
- # Processor Image Landmark test
- ADD_EXECUTABLE(test_processor_tracker_landmark_image test_processor_tracker_landmark_image.cpp)
- TARGET_LINK_LIBRARIES(test_processor_tracker_landmark_image ${PROJECT_NAME})
-
- # Simple AHP test
- ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp)
- TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME})
-
- IF (APRILTAG_LIBRARY)
- ADD_EXECUTABLE(test_apriltag test_apriltag.cpp)
- TARGET_LINK_LIBRARIES(test_apriltag ${PROJECT_NAME})
- ENDIF(APRILTAG_LIBRARY)
-
- ENDIF(Ceres_FOUND)
-
- # Testing OpenCV functions for projection of points
- ADD_EXECUTABLE(test_projection_points test_projection_points.cpp)
- TARGET_LINK_LIBRARIES(test_projection_points ${PROJECT_NAME})
-
- # Factor test
- ADD_EXECUTABLE(test_factor_AHP test_factor_AHP.cpp)
- TARGET_LINK_LIBRARIES(test_factor_AHP ${PROJECT_NAME})
-
- # ORB tracker test
- ADD_EXECUTABLE(test_tracker_ORB test_tracker_ORB.cpp)
- TARGET_LINK_LIBRARIES(test_tracker_ORB ${PROJECT_NAME})
-
-ENDIF(vision_utils_FOUND)
-
-# Processor Tracker Feature test
-ADD_EXECUTABLE(test_processor_tracker_feature test_processor_tracker_feature.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_feature ${PROJECT_NAME})
-
-# Processor Tracker Landmark test
-ADD_EXECUTABLE(test_processor_tracker_landmark test_processor_tracker_landmark.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_landmark ${PROJECT_NAME})
-
-# Processor IMU test
-ADD_EXECUTABLE(test_processor_imu test_processor_imu.cpp)
-TARGET_LINK_LIBRARIES(test_processor_imu ${PROJECT_NAME})
-
-# Processor Diff-Drive test
-ADD_EXECUTABLE(test_processor_diff_drive test_diff_drive.cpp)
-TARGET_LINK_LIBRARIES(test_processor_diff_drive ${PROJECT_NAME})
-
-# MPU6050 IMU test
-#ADD_EXECUTABLE(test_mpu test_mpu.cpp)
-#TARGET_LINK_LIBRARIES(test_mpu ${PROJECT_NAME})
-
-# Processor IMU - Jacobian checking test
-#ADD_EXECUTABLE(test_processor_imu_jacobians test_processor_imu_jacobians.cpp)
-#TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME})
-
-# Test IMU using printed Dock
-#ADD_EXECUTABLE(test_imuDock test_imuDock.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock ${PROJECT_NAME})
-
-# Test IMU with auto KF generation (Humanoids 20017)
-#ADD_EXECUTABLE(test_imuDock_autoKFs test_imuDock_autoKFs.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock_autoKFs ${PROJECT_NAME})
-
-# FactorIMU - factors test
-#ADD_EXECUTABLE(test_imu_constrained0 test_imu_constrained0.cpp)
-#TARGET_LINK_LIBRARIES(test_imu_constrained0 ${PROJECT_NAME})
-
-# IMU - Offline plateform test
-#ADD_EXECUTABLE(test_imuPlateform_Offline test_imuPlateform_Offline.cpp)
-#TARGET_LINK_LIBRARIES(test_imuPlateform_Offline ${PROJECT_NAME})
-
-# IMU - factorIMU
-#ADD_EXECUTABLE(test_factor_imu test_factor_imu.cpp)
-#TARGET_LINK_LIBRARIES(test_factor_imu ${PROJECT_NAME})
-
-# IF (laser_scan_utils_FOUND)
-# ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
-# TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME})
-# #ENDIF (laser_scan_utils_FOUND)
-
-# IF(faramotics_FOUND)
-# IF (laser_scan_utils_FOUND)
-# ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_2_lasers
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp)
-# TARGET_LINK_LIBRARIES(test_2_lasers_offline
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp)
-# TARGET_LINK_LIBRARIES(test_faramotics_simulation
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# # ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
-# # TARGET_LINK_LIBRARIES(test_autodiff
-# # ${pose_state_time_LIBRARIES}
-# # ${faramotics_LIBRARIES}
-# # ${PROJECT_NAME})
-# # IF(Suitesparse_FOUND)
-# # ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
-# # TARGET_LINK_LIBRARIES(test_iQR_wolf2
-# # ${pose_state_time_LIBRARIES}
-# # ${faramotics_LIBRARIES}
-# # ${PROJECT_NAME})
-# # ENDIF(Suitesparse_FOUND)
-# # ENDIF (laser_scan_utils_FOUND)
-# # ENDIF(faramotics_FOUND)
+#Add hello_wolf demo
+add_subdirectory(hello_wolf)
diff --git a/demos/camera_Dinesh_LAAS_params.yaml b/demos/camera_Dinesh_LAAS_params.yaml
deleted file mode 100644
index 739505d12e8a2dd224b99220ee024ddc8efd9508..0000000000000000000000000000000000000000
--- a/demos/camera_Dinesh_LAAS_params.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: mono
-camera_matrix:
- rows: 3
- cols: 3
- data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.416913, 0.264210, -0.000221, -0.000326, 0.000000]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
- rows: 3
- cols: 4
- data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_Dinesh_LAAS_params_notangentrect.yaml b/demos/camera_Dinesh_LAAS_params_notangentrect.yaml
deleted file mode 100644
index dd2f6433f2b60c21b68ebf70b595af981550923c..0000000000000000000000000000000000000000
--- a/demos/camera_Dinesh_LAAS_params_notangentrect.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: mono
-camera_matrix:
- rows: 3
- cols: 3
- data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.416913, 0.264210, 0, 0, 0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
- rows: 3
- cols: 4
- data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_logitech_c300_640_480.yaml b/demos/camera_logitech_c300_640_480.yaml
deleted file mode 100644
index 5f8a1899b71df3721e6f9722d39bd8e09e34509a..0000000000000000000000000000000000000000
--- a/demos/camera_logitech_c300_640_480.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-image_width: 640
-image_height: 480
-#camera_name: narrow_stereo
-camera_name: camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [711.687376, 0.000000, 323.306816, 0.000000, 710.933260, 232.005822, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [0.067204, -0.141639, 0, 0, 0]
-# data: [0.067204, -0.141639, 0.004462, -0.000636, 0.000000]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
- rows: 3
- cols: 4
- data: [718.941772, 0.000000, 323.016804, 0.000000, 0.000000, 717.174622, 233.475721, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_params.yaml b/demos/camera_params.yaml
deleted file mode 100644
index de0c31d0b05a024724840598a490527a62002ca2..0000000000000000000000000000000000000000
--- a/demos/camera_params.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: narrow_stereo
-camera_matrix:
- rows: 3
- cols: 3
- data: [872.791604, 0, 407.599166, 0, 883.154343, 270.343971, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.284384, -0.030014, -0.01554, -0.003363, 0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [826.19989, 0, 413.746093, 0, 0, 863.790649, 267.937027, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/demos/camera_params_canonical.yaml b/demos/camera_params_canonical.yaml
deleted file mode 100644
index 2508a0bec574125ae9dea63e558528b7211079d1..0000000000000000000000000000000000000000
--- a/demos/camera_params_canonical.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 2
-image_height: 2
-camera_name: canonical
-camera_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-distortion_model: none
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [0, 0, 0, 0, 0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/demos/camera_params_ueye.yaml b/demos/camera_params_ueye.yaml
deleted file mode 100644
index 8703fc4575d1422190e318fb7e0f8a816d37e5cb..0000000000000000000000000000000000000000
--- a/demos/camera_params_ueye.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [392.796383, 0, 350.175772, 0, 392.779002, 255.209917, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.3, 0.096, 0, 0, 0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [313.834106, 0, 361.199457, 0, 0, 349.145264, 258.654166, 0, 0, 0, 1, 0]
diff --git a/demos/camera_params_ueye_radial_dist.yaml b/demos/camera_params_ueye_radial_dist.yaml
deleted file mode 100644
index 52d1fbb4dd01de04e6c759f0850015509235f4b4..0000000000000000000000000000000000000000
--- a/demos/camera_params_ueye_radial_dist.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: narrow_stereo
-camera_matrix:
- rows: 3
- cols: 3
- data: [402.860630, 0.000000, 350.628016, 0.000000, 403.220300, 269.746108, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.276945, 0.095681, 0.000000, 0.000000, -0.013371]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
- rows: 3
- cols: 4
- data: [323.792358, 0.000000, 363.187868, 0.000000, 0.000000, 357.040344, 276.369440, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_params_ueye_sim.yaml b/demos/camera_params_ueye_sim.yaml
deleted file mode 100644
index 97f8f676b6b36629bb76a8bdc8c3fe06250b7b5b..0000000000000000000000000000000000000000
--- a/demos/camera_params_ueye_sim.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [320, 0, 320, 0, 320, 240, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [0, 0, 0, 0, 0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [320, 0, 320, 0, 0, 320, 240, 0, 0, 0, 1, 0]
diff --git a/demos/demo_2_lasers_offline.cpp b/demos/demo_2_lasers_offline.cpp
deleted file mode 100644
index 58e91c6d8a63e81ff0337ee1ec4a7b601cdb9de7..0000000000000000000000000000000000000000
--- a/demos/demo_2_lasers_offline.cpp
+++ /dev/null
@@ -1,321 +0,0 @@
-//std includes
-#include "core/sensor/sensor_GPS_fix.h"
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "core/problem/problem.h"
-#include "core/processor/processor_tracker_landmark_corner.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// laserscanutils
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/laser_scan.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-#include "core/capture/capture_pose.h"
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-void extractVector(std::ifstream& text_file, Eigen::VectorXs& vector, wolf::Scalar& ts)
-{
- std::string line;
- std::getline(text_file, line);
- std::stringstream line_stream(line);
- line_stream >> ts;
- for (auto i = 0; i < vector.size(); i++)
- line_stream >> vector(i);
-}
-
-void extractScan(std::ifstream& text_file, std::vector<float>& scan, wolf::Scalar& ts)
-{
- std::string line;
- std::getline(text_file, line);
- std::stringstream line_stream(line);
- line_stream >> ts;
- for (unsigned int i = 0; i < scan.size(); i++)
- line_stream >> scan[i];
-}
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << "\n==================================================================\n";
- std::cout << "========== 2D Robot with offline odometry and 2 LIDARs =============\n";
-
- // USER INPUT ============================================================================================
- if (argc != 2 || atoi(argv[1]) < 1 )
- {
- std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
- std::cout << " - NI is the number of iterations (NI > 0)" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- // FILES INPUT ============================================================================================
- std::ifstream laser_1_file, laser_2_file, odom_file, groundtruth_file;
- laser_1_file.open("simulated_laser_1.txt", std::ifstream::in);
- laser_2_file.open("simulated_laser_2.txt", std::ifstream::in);
- odom_file.open("simulated_odom.txt", std::ifstream::in);
- groundtruth_file.open("simulated_groundtruth.txt", std::ifstream::in);
-
- if (!(laser_1_file.is_open() && laser_2_file.is_open() && odom_file.is_open() && groundtruth_file.is_open()))
- {
- std::cout << "Error opening simulated data files. Remember to run test_faramotics_simulation before this test!" << std::endl;
- return -1;
- }
- else
- std::cout << "Simulated data files opened correctly..." << std::endl;
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
- // INITIALIZATION ============================================================================================
- //init random generators
- Scalar odom_std_factor = 0.5;
- std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
-
- //variables
- std::string line;
- Eigen::VectorXs odom_data = Eigen::VectorXs::Zero(2);
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::VectorXs ground_truth_pose(3); //last true pose
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(6);
- clock_t t1, t2;
- Scalar timestamp;
- TimeStamp ts(0);
-
- // Wolf initialization
- Eigen::VectorXs odom_pose = Eigen::VectorXs::Zero(3);
- //Eigen::VectorXs gps_position = Eigen::VectorXs::Zero(2);
- Eigen::VectorXs laser_1_params(9), laser_2_params(9);
- Eigen::VectorXs laser_1_pose(4), laser_2_pose(4); //xyz + theta
- Eigen::VectorXs laser_1_pose2D(3), laser_2_pose2D(3); //xy + theta
-
- // odometry intrinsics
- IntrinsicsOdom2D odom_intrinsics;
- odom_intrinsics.k_disp_to_disp = odom_std_factor;
- odom_intrinsics.k_rot_to_rot = odom_std_factor;
-
- // laser 1 extrinsics and intrinsics
- extractVector(laser_1_file, laser_1_params, timestamp);
- extractVector(laser_1_file, laser_1_pose, timestamp);
- laser_1_pose2D.head<2>() = laser_1_pose.head<2>();
- laser_1_pose2D(2) = laser_1_pose(3);
- std::vector<float> scan1(laser_1_params(8)); // number of ranges in a scan
- IntrinsicsLaser2D laser_1_intrinsics;
- laser_1_intrinsics.scan_params = laserscanutils::LaserScanParams({laser_1_params(0), laser_1_params(1), laser_1_params(2), laser_1_params(3), laser_1_params(4), laser_1_params(5), laser_1_params(6), laser_1_params(7)});
-
- ProcessorParamsLaser laser_1_processor_params;
- laser_1_processor_params.line_finder_params_ = laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2});
- laser_1_processor_params.new_corners_th = 10;
-
- // laser 2 extrinsics and intrinsics
- extractVector(laser_2_file, laser_2_params, timestamp);
- extractVector(laser_2_file, laser_2_pose, timestamp);
- laser_2_pose2D.head<2>() = laser_2_pose.head<2>();
- laser_2_pose2D(2) = laser_2_pose(3);
- std::vector<float> scan2(laser_2_params(8));
- IntrinsicsLaser2D laser_2_intrinsics;
- laser_2_intrinsics.scan_params = laserscanutils::LaserScanParams({laser_2_params(0), laser_2_params(1), laser_2_params(2), laser_2_params(3), laser_2_params(4), laser_2_params(5), laser_2_params(6), laser_2_params(7)});
-
- ProcessorParamsLaser laser_2_processor_params;
- laser_2_processor_params.line_finder_params_ = laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2});
- laser_2_processor_params.new_corners_th = 10;
-
- Problem problem(FRM_PO_2D);
- SensorOdom2D* odom_sensor = (SensorOdom2D*)problem.installSensor("ODOM 2D", "odometer", odom_pose, &odom_intrinsics);
- ProcessorParamsOdom2D odom_params;
- odom_params.cov_det = 1;
- odom_params.dist_traveled_th_ = 5;
- odom_params.elapsed_time_th_ = 10;
- ProcessorOdom2D* odom_processor = (ProcessorOdom2D*)problem.installProcessor("ODOM 2D", "main odometry", odom_sensor, &odom_params);
- //SensorBasePtr gps_sensor = problem.installSensor("GPS FIX", "GPS fix", gps_position);
- SensorBasePtr laser_1_sensor = problem.installSensor("LASER 2D", "front laser", laser_1_pose2D, &laser_1_intrinsics);
- SensorBasePtr laser_2_sensor = problem.installSensor("LASER 2D", "rear laser", laser_2_pose2D, &laser_2_intrinsics);
- problem.installProcessor("LASER 2D", "front laser processor", laser_1_sensor, &laser_1_processor_params);
- problem.installProcessor("LASER 2D", "rear laser processor", laser_2_sensor, &laser_2_processor_params);
-
- std::cout << "Wolf tree setted correctly!" << std::endl;
-
- CaptureMotion* odom_capture = new CaptureMotion(ts, odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
-
- // Initial pose
- ground_truth_pose << 2, 8, 0;
- ground_truth.head(3) = ground_truth_pose;
- odom_trajectory.head(3) = ground_truth_pose;
-
- // Origin Key Frame with covariance
- problem.setPrior(ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1, ts, 0.1);
-
- // Ceres wrapper
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- google::InitGoogleLogging(argv[0]);
-
- CeresManager ceres_manager(&problem, ceres_options);
- std::ofstream log_file, landmark_file; //output file
-
- std::cout << "START TRAJECTORY..." << std::endl;
- // START TRAJECTORY ============================================================================================
- for (unsigned int step = 1; step < n_execution; step++)
- {
- //get init time
- t2 = clock();
-
- // GROUNDTRUTH ---------------------------
- //std::cout << "GROUND TRUTH..." << std::endl;
- t1 = clock();
- extractVector(groundtruth_file, ground_truth_pose, timestamp);
- ground_truth.segment(step * 3, 3) = ground_truth_pose;
-
- // timestamp
- ts = TimeStamp(timestamp);
-
- // ODOMETRY DATA -------------------------------------
- //std::cout << "ODOMETRY DATA..." << std::endl;
- extractVector(odom_file, odom_data, timestamp);
- // noisy odometry
- odom_data(0) += distribution_odom(generator) * (odom_data(0) == 0 ? 1e-6 : odom_data(0));
- odom_data(1) += distribution_odom(generator) * (odom_data(1) == 0 ? 1e-6 : odom_data(1));
- // process odometry
- odom_capture->setTimeStamp(TimeStamp(ts));
- odom_capture->setData(odom_data);
- odom_processor->process(odom_capture);
- // odometry integration
- odom_trajectory.segment(step * 3, 3) = problem.getCurrentState();
-
- // LIDAR DATA ---------------------------
- extractScan(laser_1_file, scan1, timestamp);
- extractScan(laser_2_file, scan2, timestamp);
- if (step % 3 == 0)
- {
- std::cout << "--PROCESS LIDAR 1 DATA..." << std::endl;
- CaptureLaser2D* new_scan_1 = new CaptureLaser2D(ts, laser_1_sensor, scan1);
- new_scan_1->process();
- std::cout << "--PROCESS LIDAR 2 DATA..." << std::endl;
- CaptureLaser2D* new_scan_2 = new CaptureLaser2D(ts, laser_2_sensor, scan2);
- new_scan_2->process();
- }
- mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // SOLVE OPTIMIZATION ---------------------------
- std::cout << "SOLVING..." << std::endl;
- t1 = clock();
- ceres::Solver::Summary summary = ceres_manager.solve();
- //std::cout << summary.FullReport() << std::endl;
- mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // COMPUTE COVARIANCES ---------------------------
- std::cout << "COMPUTING COVARIANCES..." << std::endl;
- t1 = clock();
- ceres_manager.computeCovariances();
- mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // TIME MANAGEMENT ---------------------------
- double total_t = ((double) clock() - t2) / CLOCKS_PER_SEC;
- mean_times(5) += total_t;
- if (total_t < 0.2)
- usleep(200000 - 1e6 * total_t);
-
-// std::cout << "\nTree after step..." << std::endl;
- }
-
- // DISPLAY RESULTS ============================================================================================
- mean_times /= n_execution;
- std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
- std::cout << " data reading: " << mean_times(0) << std::endl;
- std::cout << " wolf managing: " << mean_times(1) << std::endl;
- std::cout << " ceres managing: " << mean_times(2) << std::endl;
- std::cout << " ceres optimization: " << mean_times(3) << std::endl;
- std::cout << " ceres covariance: " << mean_times(4) << std::endl;
- std::cout << " loop time: " << mean_times(5) << std::endl;
-
- // std::cout << "\nTree before deleting..." << std::endl;
-
- // Print Final result in a file -------------------------
- // Vehicle poses
- int i = 0;
- Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
- for (auto frame : *(problem.getTrajectory()->getFrameList()))
- {
- state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
- i += 3;
- }
-
- // Landmarks
- i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
- for (auto landmark : *(problem.getMap()->getLandmarkList()))
- {
- landmarks.segment(i, 2) = landmark->getP()->getVector();
- i += 2;
- }
-
- // Print log files
- std::string filepath = getenv("HOME") + std::string("/Desktop/log_file_2.txt");
- log_file.open(filepath, std::ofstream::out); //open log file
-
- if (log_file.is_open())
- {
- log_file << 0 << std::endl;
- for (unsigned int ii = 0; ii < n_execution; ii++)
- log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
- log_file.close(); //close log file
- std::cout << std::endl << "Result file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-
- std::string filepath2 = getenv("HOME") + std::string("/Desktop/landmarks_file_2.txt");
- landmark_file.open(filepath2, std::ofstream::out); //open log file
-
- if (landmark_file.is_open())
- {
- for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
- landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
- landmark_file.close(); //close log file
- std::cout << std::endl << "Landmark file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-
- std::cout << "Press any key for ending... " << std::endl << std::endl;
- std::getchar();
-
- std::cout << "Problem:" << std::endl;
- std::cout << "Frames: " << problem.getTrajectory()->getFrameList().size() << std::endl;
- std::cout << "Landmarks: " << problem.getMap()->getLandmarkList()->size() << std::endl;
- std::cout << "Sensors: " << problem.getHardware()->getSensorList()->size() << std::endl;
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_ORB.png b/demos/demo_ORB.png
deleted file mode 100644
index 016141f5309c1ed34a61d71cfa63b130ea90aa8f..0000000000000000000000000000000000000000
Binary files a/demos/demo_ORB.png and /dev/null differ
diff --git a/demos/demo_analytic_odom_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
similarity index 87%
rename from demos/demo_analytic_odom_factor.cpp
rename to demos/demo_analytic_autodiff_factor.cpp
index 32a2cbc64fcac5b0d37052347919f3e308042585..0fd8d42cc5280016d7473e23cb2624615221fda7 100644
--- a/demos/demo_analytic_odom_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -10,20 +10,20 @@
#include <cmath>
#include <queue>
+#include "../include/core/ceres_wrapper/solver_ceres.h"
//Wolf includes
#include "wolf_manager.h"
#include "core/capture/capture_void.h"
-#include "core/ceres_wrapper/ceres_manager.h"
// EIGEN
//#include <Eigen/CholmodSupport>
namespace wolf {
// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
{
for (int k=0; k<ins.outerSize(); ++k)
- for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
+ for (Eigen::SparseMatrix<double>::InnerIterator iti(ins,k); iti; ++iti)
original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
original.makeCompressed();
@@ -57,17 +57,19 @@ int main(int argc, char** argv)
std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_analytic;
// Wolf problem
- ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2D);
- ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
+ ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
// Ceres wrapper
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, ceres_options);
- CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, ceres_options);
+ SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);
+ SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options);
+ ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_manager_autodiff.getSolverOptions().max_line_search_step_contraction = 1e-3;
+ ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4;
+ ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_manager_analytic.getSolverOptions().max_line_search_step_contraction = 1e-3;
+ ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4;
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
@@ -102,7 +104,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// vertex pose
- Eigen::Vector3s vertex_pose;
+ Eigen::Vector3d vertex_pose;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -164,7 +166,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge vector
- Eigen::Vector3s edge_vector;
+ Eigen::Vector3d edge_vector;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -188,7 +190,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge covariance
- Eigen::Matrix3s edge_information;
+ Eigen::Matrix3d edge_information;
// xx
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -242,7 +244,7 @@ int main(int argc, char** argv)
FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
- FactorOdom2D* factor_ptr_autodiff = new FactorOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
+ FactorOdom2d* factor_ptr_autodiff = new FactorOdom2d(feature_ptr_autodiff, frame_old_ptr_autodiff);
feature_ptr_autodiff->addFactor(factor_ptr_autodiff);
//std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl;
@@ -255,7 +257,7 @@ int main(int argc, char** argv)
FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
capture_ptr_analytic->addFeature(feature_ptr_analytic);
- FactorOdom2DAnalytic* factor_ptr_analytic = new FactorOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
+ FactorOdom2dAnalytic* factor_ptr_analytic = new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
feature_ptr_analytic->addFactor(factor_ptr_analytic);
//std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
//std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
@@ -272,10 +274,10 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front();
- CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3s::Identity() * 0.01);
- CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3s::Identity() * 0.01);
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front();
+ CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
+ CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
first_frame_analytic->addCapture(initial_covariance_analytic);
initial_covariance_autodiff->emplaceFeatureAndFactor();
diff --git a/demos/demo_apriltag.cpp b/demos/demo_apriltag.cpp
deleted file mode 100644
index 66ab9995d34cea1b6df3333a1d9f1f2e88434a64..0000000000000000000000000000000000000000
--- a/demos/demo_apriltag.cpp
+++ /dev/null
@@ -1,290 +0,0 @@
-/**
- * \file test_apriltag.cpp
- *
- * Created on: Dec 14, 2018
- * \author: Dinesh Atchtuhan
- */
-
-//Wolf
-#include "core/wolf.h"
-#include "core/rotations.h"
-#include "core/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/sensor/sensor_camera.h"
-#include "core/processor/processor_tracker_landmark_apriltag.h"
-#include "core/capture/capture_image.h"
-#include "core/feature/feature_apriltag.h"
-
-// opencv
-#include <opencv2/imgproc/imgproc.hpp>
-#include "opencv2/opencv.hpp"
-
-// Eigen
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
-// std
-#include <iostream>
-#include <stdlib.h>
-
-
-void draw_apriltag(cv::Mat image, std::vector<cv::Point2d> corners, int thickness=1, bool draw_corners=false);
-
-
-int main(int argc, char *argv[])
-{
- /*
- * HOW TO USE ?
- * For now, just call the executable and append the list of images to be processed.
- * The images must be placed in the root folder of your wolf project.
- * Ex:
- * ./test_apriltag frame1.jpg frame2.jpg frame3.jpg
- */
-
- using namespace wolf;
-
-
- // General execution options
- const bool APPLY_CONTRAST = false;
- const bool IMAGE_OUTPUT = true;
- const bool USEMAP = false;
-
-
- WOLF_INFO( "==================== processor apriltag test ======================" )
-
- std::string wolf_root = _WOLF_ROOT_DIR;
- // Wolf problem
- ProblemPtr problem = Problem::create("PO", 3);
- ceres::Solver::Options options;
- options.function_tolerance = 1e-6;
- options.max_num_iterations = 100;
- CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, options);
-
-
- WOLF_INFO( "==================== Configure Problem ======================" )
- Eigen::Vector7s cam_extrinsics; cam_extrinsics << 0,0,0, 0,0,0,1;
- SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_logitech_c300_640_480.yaml");
-// SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml");
- SensorCameraPtr sen_cam = std::static_pointer_cast<SensorCamera>(sen);
- ProcessorBasePtr prc = problem->installProcessor("TRACKER LANDMARK APRILTAG", "apriltags", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml");
-
- if (USEMAP){
- problem->loadMap(wolf_root + "/src/examples/maps/map_apriltag_logitech_1234.yaml");
- for (auto lmk : problem->getMap()->getLandmarkList()){
- lmk->fix();
- }
- }
-
- // set prior
- Eigen::Matrix6s covariance = Matrix6s::Identity();
- Scalar std_m;
- Scalar std_deg;
- if (USEMAP){
- std_m = 100; // standard deviation on original translation
- std_deg = 180; // standard deviation on original rotation
- }
- else {
- std_m = 0.00001; // standard deviation on original translation
- std_deg = 0.00001; // standard deviation on original rotation
- }
-
- covariance.topLeftCorner(3,3) = std_m*std_m * covariance.topLeftCorner(3,3);
- covariance.bottomRightCorner(3,3) = (M_TORAD*std_deg)*(M_TORAD*std_deg) * covariance.bottomRightCorner(3,3);
-
- if (USEMAP){
- FrameBasePtr F1 = problem->setPrior((Vector7s()<<0.08, 0.15, -0.75, 0, 0, 0, 1).finished(), covariance, 0.0, 0.1);
- }
- else {
- FrameBasePtr F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), covariance, 0.0, 0.1);
- F1->fix();
- }
-
- // first argument is the name of the program.
- // following arguments are path to image (from wolf_root)
- const int inputs = argc -1;
- WOLF_DEBUG("nb of images: ", inputs);
- cv::Mat frame;
- Scalar ts(0);
- Scalar dt = 1;
-
- WOLF_INFO( "==================== Main loop ======================" )
- for (int input = 1; input <= inputs; input++) {
- std::string path = wolf_root + "/" + argv[input];
- frame = cv::imread(path, CV_LOAD_IMAGE_COLOR);
-
- if( frame.data ){ //if imread succeeded
-
- if (APPLY_CONTRAST){
- Scalar alpha = 2.0; // to tune contrast [1-3]
- int beta = 0; // to tune lightness [0-100]
- // Do the operation new_image(i,j) = alpha*image(i,j) + beta
- for( int y = 0; y < frame.rows; y++ ){
- for( int x = 0; x < frame.cols; x++ ){
- for( int c = 0; c < 3; c++ ){
- frame.at<cv::Vec3b>(y,x)[c] = cv::saturate_cast<uchar>( alpha*( frame.at<cv::Vec3b>(y,x)[c] ) + beta );
- }
- }
- }
- }
-
- CaptureImagePtr cap = std::make_shared<CaptureImage>(ts, sen_cam, frame);
- // cap->setType(argv[input]); // only for problem->print() to show img filename
- cap->setName(argv[input]);
- WOLF_DEBUG("Processing image...", path);
- sen->process(cap);
-
- if (IMAGE_OUTPUT){
- cv::namedWindow( cap->getName(), cv::WINDOW_NORMAL );// Create a window for display.
- }
-
- }
- else
- WOLF_WARN("could not load image ", path);
-
- ts += dt;
- }
-
-
- if (IMAGE_OUTPUT){
- WOLF_INFO( "==================== Draw all detections ======================" )
- for (auto F : problem->getTrajectory()->getFrameList())
- {
- if (F->isKey())
- {
- for (auto cap : F->getCaptureList())
- {
- if (cap->getType() == "IMAGE")
- {
- auto img = std::static_pointer_cast<CaptureImage>(cap);
- for (FeatureBasePtr f : img->getFeatureList())
- {
- FeatureApriltagPtr fa = std::static_pointer_cast<FeatureApriltag>(f);
- draw_apriltag(img->getImage(), fa->getTagCorners(), 1);
- }
- cv::imshow( img->getName(), img->getImage() ); // display original image.
- cv::waitKey(1);
- }
- }
- }
- }
- }
-
-
-
-// WOLF_INFO( "==================== Provide perturbed prior ======================" )
-// for (auto kf : problem->getTrajectory()->getFrameList())
-// {
-// Vector7s x;
-// if (kf->isKey())
-// {
-// x.setRandom();
-// x.tail(4).normalize();
-// kf->setState(x);
-// }
-// }
-
- WOLF_INFO( "==================== Solve problem ======================" )
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::FULL); // 0: nothing, 1: BriefReport, 2: FullReport
- WOLF_DEBUG(report);
- problem->print(3,0,1,1);
-
-
-
- WOLF_INFO("============= SOLVED PROBLEM : POS | EULER (DEG) ===============")
- for (auto kf : problem->getTrajectory()->getFrameList())
- {
- if (kf->isKey())
- for (auto cap : kf->getCaptureList())
- {
- if (cap->getType() != "POSE")
- {
- Vector3s T = kf->getP()->getState();
- Vector4s qv= kf->getO()->getState();
- Vector3s e = M_TODEG * R2e(q2R(qv));
- WOLF_DEBUG("KF", kf->id(), " => ", T.transpose(), " | ", e.transpose());
- }
- }
- }
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- Vector3s T = lmk->getP()->getState();
- Vector4s qv= lmk->getO()->getState();
- Vector3s e = M_TODEG * R2e(q2R(qv));
- WOLF_DEBUG(" L", lmk->id(), " => ", T.transpose(), " | ", e.transpose());
- }
-
-
- // ===============================================
- // COVARIANCES ===================================
- // ===============================================
- // Print COVARIANCES of all states
- WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM : POS | QUAT =======")
- ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectory()->getFrameList())
- if (kf->isKey())
- {
- Eigen::MatrixXs cov = kf->getCovariance();
- WOLF_DEBUG("KF", kf->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt());
- }
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- Eigen::MatrixXs cov = lmk->getCovariance();
- WOLF_DEBUG(" L", lmk->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt());
- }
- std::cout << std::endl;
-
-
- // ===============================================
- // SAVE MAP TO YAML ==============================
- // ===============================================
- //
- // problem->saveMap(wolf_root + "/src/examples/map_apriltag_set3_HC.yaml", "set3");
-
- if (IMAGE_OUTPUT){
- cv::waitKey(0);
- cv::destroyAllWindows();
- }
-
- return 0;
-
-}
-
-
-void draw_apriltag(cv::Mat image, std::vector<cv::Point2d> corners,
- int thickness, bool draw_corners) {
- cv::line(image, corners[0], corners[1], CV_RGB(255, 0, 0), thickness);
- cv::line(image, corners[1], corners[2], CV_RGB(0, 255, 0), thickness);
- cv::line(image, corners[2], corners[3], CV_RGB(0, 0, 255), thickness);
- cv::line(image, corners[3], corners[0], CV_RGB(255, 0, 255), thickness);
-
- ///////
- // Leads to implement other displays
- ///////
-
-// const auto line_type = cv::LINE_AA;
-// if (draw_corners) {
-// int r = thickness;
-// cv::circle(image, cv::Point2i(p[0].x, p[0].y), r, CV_RGB(255, 0, 0), -1,
-// line_type);
-// cv::circle(image, cv::Point2i(p[1].x, p[1].y), r, CV_RGB(0, 255, 0), -1,
-// line_type);
-// cv::circle(image, cv::Point2i(p[2].x, p[2].y), r, CV_RGB(0, 0, 255), -1,
-// line_type);
-// cv::circle(image, cv::Point2i(p[3].x, p[3].y), r, CV_RGB(255, 0, 255), -1,
-// line_type);
-// }
-
-// cv::putText(image, std::to_string(apriltag.id),
-// cv::Point2f(apriltag.center.x - 5, apriltag.center.y + 5),
-// cv::FONT_HERSHEY_SIMPLEX, 1, CV_RGB(255, 0, 255), 2, line_type);
-
-
-}
-
-//void DrawApriltags(cv::Mat &image, const ApriltagVec &apriltags) {
-// for (const auto &apriltag : apriltags) {
-//// DrawApriltag(image, apriltag);
-// DrawApriltag(image, apriltag, 1);
-// }
-//}
-
diff --git a/demos/demo_autodiff.cpp b/demos/demo_autodiff.cpp
deleted file mode 100644
index 6fa1e01edddf6861f52c6c50222be29521ce68a2..0000000000000000000000000000000000000000
--- a/demos/demo_autodiff.cpp
+++ /dev/null
@@ -1,422 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf_manager.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-//laser_scan_utils
-#include "iri-algorithms/laser_scan_utils/corner_detector.h"
-#include "iri-algorithms/laser_scan_utils/entities.h"
-
-namespace wolf {
-//function travel around
-void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
-{
- if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
- else if (ii <= 220) { displacement_ = 0; rotation_ =-1.8 * M_PI / 180; }
- else if (ii <= 310) { displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 487) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 600) { displacement_ = 0.2; rotation_ = 0; }
- else if (ii <= 700) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 780) { displacement_ = 0; rotation_ =-1.0 * M_PI / 180; }
- else { displacement_ = 0.3; rotation_ = 0; }
-
- pose.moveForward(displacement_);
- pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
-}
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
-
- // USER INPUT ============================================================================================
- if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1 )
- {
- std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
- std::cout << " - NI is the number of iterations (NI > 0)" << std::endl;
- std::cout << " - WS is the window size (WS > 0)" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
- unsigned int window_size = (unsigned int) atoi(argv[2]);
-
- // INITIALIZATION ============================================================================================
- //init random generators
- Scalar odom_std_factor = 0.5;
- Scalar gps_std = 1;
- std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
- std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
-
- //init google log
- //google::InitGoogleLogging(argv[0]);
-
- // Faramotics stuff
- Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
- vector < Cpose3d > devicePoses;
- vector<float> scan1, scan2;
- string modelFileName;
-
- //model and initial view point
- modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
- //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
- //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
- devicePose.setPose(2, 8, 0.2, 0, 0, 0);
- viewPoint.setPose(devicePose);
- viewPoint.moveForward(10);
- viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
- viewPoint.moveForward(-15);
- //glut initialization
- faramotics::initGLUT(argc, argv);
-
- //create a viewer for the 3D model and scan points
- CdynamicSceneRender* myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
- myRender->loadAssimpModel(modelFileName, true); //with wireframe
- //create scanner and load 3D model
- CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
- myScanner->loadAssimpModel(modelFileName);
-
- //variables
- Eigen::Vector3s odom_reading;
- Eigen::Vector2s gps_fix_reading;
- Eigen::VectorXs pose_odom(3); //current odometry integred pose
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
- clock_t t1, t2;
-
- // Wolf manager initialization
- Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
- Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
- Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
- laser_1_pose << 1.2, 0, 0, 0; //laser 1
- laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(std::make_shared<StateBlock>(gps_pose.head(2)), std::make_shared<StateBlock>(gps_pose.tail(1)), gps_std);
- SensorLaser2D laser_1_sensor(std::make_shared<StateBlock>(laser_1_pose.head(2)), std::make_shared<StateBlock>(laser_1_pose.tail(1)));
- SensorLaser2D laser_2_sensor(std::make_shared<StateBlock>(laser_2_pose.head(2)), std::make_shared<StateBlock>(laser_2_pose.tail(1)));
- laser_1_sensor.addProcessor(new ProcessorLaser2D());
- laser_2_sensor.addProcessor(new ProcessorLaser2D());
-
- // Initial pose
- pose_odom << 2, 8, 0;
- ground_truth.head(3) = pose_odom;
- odom_trajectory.head(3) = pose_odom;
-
- WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
- WolfManager* wolf_manager_wolf = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
-
- // Ceres wrapper
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- CeresManager* ceres_manager_ceres = new CeresManager(wolf_manager_ceres->getProblem(), false);
- CeresManager* ceres_manager_wolf = new CeresManager(wolf_manager_wolf->getProblem(), true);
- std::ofstream log_file, landmark_file; //output file
-
- //std::cout << "START TRAJECTORY..." << std::endl;
- // START TRAJECTORY ============================================================================================
- for (unsigned int step = 1; step < n_execution; step++)
- {
- //get init time
- t2 = clock();
-
- // ROBOT MOVEMENT ---------------------------
- //std::cout << "ROBOT MOVEMENT..." << std::endl;
- // moves the device position
- t1 = clock();
- motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
- odom_reading(1) = 0;
- devicePoses.push_back(devicePose);
-
- // SENSOR DATA ---------------------------
- //std::cout << "SENSOR DATA..." << std::endl;
- // store groundtruth
- ground_truth.segment(step * 3, 3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
-
- // compute odometry
- odom_reading(0) += distribution_odom(generator) * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
- odom_reading(1) += distribution_odom(generator) * 1e-6;
- odom_reading(2) += distribution_odom(generator) * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
-
- // odometry integration
- pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2));
- pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)) + odom_reading(1) * cos(pose_odom(2));
- pose_odom(2) = pose_odom(2) + odom_reading(1);
- odom_trajectory.segment(step * 3, 3) = pose_odom;
-
- // compute GPS
- gps_fix_reading << devicePose.pt(0), devicePose.pt(1);
- gps_fix_reading(0) += distribution_gps(generator);
- gps_fix_reading(1) += distribution_gps(generator);
-
- //compute scans
- scan1.clear();
- scan2.clear();
- // scan 1
- laser1Pose.setPose(devicePose);
- laser1Pose.moveForward(laser_1_pose(0));
- myScanner->computeScan(laser1Pose, scan1);
- // scan 2
- laser2Pose.setPose(devicePose);
- laser2Pose.moveForward(laser_2_pose(0));
- laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll());
- myScanner->computeScan(laser2Pose, scan2);
-
- mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // ADD CAPTURES ---------------------------
- std::cout << "ADD CAPTURES..." << std::endl;
- t1 = clock();
- // adding new sensor captures
- wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(),TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
- wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
- wolf_manager_ceres->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
- wolf_manager_ceres->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
- wolf_manager_wolf->addCapture(new CaptureOdom2D(TimeStamp(),TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
- wolf_manager_wolf->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
- wolf_manager_wolf->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
- wolf_manager_wolf->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
-
- // updating problem
- wolf_manager_ceres->update();
- wolf_manager_wolf->update();
- mean_times(1) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // UPDATING CERES ---------------------------
- std::cout << "UPDATING CERES..." << std::endl;
- t1 = clock();
- // update state units and factors in ceres
- ceres_manager_ceres->update();
- ceres_manager_wolf->update();
- mean_times(2) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // SOLVE OPTIMIZATION ---------------------------
- std::cout << "SOLVING..." << std::endl;
- t1 = clock();
- ceres::Solver::Summary summary_ceres = ceres_manager_ceres->solve(ceres_options);
- ceres::Solver::Summary summary_wolf = ceres_manager_wolf->solve(ceres_options);
- std::cout << "CERES AUTO DIFF" << std::endl;
- std::cout << "Jacobian evaluation: " << summary_ceres.jacobian_evaluation_time_in_seconds << std::endl;
- std::cout << "Total time: " << summary_ceres.total_time_in_seconds << std::endl;
- std::cout << "WOLF AUTO DIFF" << std::endl;
- std::cout << "Jacobian evaluation: " << summary_wolf.jacobian_evaluation_time_in_seconds << std::endl;
- std::cout << "Total time: " << summary_wolf.total_time_in_seconds << std::endl;
- mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- std::cout << "CERES AUTO DIFF solution:" << std::endl;
- std::cout << wolf_manager_ceres->getVehiclePose().transpose() << std::endl;
- std::cout << "WOLF AUTO DIFF solution:" << std::endl;
- std::cout << wolf_manager_wolf->getVehiclePose().transpose() << std::endl;
-
- // COMPUTE COVARIANCES ---------------------------
- std::cout << "COMPUTING COVARIANCES..." << std::endl;
- t1 = clock();
- ceres_manager_ceres->computeCovariances(ALL_MARGINALS);
- ceres_manager_wolf->computeCovariances(ALL_MARGINALS);
- Eigen::MatrixXs marginal_ceres(3,3), marginal_wolf(3,3);
- wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
- wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
- marginal_ceres, 0, 0);
- wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
- wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
- marginal_ceres, 0, 2);
- wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
- wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
- marginal_ceres, 2, 2);
- wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
- wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
- marginal_wolf, 0, 0);
- wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
- wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
- marginal_wolf, 0, 2);
- wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
- wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
- marginal_wolf, 2, 2);
- std::cout << "CERES AUTO DIFF covariance:" << std::endl;
- std::cout << marginal_ceres << std::endl;
- std::cout << "WOLF AUTO DIFF covariance:" << std::endl;
- std::cout << marginal_wolf << std::endl;
- mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // DRAWING STUFF ---------------------------
- t1 = clock();
- // draw detected corners
-// std::list < laserscanutils::Corner > corner_list;
-// std::vector<double> corner_vector;
-// CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
-// last_scan.extractCorners(corner_list);
-// for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
-// {
-// corner_vector.push_back(corner_it->pt_(0));
-// corner_vector.push_back(corner_it->pt_(1));
-// }
-// myRender->drawCorners(laser1Pose, corner_vector);
-
-// // draw landmarks
-// std::vector<double> landmark_vector;
-// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
-// {
-// Scalar* position_ptr = (*landmark_it)->getP()->get();
-// landmark_vector.push_back(*position_ptr); //x
-// landmark_vector.push_back(*(position_ptr + 1)); //y
-// landmark_vector.push_back(0.2); //z
-// }
-// myRender->drawLandmarks(landmark_vector);
-//
-// // draw localization and sensors
-// estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0), wolf_manager->getVehiclePose()(1), 0.2, wolf_manager->getVehiclePose()(2), 0, 0);
-// estimated_laser_1_pose.setPose(estimated_vehicle_pose);
-// estimated_laser_1_pose.moveForward(laser_1_pose(0));
-// estimated_laser_2_pose.setPose(estimated_vehicle_pose);
-// estimated_laser_2_pose.moveForward(laser_2_pose(0));
-// estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
-// myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-//
-// //Set view point and render the scene
-// //locate visualization view point, somewhere behind the device
-//// viewPoint.setPose(devicePose);
-//// viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-//// viewPoint.moveForward(-5);
-// myRender->setViewPoint(viewPoint);
-// myRender->drawPoseAxis(devicePose);
-// myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
-// myRender->render();
-// mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // TIME MANAGEMENT ---------------------------
- double dt = ((double) clock() - t2) / CLOCKS_PER_SEC;
- mean_times(6) += dt;
- if (dt < 0.1)
- usleep(100000 - 1e6 * dt);
-
-// std::cout << "\nTree after step..." << std::endl;
- }
-
- // DISPLAY RESULTS ============================================================================================
- mean_times /= n_execution;
- std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
- std::cout << " data generation: " << mean_times(0) << std::endl;
- std::cout << " wolf managing: " << mean_times(1) << std::endl;
- std::cout << " ceres managing: " << mean_times(2) << std::endl;
- std::cout << " ceres optimization: " << mean_times(3) << std::endl;
- std::cout << " ceres covariance: " << mean_times(4) << std::endl;
- std::cout << " results drawing: " << mean_times(5) << std::endl;
- std::cout << " loop time: " << mean_times(6) << std::endl;
-
-// std::cout << "\nTree before deleting..." << std::endl;
-
-// // Draw Final result -------------------------
-// std::vector<double> landmark_vector;
-// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
-// {
-// Scalar* position_ptr = (*landmark_it)->getP()->get();
-// landmark_vector.push_back(*position_ptr); //x
-// landmark_vector.push_back(*(position_ptr + 1)); //y
-// landmark_vector.push_back(0.2); //z
-// }
-// myRender->drawLandmarks(landmark_vector);
-//// viewPoint.setPose(devicePoses.front());
-//// viewPoint.moveForward(10);
-//// viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-//// viewPoint.moveForward(-10);
-// myRender->setViewPoint(viewPoint);
-// myRender->render();
-
- // Print Final result in a file -------------------------
- // Vehicle poses
-// int i = 0;
-// Eigen::VectorXs state_poses(n_execution * 3);
-// for (auto frame_it = wolf_manager->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
-// {
-// state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
-// i += 3;
-// }
-//
-// // Landmarks
-// i = 0;
-// Eigen::VectorXs landmarks(wolf_manager->getProblem()->getMap()->getLandmarkList().size() * 2);
-// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
-// {
-// Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
-// landmarks.segment(i, 2) = landmark;
-// i += 2;
-// }
-//
-// // Print log files
-// std::string filepath = getenv("HOME") + std::string("/Desktop/log_file_2.txt");
-// log_file.open(filepath, std::ofstream::out); //open log file
-//
-// if (log_file.is_open())
-// {
-// log_file << 0 << std::endl;
-// for (unsigned int ii = 0; ii < n_execution; ii++)
-// log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
-// log_file.close(); //close log file
-// std::cout << std::endl << "Result file " << filepath << std::endl;
-// }
-// else
-// std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-//
-// std::string filepath2 = getenv("HOME") + std::string("/Desktop/landmarks_file_2.txt");
-// landmark_file.open(filepath2, std::ofstream::out); //open log file
-//
-// if (landmark_file.is_open())
-// {
-// for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
-// landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
-// landmark_file.close(); //close log file
-// std::cout << std::endl << "Landmark file " << filepath << std::endl;
-// }
-// else
-// std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-//
-// std::cout << "Press any key for ending... " << std::endl << std::endl;
-// std::getchar();
-
- delete myRender;
- delete myScanner;
- delete wolf_manager_ceres;
- delete wolf_manager_wolf;
- std::cout << "wolf deleted" << std::endl;
- delete ceres_manager_ceres;
- delete ceres_manager_wolf;
- std::cout << "ceres_manager deleted" << std::endl;
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_capture_laser_2D.cpp b/demos/demo_capture_laser_2D.cpp
deleted file mode 100644
index cd5e40239fd42ca972a0e96405dc609ab707e9ca..0000000000000000000000000000000000000000
--- a/demos/demo_capture_laser_2D.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-
-//std
-#include <random>
-
-//wolf
-#include "core/capture/capture_laser_2D.h"
-
-// Eigen in std vector
-#include <Eigen/StdVector>
-
-//main
-int main(int argc, char *argv[])
-{
- using namespace wolf;
-
- std::cout << std::endl << "CaptureLaser2D class test" << std::endl;
- std::cout << "========================================================" << std::endl;
-
- //scan ranges
- Eigen::VectorXs ranges(720);
- ranges << 2.78886,2.78289,2.7832,2.78367,2.7843,2.78508,2.78603,2.78713,2.78839,2.78981,
- 2.79139,2.78669,2.78855,2.79057,2.79274,2.79507,2.79098,2.79359,2.79635,2.79927,
- 2.79566,2.79886,2.80221,2.79895,2.80258,2.80638,2.80345,2.80753,2.81176,2.80917,
- 2.81368,2.81132,2.81611,2.81396,2.81903,2.73664,2.72743,2.71782,2.70883,2.69945,
- 2.69067,2.6815,2.67294,2.66398,2.65562,2.64736,2.6387,2.63064,2.62218,2.61431,
- 2.60605,2.59837,2.59078,2.5828,2.57539,2.56759,2.56036,2.55321,2.54568,2.53871,
- 2.53182,2.52455,2.51782,2.51118,2.50415,2.49767,2.49127,2.48495,2.47824,2.47207,
- 2.46597,2.4595,2.45355,2.44768,2.44188,2.4357,2.43005,2.42446,2.41894,2.4135,
- 2.40768,2.40236,2.39712,2.39195,2.3864,2.38135,2.37637,2.37146,2.36661,2.36182,
- 2.35666,2.352,2.3474,2.34286,2.34186,2.36289,2.41051,2.43311,2.45628,2.48003,
- 2.50439,2.52937,2.555,2.61224,2.63993,2.66837,2.69757,2.72758,2.75841,2.79011,
- 2.82269,2.85621,2.8907,2.96659,3.0042,3.04295,3.08289,3.12406,3.16652,3.21033,
- 3.22693,3.23444,3.24207,3.24982,3.2577,3.26571,3.2855,3.29383,3.30229,3.31088,
- 3.3196,3.32846,3.33745,3.34658,3.35584,3.36524,3.37478,3.38445,3.39427,3.40423,
- 3.41434,3.42459,3.43499,3.44553,3.45623,3.46707,3.47807,3.48923,3.50053,3.512,
- 3.52362,3.53541,3.54736,3.55947,3.57175,3.5842,3.59681,3.6096,3.62256,3.63569,
- 3.64901,3.6625,3.67617,3.69003,3.70407,3.7183,3.73272,3.74733,3.76214,3.7931,
- 3.80843,3.82397,3.83972,3.85567,3.87183,3.88822,3.90481,3.92163,3.93867,3.95593,
- 3.97343,3.97347,3.99127,4.00931,4.02758,4.0461,4.06486,4.08387,4.10314,4.12266,
- 4.14244,4.16248,4.20237,4.22313,4.24418,4.26551,4.28712,4.30903,4.33122,4.35372,
- 4.37652,4.39963,4.42304,4.44678,4.47084,4.49523,4.51994,4.545,4.57041,4.59615,
- 4.62225,4.64871,4.67554,4.70274,4.73032,4.75828,4.78663,4.81538,4.84452,4.8741,
- 4.90409,4.9345,4.96534,4.99662,5.02836,5.06053,5.09317,5.12628,5.15987,5.19395,
- 5.22853,5.2636,5.2992,5.33532,5.37197,5.44106,5.47923,5.51796,5.55729,5.59721,
- 5.63773,5.67888,5.72066,5.76307,5.80615,5.8499,5.89432,5.93945,6.02374,6.07085,
- 6.11872,6.16734,6.21676,6.26698,6.318,6.36987,6.42258,6.47618,6.5758,6.6319,
- 6.68894,6.74694,6.80593,6.86593,6.92698,7.04022,7.10425,7.1694,7.23572,7.30322,
- 7.37192,7.49928,7.57149,7.64505,7.58372,7.51951,7.45681,7.32129,7.32938,7.34276,
- 7.35632,7.36877,7.38272,7.39687,7.41124,7.4258,7.43923,7.4542,7.46937,7.48477,
- 7.49904,7.51485,7.53087,7.54579,7.56225,7.57894,7.59587,7.61164,7.62902,8.37389,
- 8.39194,8.41173,8.43177,8.45059,8.47118,8.49202,8.51164,8.53305,8.55319,8.57516,
- 8.59739,8.61839,8.64122,8.6628,8.6862,8.70837,8.73237,8.7567,8.77979,8.80472,
- 8.82843,8.85402,8.87835,8.9046,8.9296,8.9565,8.98218,9.0082,9.03616,9.06287,
- 9.09152,9.11895,9.14834,9.17652,9.20503,9.23557,9.26487,9.29454,9.32626,9.35674,
- 9.38762,9.42055,9.45226,9.4844,9.51695,9.55162,9.58503,9.61893,9.65324,4.38929,
- 4.38536,4.36058,4.3365,4.3131,4.29036,4.26827,4.24682,4.22598,4.20576,4.18612,
- 4.1944,4.17582,4.15708,4.13859,4.12032,4.10229,4.08449,4.06691,4.04955,4.03241,
- 4.01549,3.99916,3.98265,3.96634,3.95024,3.93434,3.91901,3.90349,3.88817,3.87304,
- 3.85845,3.84368,3.82909,3.81504,3.80081,3.78674,3.7732,3.75948,3.74591,3.73287,
- 3.71963,3.7069,3.69398,3.68156,3.66894,3.65647,3.6445,3.63233,3.62065,3.60876,
- 3.59736,3.58576,3.58265,3.61553,3.62696,3.63867,3.67347,3.68596,3.72229,3.7356,
- 3.77355,3.78772,3.80219,3.84244,3.85785,3.89993,3.9163,3.93303,3.97774,3.99551,
- 4.01367,4.06121,4.0805,4.10019,4.15081,4.17174,4.19309,13.8062,13.7714,13.7384,
- 13.7075,13.8936,13.9735,14.0549,14.1382,14.3407,15.8017,15.9009,16.002,16.1054,
- 16.3519,16.462,16.5744,16.6893,16.9594,17.0819,17.2072,17.3352,17.4661,17.6,
- 8.14878,8.1334,8.11823,8.10324,8.08848,8.07391,8.0588,8.04465,8.03069,8.01693,
- 8.00338,7.99,7.97684,7.96312,7.95032,7.93773,7.92533,7.91309,7.90106,7.88922,7.87755,
- 7.86606,7.85476,7.84289,7.83195,7.82116,7.81058,7.80017,7.78993,7.77984,7.76995,
- 7.76021,7.75066,7.74128,7.73204,7.76034,7.99805,8.11853,8.24311,8.37202,12.3718,
- 12.3587,12.346,12.3336,12.3213,12.3094,12.2976,12.2862,12.275,12.264,12.2534,12.2429,
- 12.2327,12.2228,12.213,12.2036,12.1944,12.1854,12.1766,12.3577,12.667,16.7608,16.7501,
- 16.7398,16.7297,16.7201,16.7106,16.7015,16.6929,16.6844,16.9488,20.069,20.0619,20.0552,
- 20.0489,20.043,20.0374,20.0323,20.0275,20.0231,20.0191,20.0155,20.0122,20.0094,20.0069,
- 20.0048,20.0031,20.0018,20.0008,20.0002,20.0001,20.0002,20.0008,20.0018,20.0031,20.0048,
- 20.0069,20.0094,20.0122,20.0155,20.0191,20.0231,20.0275,20.0323,20.0374,20.043,20.0489,
- 20.0552,20.0619,20.069,20.0764,20.0843,20.0926,20.1012,20.1102,20.1196,20.1294,20.1397,
- 20.1502,20.1612,20.1726,20.1844,20.1966,20.2092,20.2222,20.2356,20.2494,20.2636,20.2782,
- 20.2932,20.3086,20.3244,20.3407,20.3573,20.3744,20.3919,20.4098,20.4281,20.4469,20.466,
- 20.4856,20.5057,20.5261,20.547,20.5684,20.5901,20.6123,20.635,20.6581,20.6816,20.7056,
- 20.73,20.7549,20.7802,20.806,20.8323,20.859,20.8862,20.9139,20.942,20.9706,20.9997,
- 21.0293,21.0594,21.0899,21.1209,21.1525,21.1845,21.217,21.2501,21.2836,21.3177,21.3522,
- 21.3873,21.423,21.4591,21.4958,21.533,21.5707,21.6091,21.6479,21.6873,21.7273,21.7678,
- 21.8089,21.8505,21.8928,21.9356,21.979,22.023,22.0676,22.1128,22.1586,22.2051,22.2521,
- 22.2998,22.3481,22.397,22.4466,22.4968,22.5477,22.5992,22.6515,22.7043,22.7579,22.8122,
- 22.8671,22.9228,22.9792,23.0363,23.0941,23.1526,23.2119,23.2719,23.3327,23.3943,23.4566,
- 23.5197,23.5836,23.6483,23.7138,23.7802,23.8473,23.9153,23.9842,24.0539,24.1244,24.1959,
- 24.2682,24.3414,24.4156,24.4906,24.5666,24.6435,24.7214,24.8003,24.8801,24.9609,25.0428,
- 25.1256,25.2095,25.2944,25.3804,25.4675,25.5556,25.6449,25.7353,25.8268,25.9194,26.0132,
- 26.1082,26.2044,26.3018,26.4004,26.5003,26.6015,26.7039,26.8077,26.9127,27.0191,27.1269,
- 27.2361,27.3466,27.4586,27.572,27.6869,27.8033,27.9213,28.0407,28.1617;
-
- //variable declarations and inits
- Eigen::VectorXs device_pose(6);
- device_pose << 0,0,0,0,0,0; //origin, no rotation
- TimeStamp time_stamp;
- time_stamp.setToNow();
- std::list<Eigen::Vector4s, Eigen::aligned_allocator<Eigen::Vector4s> > corner_list;
-
- //Create Device objects
- //SensorLaser2D device(device_pose, ranges.size(), M_PI, 0.2, 30.0, 0.01);
- SensorLaser2D device(device_pose, -M_PI/2, M_PI/2, M_PI/ranges.size(), 0.2, 30.0, 0.01);
- device.printSensorParameters();
-
- //init a noise generator
- std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise
-
- //Create a Capture object
- CaptureLaser2D capture(time_stamp, &device, ranges);
-
- //add noise to measurements
- //TODO
-
- //do things with the measurements
- clock_t t1, t2;
- t1=clock();
- capture.extractCorners(corner_list);
- t2=clock();
- std::cout << "seconds = " << ((double)t2-t1)/CLOCKS_PER_SEC << std::endl;
- capture.createFeatures(corner_list);
- capture.printSelf();
-
- //print corners
- std::cout << "CORNER LIST" << std::endl;
- for (auto corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it ++ )
- {
- std::cout << corner_it->x() << " , " << corner_it->y() << std::endl;
- }
-
- std::cout << "========================================================" << std::endl;
- std::cout << std::endl << "End CaptureLaser2D class test" << std::endl;
- return 0;
-}
-
diff --git a/demos/demo_ceres_2_lasers.cpp b/demos/demo_ceres_2_lasers.cpp
deleted file mode 100644
index dce55b16f3fd730ddaf8b0832736bb79bd30bccb..0000000000000000000000000000000000000000
--- a/demos/demo_ceres_2_lasers.cpp
+++ /dev/null
@@ -1,420 +0,0 @@
-//std includes
-#include "core/sensor/sensor_GPS_fix.h"
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "core/problem/problem.h"
-#include "core/processor/processor_tracker_landmark_corner.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// laserscanutils
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/laser_scan.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-#include "core/capture/capture_pose.h"
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-namespace wolf {
-class FaramoticsRobot
-{
- public:
-
- Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
- vector < Cpose3d > devicePoses;
- vector<float> scan1, scan2;
- string modelFileName;
- CrangeScan2D* myScanner;
- CdynamicSceneRender* myRender;
- Eigen::Vector3s ground_truth_pose_;
- Eigen::Vector4s laser_1_pose_, laser_2_pose_;
-
- FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
- modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
- laser_1_pose_(_laser_1_pose),
- laser_2_pose_(_laser_2_pose)
- {
- devicePose.setPose(2, 8, 0.2, 0, 0, 0);
- viewPoint.setPose(devicePose);
- viewPoint.moveForward(10);
- viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
- viewPoint.moveForward(-15);
- //glut initialization
- faramotics::initGLUT(argc, argv);
- //create a viewer for the 3D model and scan points
- myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
- myRender->loadAssimpModel(modelFileName, true); //with wireframe
- //create scanner and load 3D model
- myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
- myScanner->loadAssimpModel(modelFileName);
- }
-
- //function travel around
- Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
- {
- if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
- else if (ii <= 220) { displacement_ = 0; rotation_ =-1.8 * M_PI / 180; }
- else if (ii <= 310) { displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 487) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 600) { displacement_ = 0.2; rotation_ = 0; }
- else if (ii <= 700) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 780) { displacement_ = 0; rotation_ =-1.0 * M_PI / 180; }
- else { displacement_ = 0.3; rotation_ = 0; }
-
- devicePose.moveForward(displacement_);
- devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll());
-
- // laser 1
- laser1Pose.setPose(devicePose);
- laser1Pose.moveForward(laser_1_pose_(0));
- // laser 2
- laser2Pose.setPose(devicePose);
- laser2Pose.moveForward(laser_2_pose_(0));
- laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-
- devicePoses.push_back(devicePose);
-
- ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
- return ground_truth_pose_;
- }
-
- ~FaramoticsRobot()
- {
- std::cout << "deleting render and scanner.." << std::endl;
- delete myRender;
- delete myScanner;
- std::cout << "deleted!" << std::endl;
- }
-
- //compute scans
- vector<float> computeScan(const int scan_id)
- {
- if (scan_id == 1)
- {
- scan1.clear();
- myScanner->computeScan(laser1Pose, scan1);
- return scan1;
- }
- else
- {
- scan2.clear();
- myScanner->computeScan(laser2Pose, scan2);
- return scan2;
- }
- }
-
- void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
- {
- // detected corners
- //std::cout << " drawCorners: " << feature_list.size() << std::endl;
- std::vector<double> corner_vector;
- corner_vector.reserve(2*feature_list.size());
- for (auto corner : feature_list)
- {
- //std::cout << " corner " << corner->id() << std::endl;
- corner_vector.push_back(corner->getMeasurement(0));
- corner_vector.push_back(corner->getMeasurement(1));
- }
- myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector);
-
- // landmarks
- //std::cout << " drawLandmarks: " << landmark_list.size() << std::endl;
- std::vector<double> landmark_vector;
- landmark_vector.reserve(3*landmark_list.size());
- for (auto landmark : landmark_list)
- {
- Scalar* position_ptr = landmark->getP()->get();
- landmark_vector.push_back(*position_ptr); //x
- landmark_vector.push_back(*(position_ptr + 1)); //y
- landmark_vector.push_back(0.2); //z
- }
- myRender->drawLandmarks(landmark_vector);
-
- // draw localization and sensors
- estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0);
- estimated_laser_1_pose.setPose(estimated_vehicle_pose);
- estimated_laser_1_pose.moveForward(laser_1_pose_(0));
- estimated_laser_2_pose.setPose(estimated_vehicle_pose);
- estimated_laser_2_pose.moveForward(laser_2_pose_(0));
- estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
- myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
- //Set view point and render the scene
- //locate visualization view point, somewhere behind the device
- // viewPoint.setPose(devicePose);
- // viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
- // viewPoint.moveForward(-5);
- myRender->setViewPoint(viewPoint);
- myRender->drawPoseAxis(devicePose);
- myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
- myRender->render();
- }
-};
-}
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << "\n============================================================\n";
- std::cout << "========== 2D Robot with odometry and 2 LIDARs =============\n";
-
- // USER INPUT ============================================================================================
- if (argc != 2 || atoi(argv[1]) < 1 )
- {
- std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
- std::cout << " - NI is the number of iterations (NI > 0)" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
- // INITIALIZATION ============================================================================================
- //init random generators
- Scalar odom_std_factor = 0.5;
- Scalar gps_std = 1;
- std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
- std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
-
- //variables
- Eigen::Vector2s odom_data;
- Eigen::Vector2s gps_fix_reading;
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::Vector3s ground_truth_pose; //last true pose
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
- clock_t t1, t2;
- Scalar dt = 0.05;
- TimeStamp ts(0);
-
- // Wolf Tree initialization
- Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
- Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
- Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
- laser_1_pose << 1.2, 0, 0, 0; //laser 1
- laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
-
- Problem problem(FRM_PO_2D);
- SensorOdom2D* odom_sensor = new SensorOdom2D(std::make_shared<StateBlock>(odom_pose.head(2), true), std::make_shared<StateBlock>(odom_pose.tail(1), true), odom_std_factor, odom_std_factor);
- SensorGPSFix* gps_sensor = new SensorGPSFix(std::make_shared<StateBlock>(gps_pose.head(2), true), std::make_shared<StateBlock>(gps_pose.tail(1), true), gps_std);
- SensorLaser2D* laser_1_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_1_pose.head(2), true), std::make_shared<StateBlock>(laser_1_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- SensorLaser2D* laser_2_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_2_pose.head(2), true), std::make_shared<StateBlock>(laser_2_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- ProcessorTrackerLandmarkCorner* laser_1_processor = new ProcessorTrackerLandmarkCorner(laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2}), 3, 10);
- ProcessorTrackerLandmarkCorner* laser_2_processor = new ProcessorTrackerLandmarkCorner(laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2}), 3, 10);
- ProcessorOdom2D* odom_processor = new ProcessorOdom2D(1,1,100);
- odom_sensor->addProcessor(odom_processor);
- laser_1_sensor->addProcessor(laser_1_processor);
- laser_2_sensor->addProcessor(laser_2_processor);
- problem.addSensor(odom_sensor);
- problem.addSensor(gps_sensor);
- problem.addSensor(laser_1_sensor);
- problem.addSensor(laser_2_sensor);
- problem.setProcessorMotion(odom_processor);
-
- CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
-
- // Simulated robot
- FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
-
- // Initial pose
- ground_truth_pose << 2, 8, 0;
- ground_truth.head(3) = ground_truth_pose;
- odom_trajectory.head(3) = ground_truth_pose;
-
- // Origin Key Frame
- FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
-
- // Prior covariance
- CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
- origin_frame->addCapture(initial_covariance);
- initial_covariance->process();
-
- odom_processor->setOrigin(origin_frame);
-
- // Ceres wrapper
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- google::InitGoogleLogging(argv[0]);
-
- CeresManager ceres_manager(&problem, ceres_options);
- std::ofstream log_file, landmark_file; //output file
-
- //std::cout << "START TRAJECTORY..." << std::endl;
- // START TRAJECTORY ============================================================================================
- for (unsigned int step = 1; step < n_execution; step++)
- {
- // timestamp
- ts = TimeStamp(step*dt);
-
- //get init time
- t2 = clock();
-
- // ROBOT MOVEMENT ---------------------------
- //std::cout << "ROBOT MOVEMENT..." << std::endl;
- // moves the device position
- t1 = clock();
- ground_truth_pose = robot.motionCampus(step, odom_data(0), odom_data(1));
- ground_truth.segment(step * 3, 3) = ground_truth_pose;
-
- // ODOMETRY DATA -------------------------------------
- // noisy odometry
- odom_data(0) += distribution_odom(generator) * (odom_data(0) == 0 ? 1e-6 : odom_data(0));
- odom_data(1) += distribution_odom(generator) * (odom_data(1) == 0 ? 1e-6 : odom_data(1));
- // process odometry
- odom_capture->setTimeStamp(TimeStamp(ts));
- odom_capture->setData(odom_data);
- odom_processor->process(odom_capture);
- // odometry integration
- odom_trajectory.segment(step * 3, 3) = problem.getCurrentState();
-
- // LIDAR DATA ---------------------------
- if (step % 3 == 0)
- {
- std::cout << "--PROCESS LIDAR 1 DATA..." << laser_1_sensor->id() << std::endl;
- laser_1_processor->process(new CaptureLaser2D(ts, laser_1_sensor, robot.computeScan(1)));
- std::cout << "--PROCESS LIDAR 2 DATA..." << laser_2_sensor->id() << std::endl;
- laser_2_processor->process(new CaptureLaser2D(ts, laser_2_sensor, robot.computeScan(2)));
- }
-
- // GPS DATA ---------------------------
- if (step % 5 == 0)
- {
- // compute GPS
- gps_fix_reading = ground_truth_pose.head<2>();
- gps_fix_reading(0) += distribution_gps(generator);
- gps_fix_reading(1) += distribution_gps(generator);
- // process data
- //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
- }
- mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // SOLVE OPTIMIZATION ---------------------------
- //std::cout << "SOLVING..." << std::endl;
- t1 = clock();
- ceres::Solver::Summary summary = ceres_manager.solve();
- //std::cout << summary.FullReport() << std::endl;
- mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // COMPUTE COVARIANCES ---------------------------
- //std::cout << "COMPUTING COVARIANCES..." << std::endl;
- t1 = clock();
- ceres_manager.computeCovariances();
- mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // DRAWING STUFF ---------------------------
- //std::cout << "RENDERING..." << std::endl;
- t1 = clock();
- if (step % 3 == 0)
- robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
- //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
- mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // TIME MANAGEMENT ---------------------------
- double total_t = ((double) clock() - t2) / CLOCKS_PER_SEC;
- mean_times(6) += total_t;
- if (total_t < 0.5)
- usleep(500000 - 1e6 * total_t);
-
-// std::cout << "\nTree after step..." << std::endl;
- }
-
- // DISPLAY RESULTS ============================================================================================
- mean_times /= n_execution;
- std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
- std::cout << " data generation: " << mean_times(0) << std::endl;
- std::cout << " wolf managing: " << mean_times(1) << std::endl;
- std::cout << " ceres managing: " << mean_times(2) << std::endl;
- std::cout << " ceres optimization: " << mean_times(3) << std::endl;
- std::cout << " ceres covariance: " << mean_times(4) << std::endl;
- std::cout << " results drawing: " << mean_times(5) << std::endl;
- std::cout << " loop time: " << mean_times(6) << std::endl;
-
- // std::cout << "\nTree before deleting..." << std::endl;
-
- // Draw Final result -------------------------
- robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
-
- // Print Final result in a file -------------------------
- // Vehicle poses
- int i = 0;
- Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
- for (auto frame : *(problem.getTrajectory()->getFrameList()))
- {
- state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
- i += 3;
- }
-
- // Landmarks
- i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
- for (auto landmark : *(problem.getMap()->getLandmarkList()))
- {
- landmarks.segment(i, 2) = landmark->getP()->getVector();
- i += 2;
- }
-
- // Print log files
- std::string filepath = getenv("HOME") + std::string("/Desktop/log_file_2.txt");
- log_file.open(filepath, std::ofstream::out); //open log file
-
- if (log_file.is_open())
- {
- log_file << 0 << std::endl;
- for (unsigned int ii = 0; ii < n_execution; ii++)
- log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
- log_file.close(); //close log file
- std::cout << std::endl << "Result file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-
- std::string filepath2 = getenv("HOME") + std::string("/Desktop/landmarks_file_2.txt");
- landmark_file.open(filepath2, std::ofstream::out); //open log file
-
- if (landmark_file.is_open())
- {
- for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
- landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
- landmark_file.close(); //close log file
- std::cout << std::endl << "Landmark file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-
- std::cout << "Press any key for ending... " << std::endl << std::endl;
- std::getchar();
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_ceres_2_lasers_polylines.cpp b/demos/demo_ceres_2_lasers_polylines.cpp
deleted file mode 100644
index d79bbfaf88853ee8ccec48f5c172ecce2b83e940..0000000000000000000000000000000000000000
--- a/demos/demo_ceres_2_lasers_polylines.cpp
+++ /dev/null
@@ -1,377 +0,0 @@
-//std includes
-#include "core/sensor/sensor_GPS_fix.h"
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "core/problem/problem.h"
-#include "core/processor/processor_tracker_landmark_polyline.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// laserscanutils
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/laser_scan.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-#include "core/capture/capture_pose.h"
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-namespace wolf {
-class FaramoticsRobot
-{
- public:
-
- Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
- vector < Cpose3d > devicePoses;
- vector<float> scan1, scan2;
- string modelFileName;
- CrangeScan2D* myScanner;
- CdynamicSceneRender* myRender;
- Eigen::Vector3s ground_truth_pose_;
- Eigen::Vector4s laser_1_pose_, laser_2_pose_;
-
- FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
- modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
- laser_1_pose_(_laser_1_pose),
- laser_2_pose_(_laser_2_pose)
- {
- devicePose.setPose(2, 8, 0.2, 0, 0, 0);
- viewPoint.setPose(devicePose);
- viewPoint.moveForward(10);
- viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
- viewPoint.moveForward(-15);
- //glut initialization
- faramotics::initGLUT(argc, argv);
- //create a viewer for the 3D model and scan points
- myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
- myRender->loadAssimpModel(modelFileName, true); //with wireframe
- //create scanner and load 3D model
- myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
- myScanner->loadAssimpModel(modelFileName);
- }
-
- //function travel around
- Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
- {
- if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
- else if (ii <= 220) { displacement_ = 0; rotation_ =-1.8 * M_PI / 180; }
- else if (ii <= 310) { displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 487) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 600) { displacement_ = 0.2; rotation_ = 0; }
- else if (ii <= 700) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 780) { displacement_ = 0; rotation_ =-1.0 * M_PI / 180; }
- else { displacement_ = 0.3; rotation_ = 0; }
-
- devicePose.moveForward(displacement_);
- devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll());
-
- // laser 1
- laser1Pose.setPose(devicePose);
- laser1Pose.moveForward(laser_1_pose_(0));
- // laser 2
- laser2Pose.setPose(devicePose);
- laser2Pose.moveForward(laser_2_pose_(0));
- laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-
- devicePoses.push_back(devicePose);
-
- ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
- return ground_truth_pose_;
- }
-
- ~FaramoticsRobot()
- {
- std::cout << "deleting render and scanner.." << std::endl;
- delete myRender;
- delete myScanner;
- std::cout << "deleted!" << std::endl;
- }
-
- //compute scans
- vector<float> computeScan(const int scan_id)
- {
- if (scan_id == 1)
- {
- scan1.clear();
- myScanner->computeScan(laser1Pose, scan1);
- return scan1;
- }
- else
- {
- scan2.clear();
- myScanner->computeScan(laser2Pose, scan2);
- return scan2;
- }
- }
-
- void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
- {
- // detected corners
- std::cout << " drawCorners: " << feature_list.size() << std::endl;
- std::vector<double> corner_vector;
- corner_vector.reserve(2*feature_list.size());
- for (auto corner : feature_list)
- {
- std::cout << " corner " << corner->id() << std::endl;
- corner_vector.push_back(corner->getMeasurement(0));
- corner_vector.push_back(corner->getMeasurement(1));
- }
- myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector);
-
- // landmarks
- std::cout << " drawLandmarks: " << landmark_list.size() << std::endl;
- std::vector<double> landmark_vector;
- landmark_vector.reserve(3*landmark_list.size());
- for (auto landmark : landmark_list)
- {
- Scalar* position_ptr = landmark->getP()->get();
- landmark_vector.push_back(*position_ptr); //x
- landmark_vector.push_back(*(position_ptr + 1)); //y
- landmark_vector.push_back(0.2); //z
- }
- myRender->drawLandmarks(landmark_vector);
-
- // draw localization and sensors
- estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0);
- estimated_laser_1_pose.setPose(estimated_vehicle_pose);
- estimated_laser_1_pose.moveForward(laser_1_pose_(0));
- estimated_laser_2_pose.setPose(estimated_vehicle_pose);
- estimated_laser_2_pose.moveForward(laser_2_pose_(0));
- estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
- myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
- //Set view point and render the scene
- //locate visualization view point, somewhere behind the device
- // viewPoint.setPose(devicePose);
- // viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
- // viewPoint.moveForward(-5);
- myRender->setViewPoint(viewPoint);
- myRender->drawPoseAxis(devicePose);
- myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
- myRender->render();
- }
-};
-}
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << "\n============================================================\n";
- std::cout << "========== 2D Robot with odometry and 2 LIDARs =============\n";
-
- // USER INPUT ============================================================================================
- if (argc != 2 || atoi(argv[1]) < 1 )
- {
- std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
- std::cout << " - NI is the number of iterations (NI > 0)" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
- // INITIALIZATION ============================================================================================
- //init random generators
- Scalar odom_std_factor = 0.5;
- Scalar gps_std = 1;
- std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
- std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
-
- //variables
- Eigen::Vector2s odom_data;
- Eigen::Vector2s gps_fix_reading;
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::Vector3s ground_truth_pose; //last true pose
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
- clock_t t1, t2;
- Scalar dt = 0.05;
- TimeStamp ts(0);
-
- // Wolf Tree initialization
- Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
- Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
- Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
- laser_1_pose << 1.2, 0, 0, 0; //laser 1
- laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
-
- Problem problem(FRM_PO_2D);
- SensorOdom2D* odom_sensor = new SensorOdom2D(std::make_shared<StateBlock>(odom_pose.head(2), true), std::make_shared<StateBlock>(odom_pose.tail(1), true), odom_std_factor, odom_std_factor);
- SensorGPSFix* gps_sensor = new SensorGPSFix(std::make_shared<StateBlock>(gps_pose.head(2), true), std::make_shared<StateBlock>(gps_pose.tail(1), true), gps_std);
- SensorLaser2D* laser_1_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_1_pose.head(2), true), std::make_shared<StateBlock>(laser_1_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- SensorLaser2D* laser_2_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_2_pose.head(2), true), std::make_shared<StateBlock>(laser_2_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-
- wolf::ProcessorParamsPolyline laser_processor_params;
- laser_processor_params.line_finder_params = laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2});
- laser_processor_params.new_features_th = 3;
- laser_processor_params.loop_frames_th = 10;
- laser_processor_params.time_tolerance = 0.1;
- laser_processor_params.position_error_th = 0.5;
- ProcessorTrackerLandmarkPolyline* laser_1_processor = new ProcessorTrackerLandmarkPolyline(laser_processor_params);
- ProcessorTrackerLandmarkPolyline* laser_2_processor = new ProcessorTrackerLandmarkPolyline(laser_processor_params);
- ProcessorOdom2D* odom_processor = new ProcessorOdom2D(1,1,100);
- odom_sensor->addProcessor(odom_processor);
- laser_1_sensor->addProcessor(laser_1_processor);
- laser_2_sensor->addProcessor(laser_2_processor);
- problem.addSensor(odom_sensor);
- problem.addSensor(gps_sensor);
- problem.addSensor(laser_1_sensor);
- problem.addSensor(laser_2_sensor);
- problem.setProcessorMotion(odom_processor);
-
- CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
-
- // Simulated robot
- FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
-
- // Initial pose
- ground_truth_pose << 2, 8, 0;
- ground_truth.head(3) = ground_truth_pose;
- odom_trajectory.head(3) = ground_truth_pose;
-
- // Origin Key Frame
- FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
-
- // Prior covariance
- CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
- origin_frame->addCapture(initial_covariance);
- initial_covariance->process();
-
- odom_processor->setOrigin(origin_frame);
-
- // Ceres wrapper
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- google::InitGoogleLogging(argv[0]);
-
- CeresManager ceres_manager(&problem, ceres_options);
- std::ofstream log_file, landmark_file; //output file
-
- //std::cout << "START TRAJECTORY..." << std::endl;
- // START TRAJECTORY ============================================================================================
- for (unsigned int step = 1; step < n_execution; step++)
- {
- // timestamp
- ts = TimeStamp(step*dt);
-
- //get init time
- t2 = clock();
-
- // ROBOT MOVEMENT ---------------------------
- //std::cout << "ROBOT MOVEMENT..." << std::endl;
- // moves the device position
- t1 = clock();
- ground_truth_pose = robot.motionCampus(step, odom_data(0), odom_data(1));
- ground_truth.segment(step * 3, 3) = ground_truth_pose;
-
- // ODOMETRY DATA -------------------------------------
- // noisy odometry
- odom_data(0) += distribution_odom(generator) * (odom_data(0) == 0 ? 1e-6 : odom_data(0));
- odom_data(1) += distribution_odom(generator) * (odom_data(1) == 0 ? 1e-6 : odom_data(1));
- // process odometry
- odom_capture->setTimeStamp(TimeStamp(ts));
- odom_capture->setData(odom_data);
- odom_processor->process(odom_capture);
- // odometry integration
- odom_trajectory.segment(step * 3, 3) = problem.getCurrentState();
-
- // LIDAR DATA ---------------------------
- if (step % 3 == 0)
- {
- std::cout << "--PROCESS LIDAR 1 DATA..." << laser_1_sensor->id() << std::endl;
- laser_1_processor->process(new CaptureLaser2D(ts, laser_1_sensor, robot.computeScan(1)));
- std::cout << "--PROCESS LIDAR 2 DATA..." << laser_2_sensor->id() << std::endl;
- laser_2_processor->process(new CaptureLaser2D(ts, laser_2_sensor, robot.computeScan(2)));
- }
-
- // GPS DATA ---------------------------
- if (step % 5 == 0)
- {
- // compute GPS
- gps_fix_reading = ground_truth_pose.head<2>();
- gps_fix_reading(0) += distribution_gps(generator);
- gps_fix_reading(1) += distribution_gps(generator);
- // process data
- //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
- }
- mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // SOLVE OPTIMIZATION ---------------------------
- std::cout << "SOLVING..." << std::endl;
- t1 = clock();
- std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
- std::cout << summary << std::endl;
- mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // COMPUTE COVARIANCES ---------------------------
- std::cout << "COMPUTING COVARIANCES..." << std::endl;
- t1 = clock();
- //ceres_manager.computeCovariances();
- mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // DRAWING STUFF ---------------------------
- std::cout << "RENDERING..." << std::endl;
- t1 = clock();
-// if (step % 3 == 0)
-// robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
- //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
- mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // TIME MANAGEMENT ---------------------------
- double total_t = ((double) clock() - t2) / CLOCKS_PER_SEC;
- mean_times(6) += total_t;
- if (total_t < 0.5)
- usleep(500000 - 1e6 * total_t);
-
-// std::cout << "\nTree after step..." << std::endl;
- }
-
- // DISPLAY RESULTS ============================================================================================
- mean_times /= n_execution;
- std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
- std::cout << " data generation: " << mean_times(0) << std::endl;
- std::cout << " wolf managing: " << mean_times(1) << std::endl;
- std::cout << " ceres managing: " << mean_times(2) << std::endl;
- std::cout << " ceres optimization: " << mean_times(3) << std::endl;
- std::cout << " ceres covariance: " << mean_times(4) << std::endl;
- std::cout << " results drawing: " << mean_times(5) << std::endl;
- std::cout << " loop time: " << mean_times(6) << std::endl;
-
- std::cout << "\nTree before deleting..." << std::endl;
-
- std::cout << "Press any key for ending... " << std::endl << std::endl;
- std::getchar();
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_diff_drive.cpp b/demos/demo_diff_drive.cpp
deleted file mode 100644
index 5585fe035ca81d0b7ae8442a03cb3ff7c790680a..0000000000000000000000000000000000000000
--- a/demos/demo_diff_drive.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-/**
- * \file test_diff_drive.cpp
- *
- * Created on: Oct 26, 2017
- * \author: Jeremie Deray
- */
-
-//Wolf
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_diff_drive.h"
-#include "core/capture/capture_wheel_joint_position.h"
-#include "core/processor/processor_diff_drive.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-void getOdom2DData(std::ifstream& _stream, wolf::Scalar& _stamp, Eigen::Vector2s& _data)
-{
- /*
- * Data are logged as follows :
- *
- * header:
- * seq: xxx
- * stamp:
- * secs: xxx
- * nsecs: xxx
- * frame_id: ''
- * twist:
- * linear:
- * x: 0.0
- * y: 0.0
- * z: 0.0
- * angular:
- * x: 0.0
- * y: 0.0
- * z: 0.0
- * ---
- */
-
- std::string dummy;
-
- getline(_stream, dummy); // header:
- getline(_stream, dummy); // seq: xxx
- getline(_stream, dummy); // stamp:
- getline(_stream, dummy); // secs: xxx
-
- // Find secs
- std::string sub("secs: ");
- std::string::size_type i = dummy.find(sub);
- dummy.erase(i, sub.length());
-
- _stamp = std::stod(dummy);
-
- // Find nsecs
- getline(_stream, dummy); // nsecs: xxx
- sub = "nsecs: ";
- i = dummy.find(sub);
- dummy.erase(i, sub.length());
-
- _stamp += std::stod(dummy) * wolf::Scalar(1e-9);
-
- getline(_stream, dummy); // frame_id: ''
- getline(_stream, dummy); // twist:
- getline(_stream, dummy); // linear:
- getline(_stream, dummy); // x: 0.0
-
- sub = "x: ";
- i = dummy.find(sub);
- dummy.erase(i, sub.length());
-
- _data(0) = std::stod(dummy);
-
- getline(_stream, dummy); // y: 0.0
- getline(_stream, dummy); // z: 0.0
- getline(_stream, dummy); // angular:
- getline(_stream, dummy); // x: 0.0
- getline(_stream, dummy); // y: 0.0
- getline(_stream, dummy); // z: 0.0
-
- sub = "z: ";
- i = dummy.find(sub);
- dummy.erase(i, sub.length());
-
- _data(1) = std::stod(dummy);
-
- getline(_stream, dummy); // ---
-}
-
-void readWheelData(std::ifstream &_stream, Eigen::Vector2s &_data)
-{
- /*
- * left_wheel_joint_actual_position: [x]
- * right_wheel_joint_actual_position: [x]
- * ---
- */
-
- std::string dummy;
- std::string l_brac("[");
- std::string r_brac("]");
-
- getline(_stream, dummy);
-
- unsigned first = dummy.find(l_brac);
- unsigned last = dummy.find(r_brac);
-
- //std::cout << "READING : " << dummy.substr(first+1, last-first-1) << std::endl;
-
- _data(0) = std::stod(dummy.substr(first+1, last-first-1));
-
- getline(_stream, dummy);
-
- first = dummy.find(l_brac);
- last = dummy.find(r_brac);
-
- //std::cout << "READING : " << dummy.substr(first+1, last-first-1) << std::endl;
-
- _data(1) = std::stod(dummy.substr(first+1 , last-first-1));
-
- getline(_stream, dummy);
-}
-
-bool WHEEL_DATA = true;
-bool VERBOSE = false;
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- WOLF_INFO("==================== diff drive test ======================");
-
- //load files containing data
- std::ifstream data_file;
- const char * filename;
-
- if (argc < 2)
- {
- WOLF_ERROR("Missing input argument! :"
- " needs 2 arguments (path to data file & data type "
- "- velocities or wheel positions).");
- return EXIT_FAILURE;
- }
- else
- {
- filename = argv[1];
- if (argc >= 3) WHEEL_DATA = std::stoi(argv[2]);
-
- data_file.open(filename);
-
- WOLF_INFO("Data file: ", filename);
-
- if (!data_file.is_open())
- {
- WOLF_ERROR("Failed to open data files... Exiting");
- return EXIT_FAILURE;
- }
- }
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
-
- const std::string sensor_name("Main Odometer");
- Eigen::VectorXs extrinsics(3);
- extrinsics << 0, 0, 0;
-
- IntrinsicsBasePtr intrinsics = std::make_shared<IntrinsicsDiffDrive>();
-
- IntrinsicsDiffDrivePtr intrinsics_diff_drive =
- std::static_pointer_cast<IntrinsicsDiffDrive>(intrinsics);
-
- intrinsics_diff_drive->left_radius_ = 0.1;
- intrinsics_diff_drive->right_radius_ = 0.1;
- intrinsics_diff_drive->separation_ = 0.3517;
-
- intrinsics_diff_drive->model_ = wolf::DiffDriveModel::Three_Factor_Model;
- intrinsics_diff_drive->factors_ = Eigen::Vector3s(1,1,1);
-
- intrinsics_diff_drive->left_resolution_ = 0.0001653; // [rad]
- intrinsics_diff_drive->right_resolution_ = 0.0001653; // [rad]
-
- intrinsics_diff_drive->left_gain_ = 0.01;
- intrinsics_diff_drive->right_gain_ = 0.01;
-
- // Time and data variables
- TimeStamp t;
- Scalar stamp_secs(0);
-// Scalar period_secs(0.010); //100Hz
- Scalar period_secs(0.020); //50Hz
- Eigen::Vector2s data_; data_ << 0,0;
-
- const auto scalar_max = std::numeric_limits<Scalar>::max();
-
- ProcessorParamsDiffDrivePtr processor_params = std::make_shared<ProcessorParamsDiffDrive>();
- processor_params->time_tolerance = period_secs/2;
- processor_params->angle_turned = scalar_max;
- processor_params->dist_traveled = scalar_max;
- processor_params->max_time_span = scalar_max;
- processor_params->max_buff_length = 999;
- processor_params->unmeasured_perturbation_std = 0.0001;
-
- SensorBasePtr sensor_ptr =
- wolf_problem_ptr_->installSensor("DIFF DRIVE", sensor_name, extrinsics, intrinsics);
-
- WOLF_INFO("Sensor 'DIFF DRIVE' installed.");
-
- auto diff_drive_sensor_ptr = std::static_pointer_cast<SensorDiffDrive>(sensor_ptr);
-
- wolf_problem_ptr_->installProcessor("DIFF DRIVE", "Diffential Drive processor", sensor_ptr, processor_params);
-
- WOLF_INFO("Processor 'DIFF DRIVE' installed.");
-
- // Get initial wheel data
- if (WHEEL_DATA)
- readWheelData(data_file, data_);
- else
- getOdom2DData(data_file, stamp_secs, data_);
-
- t.set(stamp_secs);
- auto processor_diff_drive_ptr =
- std::static_pointer_cast<ProcessorDiffDrive>(wolf_problem_ptr_->getProcessorMotion());
- processor_diff_drive_ptr->setTimeTolerance(period_secs/2); // overwrite time tolerance based on new evidence
-
- // Set the origin
- // Create one capture to store the Odometry data.
- std::shared_ptr<CaptureWheelJointPosition> data_ptr =
- std::make_shared<CaptureWheelJointPosition>(t, sensor_ptr, data_, nullptr);
-
- WOLF_INFO("Process first capture.");
-
- diff_drive_sensor_ptr->process(data_ptr);
-
- // main loop
- clock_t begin = clock();
-
- while (!data_file.eof())
- {
- // read new data
- if (WHEEL_DATA)
- {
- readWheelData(data_file, data_);
- stamp_secs += period_secs;
- }
- else
- getOdom2DData(data_file, stamp_secs, data_);
-
- t.set(stamp_secs);
-
- data_ptr = std::make_shared<CaptureWheelJointPosition>(t, sensor_ptr, data_, nullptr);
-
- // process data in capture
- diff_drive_sensor_ptr->process(data_ptr);
-
- WOLF_INFO_COND(VERBOSE, "At stamp ", stamp_secs,
- " state ", processor_diff_drive_ptr->getCurrentState().transpose());
- }
-
- data_file.close();
-
- double elapsed_secs = double(clock() - begin) / CLOCKS_PER_SEC;
-
- // Final state
- WOLF_INFO("----------------------------------------- "
- "Integration results "
- "-----------------------------------------");
-
- WOLF_INFO("Integrated delta: " , /* std::fixed , std::setprecision(3),*/
- wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose());
- WOLF_INFO("Integrated state: " , /*std::fixed , std::setprecision(3),*/
- wolf_problem_ptr_->getProcessorMotion()->getCurrentState().transpose());
- WOLF_INFO("Integrated std : " , /*std::fixed , std::setprecision(3),*/
- (wolf_problem_ptr_->getProcessorMotion()->getMotion().
- delta_integr_cov_.diagonal().transpose()).array().sqrt());
-
- // Print statistics
- TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-
- double N = (double)wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
-
- WOLF_INFO("t0 : " , t0.get() , " secs");
- WOLF_INFO("tf : " , tf.get() , " secs");
- WOLF_INFO("duration : " , tf-t0 , " secs");
- WOLF_INFO("N samples : " , N );
- WOLF_INFO("frequency : " , (N-1)/(tf-t0) , " Hz" );
- WOLF_INFO("CPU time : " , elapsed_secs , " s" );
- WOLF_INFO("s/integr : " , elapsed_secs/(N-1)*1e6 , " us" );
- WOLF_INFO("integr/s : " , (N-1)/elapsed_secs , " ips" );
-
- return 0;
-}
diff --git a/demos/demo_eigen_quaternion.cpp b/demos/demo_eigen_quaternion.cpp
deleted file mode 100644
index 1e85039d7d5a25c6320c74d0d1914c372a9220f6..0000000000000000000000000000000000000000
--- a/demos/demo_eigen_quaternion.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-
-//std
-#include <iostream>
-
-//Eigen
-#include <eigen3/Eigen/Geometry>
-
-//Wolf
-#include "core/common/wolf.h"
-
-int main()
-{
- using namespace wolf;
-
- std::cout << std::endl << "Eigen Quatenrnion test" << std::endl;
-
- Scalar q1[4];
- Eigen::Map<Eigen::Quaternions> q1_map(q1);
-
- //try to find out how eigen sorts storage (real part tail or head ? )
- q1_map.w() = 1;
- q1_map.x() = 2;
- q1_map.y() = 3;
- q1_map.z() = 4;
- std::cout << "q1[0]=" << q1[0] << "; q1_map.x()=" << q1_map.x() << std::endl;
- std::cout << "q1[1]=" << q1[1] << "; q1_map.y()=" << q1_map.y() << std::endl;
- std::cout << "q1[2]=" << q1[2] << "; q1_map.z()=" << q1_map.z() << std::endl;
- std::cout << "q1[3]=" << q1[3] << "; q1_map.w()=" << q1_map.w() << std::endl;
- std::cout << std::endl << "RESULT: Eigen stores REAL part in the LAST memory position of the quaternion." << std::endl;
-
- std::cout << std::endl << "End of Eigen Quatenrnion test" << std::endl;
- return 0;
-}
-
diff --git a/demos/demo_eigen_template.cpp b/demos/demo_eigen_template.cpp
deleted file mode 100644
index 47e5aa4191e1d680b4cbc5a8702d7c55f74ed52e..0000000000000000000000000000000000000000
--- a/demos/demo_eigen_template.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/**
- * \file test_eigen_template.cpp
- *
- * Created on: Sep 12, 2016
- * \author: jsola
- */
-
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-#include <iostream>
-
-template <int Size, int DesiredSize>
-struct StaticSizeCheck {
- template <typename T>
- StaticSizeCheck(const T&) {
- static_assert(Size == DesiredSize, "Static sizes do not match");
- }
-};
-
-template <int DesiredSize>
-struct StaticSizeCheck<Eigen::Dynamic, DesiredSize> {
- template <typename T>
- StaticSizeCheck(const T& t) {
- assert(t == DesiredSize && "Dynamic sizes do not match");
- }
-};
-
-template <int DesiredR, int DesiredC>
-struct MatrixSizeCheck {
- template <typename T>
- static void check(const T& t) {
- StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
- StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
- }
-};
-
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<Derived>& _v){
-
- MatrixSizeCheck<3, 1>::check(_v);
-
- Eigen::Quaternion<typename Derived::Scalar> q;
- typename Derived::Scalar angle = _v.norm();
- typename Derived::Scalar angle_half = angle/2.0;
- if (angle > 1e-8)
- {
- q.w() = cos(angle_half);
- q.vec() = _v / angle * sin(angle_half);
- return q;
- }
- else
- {
- q.w() = cos(angle_half);
- q.vec() = _v * ( (typename Derived::Scalar)0.5 - angle_half*angle_half/(typename Derived::Scalar)12.0 ); // see the Taylor series of sinc(x) ~ 1 - x^2/3!, and have q.vec = v/2 * sinc(angle_half)
- return q;
- }
-}
-
-int main(void)
-{
- using namespace Eigen;
-
- VectorXd x(10);
- x << 1,2,3,4,5,6,7,8,9,10;
-
- Quaterniond q;
- Map<Quaterniond> qm(x.data()+5);
-
- // Static vector
- Vector3d v;
- v << 1,2,3;
- q = v2q(v);
- qm = v2q(v);
- std::cout << q.coeffs().transpose() << std::endl;
- std::cout << qm.coeffs().transpose() << std::endl;
-
- // Dynamic matrix
- Matrix<double, Dynamic, Dynamic> M(3,1);
- M << 1, 2, 3;
- q = v2q(M);
- std::cout << q.coeffs().transpose() << std::endl;
-
- // Dynamic vector segment
- x << 1,2,3,4,5,6,7,8,9,10;
- q = v2q(x.head(3));
- std::cout << q.coeffs().transpose() << std::endl;
-
- // Map over dynamic vector
- Map<VectorXd> m(x.data(), 3);
- q = v2q(m);
- std::cout << q.coeffs().transpose() << std::endl;
-
- // Float scalar
- Vector3f vf;
- Quaternionf qf;
- vf << 1,2,3;
- qf = v2q(vf);
- std::cout << qf.coeffs().transpose() << std::endl;
-
- // // Static assert at compile time
- // Vector2d v2;
- // q= v2q(v2);
- // std::cout << q.coeffs().transpose() << std::endl;
-
-}
diff --git a/demos/demo_factor_AHP.cpp b/demos/demo_factor_AHP.cpp
deleted file mode 100644
index 8509ff7f573c31ccef85ace45db7d5036b44d470..0000000000000000000000000000000000000000
--- a/demos/demo_factor_AHP.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-#include "core/math/pinhole_tools.h"
-#include "core/landmark/landmark_AHP.h"
-#include "core/factor/factor_AHP.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/sensor/sensor_camera.h"
-#include "core/capture/capture_image.h"
-
-int main()
-{
- using namespace wolf;
-
- std::cout << std::endl << "==================== factor AHP test ======================" << std::endl;
-
- TimeStamp t = 1;
-
- //=====================================================
- // Environment variable for configuration files
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::cout << wolf_root << std::endl;
- //=====================================================
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 3);
-
- /* Do this while there aren't extrinsic parameters on the yaml */
- Eigen::Vector7s extrinsic_cam;
- extrinsic_cam.setRandom();
-// extrinsic_cam[0] = 0; //px
-// extrinsic_cam[1] = 0; //py
-// extrinsic_cam[2] = 0; //pz
-// extrinsic_cam[3] = 0; //qx
-// extrinsic_cam[4] = 0; //qy
-// extrinsic_cam[5] = 0; //qz
-// extrinsic_cam[6] = 1; //qw
- extrinsic_cam.tail<4>().normalize();
- std::cout << "========extrinsic_cam: " << extrinsic_cam.transpose() << std::endl;
- const Eigen::VectorXs extr = extrinsic_cam;
- /* Do this while there aren't extrinsic parameters on the yaml */
-
- SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("CAMERA", "PinHole", extr, wolf_root + "/src/examples/camera_params_ueye_radial_dist.yaml");
- SensorCameraPtr camera_ptr_ = std::static_pointer_cast<SensorCamera>(sensor_ptr);
-
- // PROCESSOR
- // one-liner API
- ProcessorBasePtr processor_ptr = wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml");
-
- // create the current frame
- Eigen::Vector7s frame_pos_ori;
- frame_pos_ori.setRandom();
-// frame_pos_ori[0] = 0; //px
-// frame_pos_ori[1] = 0; //py
-// frame_pos_ori[2] = 0; //pz
-// frame_pos_ori[3] = 0; //qx
-// frame_pos_ori[4] = 0; //qy
-// frame_pos_ori[5] = 0; //qz
-// frame_pos_ori[6] = 1; //qw
- frame_pos_ori.tail<4>().normalize();
- const Eigen::VectorXs frame_val = frame_pos_ori;
-
- FrameBasePtr last_frame = std::make_shared<FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
- std::cout << "Last frame" << std::endl;
- wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
- // Capture
- CaptureImagePtr image_ptr;
- t.setToNow();
- cv::Mat frame; //puede que necesite una imagen
-
- image_ptr = std::make_shared< CaptureImage>(t, camera_ptr_, frame);
- last_frame->addCapture(image_ptr);
-
- // create the feature
- cv::KeyPoint kp; kp.pt = {10,20};
- cv::Mat desc;
-
- FeaturePointImagePtr feat_point_image_ptr = std::make_shared<FeaturePointImage>(kp, 0, desc, Eigen::Matrix2s::Identity());
- image_ptr->addFeature(feat_point_image_ptr);
-
- FrameBasePtr anchor_frame = std::make_shared< FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
- //FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectory()->getLastFrame();
-
- // create the landmark
- Eigen::Vector2s point2D;
- point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
- point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
- std::cout << "point2D: " << point2D.transpose() << std::endl;
-
- Scalar distance = 2; // arbitrary value
- Eigen::Vector4s vec_homogeneous;
-
- Eigen::VectorXs correction_vec = (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector();
- std::cout << "correction vector: " << correction_vec << std::endl;
- std::cout << "distortion vector: " << (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector() << std::endl;
- point2D = pinhole::depixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D);
- std::cout << "point2D depixellized: " << point2D.transpose() << std::endl;
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector(),point2D);
- std::cout << "point2D undistorted: " << point2D.transpose() << std::endl;
-
- Eigen::Vector3s point3D;
- point3D.head(2) = point2D;
- point3D(2) = 1;
-
- point3D.normalize();
- std::cout << "point3D normalized: " << point3D.transpose() << std::endl;
-
- vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
- std::cout << "vec_homogeneous: " << vec_homogeneous.transpose() << std::endl;
-
- LandmarkAHPPtr landmark = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, image_ptr->getSensor(), feat_point_image_ptr->getDescriptor());
-
- std::cout << "Landmark AHP created" << std::endl;
-
- // Create the factor
- FactorAHPPtr factor_ptr = std::make_shared<FactorAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark), processor_ptr);
-
- feat_point_image_ptr->addFactor(factor_ptr);
- std::cout << "Factor AHP created" << std::endl;
-
- Eigen::Vector2s point2D_proj = point3D.head<2>()/point3D(2);
- std::cout << "point2D projected: " << point2D_proj.transpose() << std::endl;
-
- Eigen::Vector2s point2D_dist;
- point2D_dist = pinhole::distortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector(),point2D_proj);
- std::cout << "point2D distorted: " << point2D_dist.transpose() << std::endl;
-
- Eigen::Vector2s point2D_pix;
- point2D_pix = pinhole::pixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D_dist);
- std::cout << "point2D pixellized: " << point2D_pix.transpose() << std::endl;
-
- Eigen::Vector2s expectation;
- Eigen::Vector3s current_frame_p = last_frame->getP()->getState();
- Eigen::Vector4s current_frame_o = last_frame->getO()->getState();
- Eigen::Vector3s anchor_frame_p = landmark->getAnchorFrame()->getP()->getState();
- Eigen::Vector4s anchor_frame_o = landmark->getAnchorFrame()->getO()->getState();
- Eigen::Vector4s landmark_ = landmark->getP()->getState();
-
- factor_ptr ->expectation(current_frame_p.data(), current_frame_o.data(),
- anchor_frame_p.data(), anchor_frame_o.data(),
- landmark_.data(), expectation.data());
-// current_frame p; current_frame o; anchor_frame p; anchor_frame o; homogeneous_vector landmark, residual
-
- std::cout << "expectation computed" << std::endl;
- std::cout << "expectation = " << expectation[0] << " " << expectation[1] << std::endl;
-
- return 0;
-
-}
diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp
deleted file mode 100644
index 36e6bfa52385e37e51a8d1616e3c20640f35b371..0000000000000000000000000000000000000000
--- a/demos/demo_factor_imu.cpp
+++ /dev/null
@@ -1,279 +0,0 @@
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/capture/capture_pose.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//#define DEBUG_RESULTS
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::cout << std::endl << "==================== test_factor_imu ======================" << std::endl;
-
- bool c0(false), c1(false);// c2(true), c3(true), c4(true);
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, shared_ptr<IntrinsicsBase>());
- wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
- // Ceres wrappers
- 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;
- CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
-
- // Time and data variables
- TimeStamp t;
- Eigen::Vector6s data_;
- Scalar mpu_clock = 0;
-
- t.set(mpu_clock);
-
- // Set the origin
- Eigen::VectorXs x0(16);
- x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
- wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t); //this also creates a keyframe at origin
- wolf_problem_ptr_->getTrajectory()->getFrameList().front()->fix(); //fix the keyframe at origin
-
- TimeStamp ts(0);
- Eigen::VectorXs origin_state = x0;
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
- imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
-
- // set variables
- using namespace std;
- Eigen::VectorXs state_vec;
- Eigen::VectorXs delta_preint;
- //FrameIMUPtr last_frame;
- Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
-
- //process data
- mpu_clock = 0.001003;
- //data_ << 0.579595, -0.143701, 9.939331, 0.127445, 0.187814, -0.055003;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- // assign data to capture
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- // process data in capture
- sensor_ptr->process(imu_ptr);
-
- if(c0){
- /// ******************************************************************************************** ///
- /// factor creation
- //create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
- wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
- std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapture(imu_ptr);
-
- //create a factorIMU
- FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
- feat_imu->addFactor(factor_imu);
- last_frame->addConstrainedBy(factor_imu);
-
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
- residu << 0,0,0, 0,0,0, 0,0,0;
-
- factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, current_frame_p, current_frame_o, current_frame_v, expect);
- std::cout << "expectation : " << expect.transpose() << std::endl;
-
- factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
- std::cout << "residuals : " << residu.transpose() << std::endl;
-
- //reset origin of motion to new frame
- wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
- imu_ptr->setFrame(last_frame);
- }
- /// ******************************************************************************************** ///
-
- mpu_clock = 0.002135;
- //data_ << 0.581990, -0.191602, 10.071057, 0.136836, 0.203912, -0.057686;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- mpu_clock = 0.003040;
- //data_ << 0.596360, -0.225132, 10.205178, 0.154276, 0.174399, -0.036221;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- if(c1){
- /// ******************************************************************************************** ///
- /// factor creation
- //create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
- wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
- std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapture(imu_ptr);
-
- //create a factorIMU
- FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
- feat_imu->addFactor(factor_imu);
- last_frame->addConstrainedBy(factor_imu);
-
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
- residu << 0,0,0, 0,0,0, 0,0,0;
-
- factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
- std::cout << "expectation : " << expect.transpose() << std::endl;
-
- factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
- std::cout << "residuals : " << residu.transpose() << std::endl;
-
- //reset origin of motion to new frame
- wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
- imu_ptr->setFrame(last_frame);
- }
-
- mpu_clock = 0.004046;
- //data_ << 0.553250, -0.203577, 10.324929, 0.128787, 0.156959, -0.044270;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- mpu_clock = 0.005045;
- //data_ << 0.548459, -0.184417, 10.387200, 0.083175, 0.120738, -0.026831;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- //create the factor
- //create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
- wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
- std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapture(imu_ptr);
-
- //create a factorIMU
- FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
- feat_imu->addFactor(factor_imu);
- last_frame->addConstrainedBy(factor_imu);
-
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
- residu << 0,0,0, 0,0,0, 0,0,0;
-
- factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
- std::cout << "expectation : " << expect.transpose() << std::endl;
-
- factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
- std::cout << "residuals : " << residu.transpose() << std::endl;
-
- if(wolf_problem_ptr_->check(1)){
- wolf_problem_ptr_->print(4,1,1,1);
- }
-
- ///having a look at covariances
- Eigen::MatrixXs predelta_cov;
- predelta_cov.resize(9,9);
- predelta_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
- //std::cout << "predelta_cov : \n" << predelta_cov << std::endl;
-
- ///Optimization
- // PRIOR
- //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front();
- wolf_problem_ptr_->getProcessorMotion()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front());
- //SensorBasePtr sensorbase = std::make_shared<SensorBase>("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0);
- //CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6s::Identity() * 0.01);
- //first_frame->addCapture(initial_covariance);
- //initial_covariance->process();
- //std::cout << "initial covariance: factor " << initial_covariance->getFeatureList().front()->getConstrainedByList().front()->id() << std::endl << initial_covariance->getFeatureList().front()->getMeasurementCovariance() << std::endl;
-
- // COMPUTE COVARIANCES
- std::cout << "computing covariances..." << std::endl;
- ceres_manager_wolf_diff->computeCovariances(ALL_MARGINALS);//ALL_MARGINALS, ALL
- std::cout << "computed!" << std::endl;
-
- /*
- // SOLVING PROBLEMS
- ceres::Solver::Summary summary_diff;
- std::cout << "solving..." << std::endl;
- Eigen::VectorXs prev_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
- summary_diff = ceres_manager_wolf_diff->solve();
- Eigen::VectorXs post_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
- std::cout << " prev_wolf_state : " << prev_wolf_state.transpose() << "\n post_wolf_state : " << post_wolf_state.transpose() << std::endl;
- //std::cout << summary_wolf_diff.BriefReport() << std::endl;
- std::cout << "solved!" << std::endl;
- */
-
- /*
- std::cout << "WOLF AUTO DIFF" << std::endl;
- std::cout << "Jacobian evaluation: " << summary_wolf.jacobian_evaluation_time_in_seconds << std::endl;
- std::cout << "Total time: " << summary_wolf.total_time_in_seconds << std::endl;
- */
-
- return 0;
-}
diff --git a/demos/demo_factor_odom_3D.cpp b/demos/demo_factor_odom_3D.cpp
deleted file mode 100644
index be7a81c4f454a8c3c6a4bc621b9de8cb3ef39a0d..0000000000000000000000000000000000000000
--- a/demos/demo_factor_odom_3D.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-/*
- * test_factor_odom_3D.cpp
- *
- * Created on: Oct 7, 2016
- * Author: jsola
- */
-
-#include "core/factor/factor_odom_3D.h"
-
-namespace wolf
-{
-
-} /* namespace wolf */
diff --git a/demos/demo_faramotics_simulation.cpp b/demos/demo_faramotics_simulation.cpp
deleted file mode 100644
index 4414520dc1fc0edf2260ad4cf416ee3ddd2ea56d..0000000000000000000000000000000000000000
--- a/demos/demo_faramotics_simulation.cpp
+++ /dev/null
@@ -1,244 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-// wolf
-#include "core/common/wolf.h"
-#include "core/feature/feature_base.h"
-#include "core/landmark/landmark_base.h"
-#include "core/state_block/state_block.h"
-
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-namespace wolf {
-class FaramoticsRobot
-{
- public:
-
- Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
- vector < Cpose3d > devicePoses;
- vector<float> scan1, scan2;
- string modelFileName;
- CrangeScan2D* myScanner;
- CdynamicSceneRender* myRender;
- Eigen::Vector3s ground_truth_pose_;
- Eigen::Vector4s laser_1_pose_, laser_2_pose_;
-
- FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
- modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
- laser_1_pose_(_laser_1_pose),
- laser_2_pose_(_laser_2_pose)
- {
- devicePose.setPose(2, 8, 0.2, 0, 0, 0);
- viewPoint.setPose(devicePose);
- viewPoint.moveForward(10);
- viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
- viewPoint.moveForward(-15);
- //glut initialization
- faramotics::initGLUT(argc, argv);
- //create a viewer for the 3D model and scan points
- myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
- myRender->loadAssimpModel(modelFileName, true); //with wireframe
- //create scanner and load 3D model
- myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
- myScanner->loadAssimpModel(modelFileName);
- }
-
- //function travel around
- Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
- {
- if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
- else if (ii <= 220) { displacement_ = 0; rotation_ =-1.8 * M_PI / 180; }
- else if (ii <= 310) { displacement_ = 0.1; rotation_ = 0; }
- else if (ii <= 487) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 600) { displacement_ = 0.2; rotation_ = 0; }
- else if (ii <= 700) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
- else if (ii <= 780) { displacement_ = 0; rotation_ =-1.0 * M_PI / 180; }
- else { displacement_ = 0.3; rotation_ = 0; }
-
- devicePose.moveForward(displacement_);
- devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll());
-
- // laser 1
- laser1Pose.setPose(devicePose);
- laser1Pose.moveForward(laser_1_pose_(0));
- // laser 2
- laser2Pose.setPose(devicePose);
- laser2Pose.moveForward(laser_2_pose_(0));
- laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-
- devicePoses.push_back(devicePose);
-
- ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
- return ground_truth_pose_;
- }
-
- ~FaramoticsRobot()
- {
- std::cout << "deleting render and scanner.." << std::endl;
- delete myRender;
- delete myScanner;
- std::cout << "deleted!" << std::endl;
- }
-
- //compute scans
- vector<float> computeScan(const int scan_id)
- {
- if (scan_id == 1)
- {
- scan1.clear();
- myScanner->computeScan(laser1Pose, scan1);
- return scan1;
- }
- else
- {
- scan2.clear();
- myScanner->computeScan(laser2Pose, scan2);
- return scan2;
- }
- }
-
- void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
- {
- // detected corners
- //std::cout << " drawCorners: " << feature_list.size() << std::endl;
- std::vector<double> corner_vector;
- corner_vector.reserve(2*feature_list.size());
- for (auto corner : feature_list)
- {
- //std::cout << " corner " << corner->id() << std::endl;
- corner_vector.push_back(corner->getMeasurement(0));
- corner_vector.push_back(corner->getMeasurement(1));
- }
- myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector);
-
- // landmarks
- //std::cout << " drawLandmarks: " << landmark_list.size() << std::endl;
- std::vector<double> landmark_vector;
- landmark_vector.reserve(3*landmark_list.size());
- for (auto landmark : landmark_list)
- {
- Scalar* position_ptr = landmark->getP()->get();
- landmark_vector.push_back(*position_ptr); //x
- landmark_vector.push_back(*(position_ptr + 1)); //y
- landmark_vector.push_back(0.2); //z
- }
- myRender->drawLandmarks(landmark_vector);
-
- // draw localization and sensors
- estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0);
- estimated_laser_1_pose.setPose(estimated_vehicle_pose);
- estimated_laser_1_pose.moveForward(laser_1_pose_(0));
- estimated_laser_2_pose.setPose(estimated_vehicle_pose);
- estimated_laser_2_pose.moveForward(laser_2_pose_(0));
- estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
- myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
- //Set view point and render the scene
- //locate visualization view point, somewhere behind the device
- // viewPoint.setPose(devicePose);
- // viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
- // viewPoint.moveForward(-5);
- myRender->setViewPoint(viewPoint);
- myRender->drawPoseAxis(devicePose);
- myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
- myRender->render();
- }
-};
-}
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << "\n============================================================\n";
- std::cout << "========== 2D Robot with odometry and 2 LIDARs =============\n";
-
- unsigned int n_execution = 900; //number of iterations of the whole execution
-
- // VARIABLES ============================================================================================
- Eigen::Vector2s odom_data;
- Eigen::Vector3s ground_truth;
- Scalar dt = 0.05;
-
- // Laser params
- Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
- laser_1_pose << 1.2, 0, 0, 0; //laser 1
- laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- Eigen::VectorXs laser_params(8);
- laser_params << M_PI/2, -M_PI/2, -M_PI/720, 0.01, 0.2, 100, 0.01, 0.01;
- std::vector<float> scan1, scan2;
-
- // Simulated robot
- FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
-
- // Initial pose
- ground_truth << 2, 8, 0;
-
- //output file
- std::ofstream laser_1_file, laser_2_file, odom_file, groundtruth_file;
- groundtruth_file.open("simulated_groundtruth.txt", std::ofstream::out); //open log file
- odom_file.open("simulated_odom.txt", std::ofstream::out); //open log file
- laser_1_file.open("simulated_laser_1.txt", std::ofstream::out); //open log file
- laser_2_file.open("simulated_laser_2.txt", std::ofstream::out); //open log file
-
- // write laser params
- laser_1_file << 0 << " " << laser_params.transpose() << " " << robot.myScanner->getNumPoints() << std::endl;
- laser_2_file << 0 << " " << laser_params.transpose() << " " << robot.myScanner->getNumPoints() << std::endl;
- laser_1_file << 0 << " " << laser_1_pose.transpose() << std::endl;
- laser_2_file << 0 << " " << laser_2_pose.transpose() << std::endl;
-
- // origin frame groundtruth
- groundtruth_file << 0 << " " << ground_truth.transpose() << std::endl;
-
- //std::cout << "START TRAJECTORY..." << std::endl;
- // START TRAJECTORY ============================================================================================
- for (unsigned int step = 1; step < n_execution; step++)
- {
- // ROBOT MOVEMENT ---------------------------
- ground_truth = robot.motionCampus(step, odom_data(0), odom_data(1));
-
- // LIDAR DATA ---------------------------
- scan1 = robot.computeScan(1);
- scan2 = robot.computeScan(2);
-
- // writing files ---------------------------
- groundtruth_file << step*dt << " " << ground_truth.transpose() << std::endl;
- odom_file << step*dt << " " << odom_data.transpose() << std::endl;
- laser_1_file << step*dt << " ";
- for (auto range : scan1)
- laser_1_file << range << " ";
- laser_1_file << std::endl;
- laser_2_file << step*dt << " ";
- for (auto range : scan1)
- laser_2_file << range << " ";
- laser_2_file << std::endl;
- }
-
- groundtruth_file.close(); //close log file
- odom_file.close(); //close log file
- laser_1_file.close(); //close log file
- laser_2_file.close(); //close log file
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_fcn_ptr.cpp b/demos/demo_fcn_ptr.cpp
deleted file mode 100644
index 8ca82790e934cb0c9236ad83cfe7a92d1cc6d284..0000000000000000000000000000000000000000
--- a/demos/demo_fcn_ptr.cpp
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * test_fcn_ptr.cpp
- *
- * Created on: Nov 29, 2016
- * Author: jsola
- */
-
-#include <iostream>
-#include <cstdarg>
-
-// define some fcns with differnt # of args
-double half (double _a) { return _a/2; }
-double quarter (double _a) { return _a/4; }
-double divide (double _n, double _d) { return _n/_d; }
-double mult (double _x, double _y) { return _x*_y; }
-double mult_div (double _x, double _y, double _z) { return _x*_y/_z; }
-
-//======== test_simple usage of function pointers ============
-typedef double (*FcnType)(double);
-
-double run_simple(FcnType f, double a){ return f(a); }
-
-void test_simple()
-{
- std::cout << "simple..." << std::endl;
- std::cout << "4/2 = " << run_simple(half, 4) << std::endl;
- std::cout << "4/4 = " << run_simple(quarter, 4) << std::endl;
-}
-
-//======== more usage of function pointers ============
-typedef double (*FcnType2)(double a, double b);
-typedef double (*FcnType3)(double a, double b, double c);
-
-double run_2(FcnType2 f, double a, double b) { return f(a, b); }
-double run_3(FcnType3 f, double a, double b, double c) { return f(a, b, c); }
-
-void test_more()
-{
- std::cout << "more..." << std::endl;
- std::cout << "4/2 = " << run_2(divide, 4, 2) << std::endl;
- std::cout << "4*2 = " << run_2(mult, 4, 2) << std::endl;
- std::cout << "4*3/6 = " << run_3(mult_div, 4, 3, 6) << std::endl;
-}
-
-//======== variadic usage of function pointers =========
-typedef double (*FcnTypeV)(...);
-
-// we will try to use half(), quarter(), mult(), divide() and mult_div() above
-
-//------------------------------------------------------------------------------------
-// ---- try just to read the args of the variadic fcn; no function pointer yet
-double run_v_dummy(int n, ...)
-{
- va_list args;
- va_start(args, n);
- double b = 0;
- for (int i=0; i<n; i++)
- b += va_arg(args, double);
- return b;
-}
-
-void test_var_dummy()
-{
- std::cout << "var dummy..." << std::endl;
- std::cout << "1 = " << run_v_dummy(1, 1.0) << std::endl;
- std::cout << "1+2 = " << run_v_dummy(2, 1.0, 2.0) << std::endl;
- std::cout << "1+2+3 = " << run_v_dummy(3, 1.0, 2.0, 3.0) << std::endl;
-}
-
-//------------------------------------------------------------------------------------
-// ---- call function through pointer; ugly solution with switch / case on the # of args:
-double run_v_switch(FcnTypeV f, int n, ...)
-{
- va_list args ;
- va_start(args, n); // args start after n
- switch (n)
- {
- case 1:
- return f(va_arg(args, double));
- break;
- case 2:
- return f(va_arg(args, double), va_arg(args, double));
- break;
- case 3:
- return f(va_arg(args, double), va_arg(args, double), va_arg(args, double));
- break;
- default:
- return 0;
- }
-}
-
-void test_var_switch()
-{
- std::cout << "var using switch/case of the # of args (ugly)..." << std::endl;
- std::cout << "4/2 = " << run_v_switch((FcnTypeV)half, 1, 4.0 ) << std::endl;
- std::cout << "4/4 = " << run_v_switch((FcnTypeV)quarter, 1, 4.0 ) << std::endl;
- std::cout << "4/2 = " << run_v_switch((FcnTypeV)divide, 2, 4.0, 2.0 ) << std::endl;
- std::cout << "4*2 = " << run_v_switch((FcnTypeV)mult, 2, 4.0, 2.0 ) << std::endl;
- std::cout << "4*3/6 = " << run_v_switch((FcnTypeV)mult_div, 3, 4.0, 3.0, 6.0) << std::endl;
-}
-
-//------------------------------------------------------------------------------------
-// ---- call function through pointer; try to forward all args straight into the inner function!
-double run_v(FcnTypeV f, int n, ...)
-{
- va_list args;
- va_start(args,n); // args start after n
- return f(args); // hop!
-}
-
-void test_var()
-{
- std::cout << "var forwarding all args to the inner fcn (nice!)..." << std::endl;
- std::cout << "4/2 = " << run_v((FcnTypeV)half, 1, 4.0 ) << std::endl;
- std::cout << "4/4 = " << run_v((FcnTypeV)quarter, 1, 4.0 ) << std::endl;
- std::cout << "4/2 = " << run_v((FcnTypeV)divide, 2, 4.0, 2.0 ) << std::endl;
- std::cout << "4*2 = " << run_v((FcnTypeV)mult, 2, 4.0, 2.0 ) << std::endl;
- std::cout << "4*3/6 = " << run_v((FcnTypeV)mult_div, 3, 4.0, 3.0, 6.0) << std::endl;
-}
-
-//####################################################################################
-
-int main()
-{
- test_simple();
- test_more();
- test_var_dummy();
- test_var_switch();
- test_var();
-}
diff --git a/demos/demo_image.cpp b/demos/demo_image.cpp
deleted file mode 100644
index 3997b9476334d39092ee2eaf5140c5b9a9facfc0..0000000000000000000000000000000000000000
--- a/demos/demo_image.cpp
+++ /dev/null
@@ -1,177 +0,0 @@
-// Testing things for the 3D image odometry
-
-//Wolf includes
-#include "core/sensor/sensor_camera.h"
-#include "core/capture/capture_image.h"
-#include "core/processor/processor_tracker_feature_image.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// Vision utils includes
-#include <vision_utils.h>
-#include <sensors.h>
-#include <common_class/buffer.h>
-#include <common_class/frame.h>
-
-//std includes
-#include <ctime>
-#include <iostream>
-#include <string>
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- //ProcessorImageFeature test
- std::cout << std::endl << " ========= ProcessorImageFeature test ===========" << std::endl << std::endl;
-
- // Sensor or sensor recording
- vision_utils::SensorCameraPtr sen_ptr = vision_utils::askUserSource(argc, argv);
- if (sen_ptr==NULL)
- return 0;
-
- unsigned int buffer_size = 8;
- vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
-
- unsigned int img_width = frame_buff.back()->getImage().cols;
- unsigned int img_height = frame_buff.back()->getImage().rows;
- std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
-
- // graphics
- cv::namedWindow("Feature tracker"); // Creates a window for display.
- cv::moveWindow("Feature tracker", 0, 0);
- cv::startWindowThread();
-
- CaptureImagePtr image_ptr;
-
- TimeStamp t = 1;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::cout << "Wolf root: " << wolf_root << std::endl;
-
- ProblemPtr wolf_problem_ = Problem::create("PO", 3);
-
- //=====================================================
- // Method 1: Use data generated here for sensor and processor
- //=====================================================
-
-// // SENSOR
-// Eigen::Vector4s k = {320,240,320,320};
-// SensorCameraPtr camera_ptr = std::make_shared<SensorCamera>(std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
-// std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
-// std::make_shared<StateBlock>(k,false),img_width,img_height);
-//
-// wolf_problem_->getHardware()->addSensor(camera_ptr);
-//
-// // PROCESSOR
-// ProcessorParamsImage tracker_params;
-// tracker_params.matcher.min_normalized_score = 0.75;
-// tracker_params.matcher.similarity_norm = cv::NORM_HAMMING;
-// tracker_params.matcher.roi_width = 30;
-// tracker_params.matcher.roi_height = 30;
-// tracker_params.active_search.grid_width = 12;
-// tracker_params.active_search.grid_height = 8;
-// tracker_params.active_search.separation = 1;
-// tracker_params.max_new_features =0;
-// tracker_params.min_features_for_keyframe = 20;
-//
-// DetectorDescriptorParamsOrb orb_params;
-// orb_params.type = DD_ORB;
-//
-// DetectorDescriptorParamsBrisk brisk_params;
-// brisk_params.type = DD_BRISK;
-//
-// // select the kind of detector-descriptor parameters
-// tracker_params.detector_descriptor_params_ptr = std::make_shared<DetectorDescriptorParamsOrb>(orb_params); // choose ORB
-//// tracker_params.detector_descriptor_params_ptr = std::make_shared<DetectorDescriptorParamsBrisk>(brisk_params); // choose BRISK
-//
-// std::cout << tracker_params.detector_descriptor_params_ptr->type << std::endl;
-//
-// ProcessorTrackerTrifocalTensorPtr prc_image = std::make_shared<ProcessorImageFeature>(tracker_params);
-//// camera_ptr->addProcessor(prc_image);
-// std::cout << "sensor & processor created and added to wolf problem" << std::endl;
- //=====================================================
-
- //=====================================================
- // Method 2: Use factory to create sensor and processor
- //=====================================================
-
- /* Do this while there aren't extrinsic parameters on the yaml */
- Eigen::Vector7s extrinsic_cam;
- extrinsic_cam[0] = 0; //px
- extrinsic_cam[1] = 0; //py
- extrinsic_cam[2] = 0; //pz
- extrinsic_cam[3] = 0; //qx
- extrinsic_cam[4] = 0; //qy
- extrinsic_cam[5] = 0; //qz
- extrinsic_cam[6] = 1; //qw
- std::cout << "========extrinsic_cam: " << extrinsic_cam.transpose() << std::endl;
- const Eigen::VectorXs extr = extrinsic_cam;
- /* Do this while there aren't extrinsic parameters on the yaml */
-
- // SENSOR
- // one-liner API
- SensorBasePtr sensor_ptr = wolf_problem_->installSensor("CAMERA", "PinHole", Eigen::VectorXs::Zero(7), wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- SensorCameraPtr camera_ptr = static_pointer_cast<SensorCamera>(sensor_ptr);
- camera_ptr->setImgWidth(img_width);
- camera_ptr->setImgHeight(img_height);
-
- // PROCESSOR
- // one-liner API
- ProcessorTrackerFeatureImagePtr prc_img_ptr = std::static_pointer_cast<ProcessorTrackerFeatureImage>( wolf_problem_->installProcessor("IMAGE FEATURE", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml") );
- std::cout << "sensor & processor created and added to wolf problem" << std::endl;
- //=====================================================
-
-// // Ceres wrapper
-// ceres::Solver::Options ceres_options;
-// ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
-// ceres_options.max_line_search_step_contraction = 1e-3;
-// // ceres_options.minimizer_progress_to_stdout = false;
-// // ceres_options.line_search_direction_type = ceres::LBFGS;
-// // ceres_options.max_num_iterations = 100;
-// google::InitGoogleLogging(argv[0]);
-// CeresManager ceres_manager(wolf_problem_, ceres_options);
-
- for(int f = 0; f<10000; ++f)
- {
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), f) );
-
- std::cout << "\n=============== Frame #: " << f << " ===============" << std::endl;
-
- t.setToNow();
- clock_t t1 = clock();
-
- // Preferred method with factory objects:
- image_ptr = make_shared<CaptureImage>(t, camera_ptr, frame_buff.back()->getImage());
-
- camera_ptr->process(image_ptr);
-
- std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
-
- wolf_problem_->print();
-
- cv::Mat image = frame_buff.back()->getImage().clone();
- prc_img_ptr->drawFeatures(image);
- prc_img_ptr->drawRoi(image,prc_img_ptr->detector_roi_,cv::Scalar(0.0,255.0, 255.0)); //active search roi
- prc_img_ptr->drawRoi(image,prc_img_ptr->tracker_roi_, cv::Scalar(255.0, 0.0, 255.0)); //tracker roi
- prc_img_ptr->drawTarget(image,prc_img_ptr->tracker_target_);
- cv::imshow("Feature tracker", image);
- cv::waitKey(1);
-
-// if((f%100) == 0)
-// {
-// std::string summary = ceres_manager.solve(2);
-// std::cout << summary << std::endl;
-//
-// std::cout << "Last key frame pose: "
-// << wolf_problem_->getLastKeyFrame()->getP()->getState().transpose() << std::endl;
-// std::cout << "Last key frame orientation: "
-// << wolf_problem_->getLastKeyFrame()->getO()->getState().transpose() << std::endl;
-//
-// cv::waitKey(0);
-// }
- }
-}
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
deleted file mode 100644
index c05fbbf9c4e676c5611c7073b94b9f55385a9267..0000000000000000000000000000000000000000
--- a/demos/demo_imuDock.cpp
+++ /dev/null
@@ -1,318 +0,0 @@
-/**
- * \file test_imuDock.cpp
- *
- * Created on: July 18, 2017
- * \author: Dinesh Atchuthan
- */
-
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-
-//Factors headers
-#include "core/factor/factor_fix_bias.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include "core/factor/factor_pose_3D.h"
-
-#define OUTPUT_RESULTS
-//#define ADD_KF3
-
-/* OFFLINE VERSION
- In this test, we use the experimental conditions needed for Humanoids 2017.
- IMU data are acquired using the docking station.
-
- Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
- invar : P1, V1, V2
- var : Q1,B1,P2,Q2,B2
- factors : Odometry factor between KeyFrames
- IMU factor
- FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
- Fix3D factor
-
- What we expect : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
- Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
-
- Representation of the application:
-
- Imu
- KF1----------â—¼----------KF2
- /----P1----------\ /----------P2 invar : P1, V1, V2
- Abs|------â—¼ â—¼ var : Q1,B1,P2,Q2,B2
- \----Q1----------/ \----------Q2
- V1 Odom
- Abs|------â—¼-----B1
-*/
-int main(int argc, char** argv)
-{
- //#################################################### INITIALIZATION
- using namespace wolf;
-
- //___get input file for imu data___
- std::ifstream imu_data_input;
- const char * filename_imu;
- if (argc < 02)
- {
- WOLF_ERROR("Missing 1 input argument (path to imu data file).")
- return 1; //return with error
- }
- else
- {
- filename_imu = argv[1];
-
- imu_data_input.open(filename_imu);
- WOLF_INFO("imu file : ", filename_imu)
- }
-
- // ___Check if the file is correctly opened___
- if(!imu_data_input.is_open()){
- WOLF_ERROR("Failed to open data file ! Exiting")
- return 1;
- }
-
- #ifdef OUTPUT_RESULTS
- //define output file
- std::ofstream output_results_before, output_results_after, checking;
- output_results_before.open("imu_dock_beforeOptim.dat");
- output_results_after.open("imu_dock_afterOptim.dat");
- checking.open("KF_pose_stdev.dat");
- #endif
-
- // ___initialize variabes that will be used through the code___
- Eigen::VectorXs problem_origin(16);
- Eigen::Vector7s imu_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished());
- problem_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0;
-
- //Create vectors to store data and time
- Eigen::Vector6s data_imu, data_odom;
- Scalar clock;
- TimeStamp ts(0), ts_output(0); //will be used to store the data timestamps and set timestamps in captures
-
- // ___Define expected values___
- Eigen::Vector7s expected_KF1_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7s()<<0,-0.06,0,0,0,0,11).finished());
-
- //#################################################### SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // ___Create the WOLF Problem + define origin of the problem___
- ProblemPtr problem = Problem::create("PQVBB 3D");
- CeresManager* ceres_manager = new CeresManager(problem);
-
- // ___Configure Ceres if needed___
-
- // ___Create sensors + processors___
- SensorIMUPtr sensorIMU = std::static_pointer_cast<SensorIMU>(problem->installSensor("IMU", "Main IMU", imu_pose, wolf_root + "/src/examples/sensor_imu.yaml"));
- ProcessorIMUPtr processorIMU = std::static_pointer_cast<ProcessorIMU>(problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"));
-
- SensorOdom3DPtr sensorOdom = std::static_pointer_cast<SensorOdom3D>(problem->installSensor("ODOM 3D", "odom", odom_pose, wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"));
- ProcessorOdom3DPtr processorOdom = std::static_pointer_cast<ProcessorOdom3D>(problem->installProcessor("ODOM 3D", "odom", "odom", wolf_root + "/src/examples/processor_odom_3D.yaml"));
-
- // ___set origin of processors to the problem's origin___
- FrameIMUPtr KF1 = std::static_pointer_cast<FrameIMU>(processorIMU->setOrigin(problem_origin, ts)); // XXX JS: setting ts to zero, and then reading clock from data, is inconsistent.
- processorOdom->setOrigin(KF1);
-
- //#################################################### PROCESS DATA
- // ___process IMU and odometry___
-
- //Create captures
- CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6s::Identity(), Vector6s::Zero()); //ts is set at 0
- CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(ts, sensorOdom, data_odom, 7, 6, nullptr);
-
- //while we do not reach the end of file, read IMU input (ts, Ax, Ay, Az, Wx, Wy, Wz) and process data through capture
- while(!imu_data_input.eof())
- {
- //read
- imu_data_input >> clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5];
-
- //set capture
- ts.set(clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- //process
- sensorIMU->process(imu_ptr);
- }
-
- //All IMU data have been processed, close the file
- imu_data_input.close();
-
- //A KeyFrame should have been created (depending on time_span in processors). get the last KeyFrame
- // XXX JS: in my opinion, we should control the KF creation better, not using time span. Is it possible?
- #ifdef ADD_KF3
- //Add a KeyFrame just before the motion actually starts (we did not move yet)
- data_odom << 0,0,0, 0,0,0;
- TimeStamp t_middle(0.307585);
- mot_ptr->setTimeStamp(t_middle);
- mot_ptr->setData(data_odom);
- sensorOdom->process(mot_ptr);
-
- //Also add a keyframe at the end of the motion
- data_odom << 0,-0.06,0, 0,0,0;
- ts.set(0.981573); //comment this if you want the last KF to be created at last imu's ts
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom);
- sensorOdom->process(mot_ptr);
-
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(t_middle));
- FrameIMUPtr KF3 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
- #else
- //now impose final odometry using last timestamp of imu
- data_odom << 0,-0.06,0, 0,0,0;
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom);
- sensorOdom->process(mot_ptr);
-
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
- #endif
-
- //#################################################### OPTIMIZATION PART
- // ___Create needed factors___
-
- //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
- featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
- featureFix_cov(3,3) = pow( .02 , 2); // roll variance
- featureFix_cov(4,4) = pow( .02 , 2); // pitch variance
- featureFix_cov(5,5) = pow( .01 , 2); // yaw variance
- CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
- FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
-
- Eigen::MatrixXs featureFixBias_cov(6,6);
- featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
- featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
- featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
- CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
- //create a FeatureBase to factor biases
- FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
-
- // ___Fix/Unfix stateblocks___
- KF1->getP()->fix();
- KF1->getO()->unfix();
- KF1->getV()->fix();
- KF1->getAccBias()->unfix();
- KF1->getGyroBias()->unfix();
-
- #ifdef ADD_KF3
- KF2->getP()->fix();
- KF2->getO()->unfix();
- KF2->getV()->fix();
- KF2->getAccBias()->unfix();
- KF2->getGyroBias()->unfix();
-
- KF3->getP()->unfix();
- KF3->getO()->unfix();
- KF3->getV()->fix();
- KF3->getAccBias()->unfix();
- KF3->getGyroBias()->unfix();
- #else
- KF2->getP()->unfix();
- KF2->getO()->unfix();
- KF2->getV()->fix();
- KF2->getAccBias()->unfix();
- KF2->getGyroBias()->unfix();
- #endif
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * first output : estimated trajectory BEFORE optimization (getting the states each millisecond)
- */
-
- unsigned int time_iter(0);
- Scalar ms(0.001);
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_before << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
- #endif
-
- // ___Solve + compute covariances___
- problem->print(4,0,1,0);
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- problem->print(1,0,1,0);
-
- //#################################################### RESULTS PART
-
- // ___Get standard deviation from covariances___
- #ifdef ADD_KF3
- Eigen::MatrixXs cov_KF1(16,16), cov_KF2(16,16), cov_KF3(16,16);
-
- problem->getFrameCovariance(KF1, cov_KF1);
- problem->getFrameCovariance(KF2, cov_KF2);
- problem->getFrameCovariance(KF3, cov_KF3);
-
- Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF1, stdev_KF2, stdev_KF3;
-
- stdev_KF1 = cov_KF1.diagonal().array().sqrt();
- stdev_KF2 = cov_KF2.diagonal().array().sqrt();
- stdev_KF3 = cov_KF3.diagonal().array().sqrt();
-
- WOLF_DEBUG("stdev KF1 : ", stdev_KF1.transpose());
- WOLF_DEBUG("stdev KF2 : ", stdev_KF2.transpose());
- WOLF_DEBUG("stdev KF3 : ", stdev_KF3.transpose());
- #else
- Eigen::MatrixXs cov_KF1(16,16), cov_KF2(16,16);
-
- problem->getFrameCovariance(KF1, cov_KF1);
- problem->getFrameCovariance(KF2, cov_KF2);
-
- Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF1, stdev_KF2;
-
- stdev_KF1 = cov_KF1.diagonal().array().sqrt();
- stdev_KF2 = cov_KF2.diagonal().array().sqrt();
-
- WOLF_DEBUG("stdev KF1 : \n", stdev_KF1.transpose());
- WOLF_DEBUG("stdev KF2 : \n", stdev_KF2.transpose());
- #endif
-
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * Second output: KF2 position standard deviation computed
- * estimated trajectory AFTER optimization
- * + get KF2 timestamp + state just in case the loop is not working as expected
- */
-
- //estimated trajectort
- time_iter = 0;
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_after << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
-
- //finally, output the timestamp, state and stdev associated to KFs
- #ifdef ADD_KF3
- checking << KF2->getTimeStamp().get() << "\t" << KF2->getState().transpose() << "\t" << stdev_KF2.transpose() << std::endl;
- checking << KF3->getTimeStamp().get() << "\t" << KF3->getState().transpose() << "\t" << stdev_KF3.transpose() << std::endl;
- #else
- checking << KF2->getTimeStamp().get() << "\t" << KF2->getState().transpose() << "\t" << stdev_KF2.transpose() << std::endl;
- #endif
- #endif
-
- // ___Are expected values in the range of estimated +/- 2*stdev ?___
-
- #ifdef OUTPUT_RESULTS
- output_results_before.close();
- output_results_after.close();
- checking.close();
- #endif
-
- return 0;
-}
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
deleted file mode 100644
index 039615445807ab6ef9e1dadab7e273135bc650ef..0000000000000000000000000000000000000000
--- a/demos/demo_imuDock_autoKFs.cpp
+++ /dev/null
@@ -1,311 +0,0 @@
-/**
- * \file test_imuDock_autoKFs.cpp
- *
- * Created on: July 22, 2017
- * \author: Dinesh Atchuthan
- */
-
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-
-//Factors headers
-#include "core/factor/factor_fix_bias.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include "core/factor/factor_pose_3D.h"
-
-#define OUTPUT_RESULTS
-//#define AUTO_KFS
-
-/* OFFLINE VERSION
- In this test, we use the experimental conditions needed for Humanoids 2017.
- IMU data are acquired using the docking station.
-
- Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
- invar : P1, V1, V2
- var : Q1,B1,P2,Q2,B2
-
- All Keyframes coming after KF2 are constrained just like KF2
- factors : Odometry factor between KeyFrames
- IMU factor
- FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
- Fix3D factor
-
- What we expect : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
- Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
-
- Representation of the application:
-
- Imu
- KF1----------â—¼----------KF2--..
- /----P1----------\ /----------P2--.. invar : P1, V1, V2
- Abs|------â—¼ â—¼ var : Q1,B1,P2,Q2,B2
- \----Q1----------/ \----------Q2--..
- V1 Odom v2 ..
- Abs|------â—¼-----B1 B2 ..
-*/
-int main(int argc, char** argv)
-{
- //#################################################### INITIALIZATION
- using namespace wolf;
-
- //___get input files for imu and odom data___
- std::ifstream imu_data_input, odom_data_input;
- const char * filename_imu;
- const char * filename_odom;
- if (argc < 03)
- {
- WOLF_ERROR("Missing input argument : path to imu or/and odom data file(s).")
- return 1; //return with error
- }
- else
- {
- filename_imu = argv[1];
- filename_odom = argv[2];
-
- imu_data_input.open(filename_imu);
- WOLF_INFO("imu file : ", filename_imu)
- odom_data_input.open(filename_odom);
- WOLF_INFO("odom file : ", filename_odom)
- }
-
- // ___Check if the file is correctly opened___
- if(!imu_data_input.is_open() || !odom_data_input.is_open()){
- WOLF_ERROR("Failed to open data file ! Exiting")
- return 1;
- }
-
- #ifdef OUTPUT_RESULTS
- //define output file
- std::ofstream output_results_before, output_results_after, checking;
- output_results_before.open("imu_dock_beforeOptim.dat");
- output_results_after.open("imu_dock_afterOptim.dat");
- checking.open("KF_pose_stdev.dat");
- #endif
-
- // ___initialize variabes that will be used through the code___
- Eigen::VectorXs problem_origin(16);
- Eigen::Vector7s imu_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished());
- problem_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0;
-
- //Create vectors to store data and time
- Eigen::Vector6s data_imu, data_odom;
- Scalar clock;
- TimeStamp ts(0), ts_output(0); //will be used to store the data timestamps and set timestamps in captures
-
- // ___Define expected values___
- Eigen::Vector7s expected_KF1_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7s()<<0,-0.06,0,0,0,0,11).finished());
-
- #ifdef AUTO_KFs
- std::array<Scalar, 50> lastms_imuData;
- #endif
- //#################################################### SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // ___Create the WOLF Problem + define origin of the problem___
- ProblemPtr problem = Problem::create("PQVBB 3D");
- CeresManager* ceres_manager = new CeresManager(problem);
-
- // ___Configure Ceres if needed___
-
- // ___Create sensors + processors___
- SensorIMUPtr sensorIMU = std::static_pointer_cast<SensorIMU>(problem->installSensor("IMU", "Main IMU", imu_pose, wolf_root + "/src/examples/sensor_imu.yaml"));
- ProcessorIMUPtr processorIMU = std::static_pointer_cast<ProcessorIMU>(problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"));
-
- SensorOdom3DPtr sensorOdom = std::static_pointer_cast<SensorOdom3D>(problem->installSensor("ODOM 3D", "odom", odom_pose, wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"));
- ProcessorOdom3DPtr processorOdom = std::static_pointer_cast<ProcessorOdom3D>(problem->installProcessor("ODOM 3D", "odom", "odom", wolf_root + "/src/examples/processor_odom_3D.yaml"));
-
- // ___set origin of processors to the problem's origin___
- FrameIMUPtr KF1 = std::static_pointer_cast<FrameIMU>(processorIMU->setOrigin(problem_origin, ts)); // XXX JS: setting ts to zero, and then reading clock from data, is inconsistent.
- processorOdom->setOrigin(KF1);
-
- //#################################################### PROCESS DATA
- // ___process IMU and odometry___
-
- //Create captures
- CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6s::Identity(), Vector6s::Zero()); //ts is set at 0
- CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(ts, sensorOdom, data_odom, 7, 6, nullptr);
-
- //while we do not reach the end of file, read IMU input (ts, Ax, Ay, Az, Wx, Wy, Wz) and process data through capture
- while(!imu_data_input.eof())
- {
- //read
- imu_data_input >> clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5];
-
- //set capture
- ts.set(clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- //process
- sensorIMU->process(imu_ptr);
- }
-
- //Process all the odom data
- // XXX JS: in my opinion, we should control the KF creation better, not using time span. Is it possible?
- while(!odom_data_input.eof())
- {
- //read
- odom_data_input >> clock >> data_odom[0] >> data_odom[1] >> data_odom[2] >> data_odom[3] >> data_odom[4] >> data_odom[5];
-
- //set capture
- ts.set(clock);
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom);
-
- #ifdef AUTO_KFS
- /* We want the KFs to be generated automatically but not using time span as argument of this generation
- * For our application, w want the KFs to be generated when an odometry data is given under condition that the IMU is not moving
- * We check wether the IMU is moving or not by computing the current stdev of the IMU based on data received during 50ms before the odom timestamp
- * We compare this value to the stdev (noise) of the sensor (see sensor_imu.yaml)
- * If the current stdev is below a threshold then we process the odometry data !
- */
-
- // TODO : get data to compute stdev with directly from the capture
- // -> see how these data are stored and change getIMUStdev(..) function defined below main() in this file
- // -> then just use the function to get this stdev of corresponding data
-
- #else
- //process anyway. KFs will be generated based on the configuration given in processor_odom_3D.yaml
- sensorOdom->process(mot_ptr);
- #endif
- }
-
- //All data have been processed, close the files
- imu_data_input.close();
- odom_data_input.close();
-
- //A KeyFrame should have been created (depending on time_span in processors). get all frames
- FrameBasePtrList trajectory = problem->getTrajectory()->getFrameList();
-
-
- //#################################################### OPTIMIZATION PART
- // ___Create needed factors___
-
- //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
- featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
- featureFix_cov(3,3) = pow( .01 , 2); // roll variance
- featureFix_cov(4,4) = pow( .01 , 2); // pitch variance
- featureFix_cov(5,5) = pow( .001 , 2); // yaw variance
- CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
- FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
-
- Eigen::MatrixXs featureFixBias_cov(6,6);
- featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
- featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
- featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
- CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
- //create a FeatureBase to factor biases
- FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
-
- // ___Fix/Unfix stateblocks___
- // fix all Keyframes here
-
- FrameIMUPtr frame_imu;
- for(auto frame : trajectory)
- {
- frame_imu = std::static_pointer_cast<FrameIMU>(frame);
- if(frame_imu->isKey())
- {
- frame_imu->getP()->fix();
- frame_imu->getO()->unfix();
- frame_imu->getV()->setState((Eigen::Vector3s()<<0,0,0).finished()); //fix all velocties to 0 ()
- frame_imu->getV()->fix();
- frame_imu->getAccBias()->unfix();
- frame_imu->getGyroBias()->unfix();
- }
- }
-
- //KF1 (origin) needs to be also fixed in position
- KF1->getP()->fix();
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * first output : estimated trajectory BEFORE optimization (getting the states each millisecond)
- */
-
- unsigned int time_iter(0);
- Scalar ms(0.001);
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_before << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
- #endif
-
- // ___Solve + compute covariances___
- problem->print(4,0,1,0);
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- problem->print(1,0,1,0);
-
- //#################################################### RESULTS PART
-
- // ___Get standard deviation from covariances___ and output this in a file
- Eigen::MatrixXs cov_KF(16,16);
- Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF;
- for(auto frame : trajectory)
- {
- if(frame->isKey())
- {
- problem->getFrameCovariance(frame, cov_KF);
- stdev_KF = cov_KF.diagonal().array().sqrt();
- #ifdef OUTPUT_RESULTS
- checking << frame->getTimeStamp().get() << "\t" << frame->getState().transpose() << "\t" << stdev_KF.transpose() << std::endl;
- #endif
- }
- }
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * Second output: KF position standard deviation computed
- * estimated trajectory AFTER optimization
- */
-
- //estimated trajectort
- time_iter = 0;
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_after << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
-
- //Finished writing in files : close them
- output_results_before.close();
- output_results_after.close();
- checking.close();
- #endif
-
- return 0;
-}
-
-/*Scalar getIMUStdev(Eigen::VectorXs _data) //input argument : whatever will contain the data in the capture
-{
- Eigen::Vector6s mean(Eigen::Vector6s::Zero()), stdev(Eigen::Vector6s::Zero());
- unsigned int _data_size(_data.size());
-
- mean = _data.mean()/_data_size;
-
- for (unsigned int i = 0; i < _data_size; i++)
- {
- stdev += pow(_data()-mean,2); //get the correct data from the container
- }
- return (stdev.array().sqrt()).maxCoeff();
-}*/
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
deleted file mode 100644
index e0e4e4778e91090221d70121bd89907ad3756823..0000000000000000000000000000000000000000
--- a/demos/demo_imuPlateform_Offline.cpp
+++ /dev/null
@@ -1,266 +0,0 @@
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-#define DEBUG_RESULTS
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- // LOADING DATA FILE (IMU)
- // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
-
- std::ifstream imu_data_input;
- const char * filename_imu;
- if (argc < 02)
- {
-
- WOLF_ERROR("Missing input argument! : needs 1 argument (path to imu data file).")
- return 1;
- }
- else
- {
- filename_imu = argv[1];
-
- imu_data_input.open(filename_imu);
- WOLF_INFO("imu file : ", filename_imu)
-
- //std::string dummy; //this is needed only if first line is headers or useless data
- //getline(imu_data_input, dummy);
- }
-
- if(!imu_data_input.is_open()){
- std::cerr << "Failed to open data files... Exiting" << std::endl;
- return 1;
- }
-
- #ifdef DEBUG_RESULTS
- std::ofstream debug_results;
- debug_results.open("debug_results_imu_constrained0.dat");
- if(debug_results)
- debug_results << "%%TimeStamp\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
- << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
- #endif
-
- //===================================================== SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // WOLF PROBLEM
- ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs x_origin(16);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; //INITIAL CONDITIONS
- TimeStamp t(0);
-
- // 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;
- CeresManager* 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");
- ProcessorIMUParamsPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 10;
- 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;
-
- ProcessorBasePtr processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", sen0_ptr, prc_imu_params);
- SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- ProcessorIMUPtr 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.yaml");
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 1.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);
- SensorOdom3DPtr sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- ProcessorOdom3DPtr processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- // reset origin of problem
- t.set(0);
-
- FrameIMUPtr origin_KF = std::static_pointer_cast<FrameIMU>(processor_ptr_imu->setOrigin(x_origin, t));
- processor_ptr_odom3D->setOrigin(origin_KF);
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu, data_odom3D;
- data_imu << 0,0,-wolf::gravity()(2), 0,0,0;
- data_odom3D << 0,-0.06,0, 0,0,0;
-
- Scalar input_clock;
- TimeStamp ts(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6s::Identity(), Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 6, 6, nullptr);
-
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- t = ts;
-
- clock_t begin = clock();
-
- while( !imu_data_input.eof())
- {
- // PROCESS IMU DATA
- // Time and data variables
- imu_data_input >> input_clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5]; //Ax, Ay, Az, Gx, Gy, Gz
-
- ts.set(input_clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- // process data in capture
- imu_ptr->getTimeStamp();
- sen_imu->process(imu_ptr);
- }
-
- TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
-
- //Finally, process the only one odom input
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-
- //closing file
- imu_data_input.close();
- //===================================================== END{PROCESS DATA}
-
- double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
- // Final state
- std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
- std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x_origin.head(16).transpose() << std::endl;
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
- std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
- // Print statistics
- std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
- std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
- /*TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();*/
- std::cout << "t0 : " << t0.get() << " s" << std::endl;
- std::cout << "tf : " << tf.get() << " s" << std::endl;
- std::cout << "duration : " << tf-t0 << " s" << std::endl;
- std::cout << "N samples : " << N << std::endl;
- std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
- std::cout << "CPU time : " << elapsed_secs << " s" << std::endl;
- std::cout << "s/integr : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
- std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- std::cout << "\t\t\t ______solving______" << std::endl;
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
- std::cout << "\t\t\t ______solved______" << std::endl;
-
- wolf_problem_ptr_->print(4,1,1,1);
-
- #ifdef DEBUG_RESULTS
- Eigen::VectorXs frm_state(16);
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev;
- Eigen::MatrixXs covX(16,16);
- Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
-
- wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
- for(FrameBasePtr frm_ptr : frame_list)
- {
- if(frm_ptr->isKey())
- {
- //prepare needed variables
- FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
- frm_state = frmIMU_ptr->getState();
- ts = frmIMU_ptr->getTimeStamp();
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
- covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
- covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
- covX.block(13,13,3,3) = cov3;
- for(int i = 0; i<16; 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)
-
- debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
- << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
- << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
- << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15)
- << "\t" << cov_stdev(0) << "\t" << cov_stdev(1) << "\t" << cov_stdev(2)
- << "\t" << cov_stdev(3) << "\t" << cov_stdev(4) << "\t" << cov_stdev(5) << "\t" << cov_stdev(6)
- << "\t" << cov_stdev(7) << "\t" << cov_stdev(8) << "\t" << cov_stdev(9)
- << "\t" << cov_stdev(10) << "\t" << cov_stdev(11) << "\t" << cov_stdev(12) << "\t" << cov_stdev(13) << "\t" << cov_stdev(14) << "\t" << cov_stdev(15) << std::endl;
- }
- }
-
- debug_results.close();
- WOLF_WARN("WARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)")
-
- #endif
-
- return 0;
-
-}
-
-int _kbhit()
-{
- struct timeval tv;
- fd_set fds;
- tv.tv_sec = 0;
- tv.tv_usec = 0;
- FD_ZERO(&fds);
- FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
- select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
- return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
deleted file mode 100644
index 817b08a14a4c71caf9f9e4807c71cc14cc43d78e..0000000000000000000000000000000000000000
--- a/demos/demo_imu_constrained0.cpp
+++ /dev/null
@@ -1,374 +0,0 @@
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-#define DEBUG_RESULTS
-#define KF0_EVOLUTION
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- // LOADING DATA FILES (IMU + ODOM)
- // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
- // FOR ODOM, file content is : Timestampt\t Δpx\t Δpy\t Δpz\t Δox\t Δoy\t Δoz
-
- std::ifstream imu_data_input;
- std::ifstream odom_data_input;
- const char * filename_imu;
- const char * filename_odom;
- if (argc < 3)
- {
- std::cout << "Missing input argument! : needs 2 argument (path to imu and odom data files)." << std::endl;
- return 1;
- }
- else
- {
- filename_imu = argv[1];
- filename_odom = argv[2];
-
- imu_data_input.open(filename_imu);
- odom_data_input.open(filename_odom);
-
- std::cout << "file imu : " << filename_imu <<"\t file odom : " << filename_odom << std::endl;
-
- //std::string dummy; //this is needed only if first line is headers or useless data
- //getline(imu_data_input, dummy);
- }
-
- if(!imu_data_input.is_open() || !odom_data_input.is_open()){
- std::cerr << "Failed to open data files... Exiting" << std::endl;
- return 1;
- }
-
- #ifdef DEBUG_RESULTS
- std::ofstream debug_results;
- debug_results.open("debug_results_imu_constrained0.dat");
- if(debug_results)
- debug_results << "%%TimeStamp\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
- << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
- #endif
-
- #ifdef KF0_EVOLUTION
- std::ofstream KF0_evolution;
- KF0_evolution.open("KF0_evolution.dat");
- if(KF0_evolution)
- KF0_evolution << "%%TimeStamp\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
- << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
- #endif
-
- //===================================================== SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // WOLF PROBLEM
- ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs x_origin(16);
- Eigen::Vector6s origin_bias;
- x_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; //INITIAL CONDITIONS 0.05,0.03,.00, 0.2,-0.05,.00;
- TimeStamp t(0);
-
- // initial conditions defined from data file
- // remember that matlab's quaternion is W,X,Y,Z and the one in Eigen has X,Y,Z,W form
- imu_data_input >> x_origin[0] >> x_origin[1] >> x_origin[2] >> x_origin[6] >> x_origin[3] >> x_origin[4] >> x_origin[5] >> x_origin[7] >> x_origin[8] >> x_origin[9];
- imu_data_input >> origin_bias[0] >> origin_bias[1] >> origin_bias[2] >> origin_bias[3] >> origin_bias[4] >> origin_bias[5];
-
- // 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;
- CeresManager* 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");
- ProcessorIMUParamsPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 10;
- 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;
-
- ProcessorBasePtr processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", sen0_ptr, prc_imu_params);
- SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- ProcessorIMUPtr 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 = 20.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);
- SensorOdom3DPtr sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- ProcessorOdom3DPtr processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- // reset origin of problem
- t.set(0);
- Eigen::Matrix<wolf::Scalar, 10, 1> expected_final_state;
-
- FrameIMUPtr origin_KF = std::static_pointer_cast<FrameIMU>(processor_ptr_imu->setOrigin(x_origin, t));
- processor_ptr_odom3D->setOrigin(origin_KF);
-
- odom_data_input >> expected_final_state[0] >> expected_final_state[1] >> expected_final_state[2] >> expected_final_state[6] >> expected_final_state[3] >>
- expected_final_state[4] >> expected_final_state[5] >> expected_final_state[7] >> expected_final_state[8] >> expected_final_state[9];
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu, data_odom3D;
- data_imu << 0,0,-wolf::gravity()(2), 0,0,0;
- data_odom3D << 0,0,0, 0,0,0;
-
- Scalar input_clock;
- TimeStamp ts(0);
- TimeStamp t_odom(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6s::Identity(), Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 6, 6, nullptr);
-
- //read first odom data from file
- odom_data_input >> input_clock >> data_odom3D[0] >> data_odom3D[1] >> data_odom3D[2] >> data_odom3D[3] >> data_odom3D[4] >> data_odom3D[5];
- t_odom.set(input_clock);
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- t = ts;
-
- clock_t begin = clock();
-
- while( !imu_data_input.eof() && !odom_data_input.eof() )
- {
- // PROCESS IMU DATA
- // Time and data variables
- imu_data_input >> input_clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5]; //Ax, Ay, Az, Gx, Gy, Gz
-
- ts.set(input_clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- // process data in capture
- imu_ptr->getTimeStamp();
- sen_imu->process(imu_ptr);
-
- if(ts.get() == t_odom.get())
- {
- // PROCESS ODOM 3D DATA
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement if there is any
- odom_data_input >> input_clock >> data_odom3D[0] >> data_odom3D[1] >> data_odom3D[2] >> data_odom3D[3] >> data_odom3D[4] >> data_odom3D[5];
- t_odom.set(input_clock);
- }
-
- #ifdef KF0_EVOLUTION
-
- if( (ts.get() - t.get()) >= 0.05 )
- {
- t = ts;
- //std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
-
- Eigen::VectorXs frm_state(16);
- frm_state = origin_KF->getState();
-
- KF0_evolution << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
- << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
- << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
- << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15) << std::endl;
- }
-
- #endif
- }
-
- clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-
- //closing file
- imu_data_input.close();
- odom_data_input.close();
-
- #ifdef KF0_EVOLUTION
- KF0_evolution.close();
- #endif
-
- //===================================================== END{PROCESS DATA}
-
- double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
- // Final state
- std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
- std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x_origin.head(16).transpose() << std::endl;
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
- std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
-
- // Print statistics
- std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
- std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
- TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
- std::cout << "t0 : " << t0.get() << " s" << std::endl;
- std::cout << "tf : " << tf.get() << " s" << std::endl;
- std::cout << "duration : " << tf-t0 << " s" << std::endl;
- std::cout << "N samples : " << N << std::endl;
- std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
- std::cout << "CPU time : " << elapsed_secs << " s" << std::endl;
- std::cout << "s/integr : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
- std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
-
- std::cout << "\t\t\t ______solving______" << std::endl;
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL); //0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
-
- last_KF->getAccBias()->fix();
- last_KF->getGyroBias()->fix();
-
- std::cout << "\t\t\t solving after fixBias" << std::endl;
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); //0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
- std::cout << "\t\t\t ______solved______" << std::endl;
-
- wolf_problem_ptr_->print(4,1,1,1);
-
- #ifdef DEBUG_RESULTS
- Eigen::VectorXs frm_state(16);
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev;
- Eigen::MatrixXs covX(16,16);
- Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
-
- wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
- for(FrameBasePtr frm_ptr : frame_list)
- {
- if(frm_ptr->isKey())
- {
- //prepare needed variables
- FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
- frm_state = frmIMU_ptr->getState();
- ts = frmIMU_ptr->getTimeStamp();
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
- covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
- covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
- covX.block(13,13,3,3) = cov3;
- for(int i = 0; i<16; 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)
-
- debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
- << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
- << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
- << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15)
- << "\t" << cov_stdev(0) << "\t" << cov_stdev(1) << "\t" << cov_stdev(2)
- << "\t" << cov_stdev(3) << "\t" << cov_stdev(4) << "\t" << cov_stdev(5) << "\t" << cov_stdev(6)
- << "\t" << cov_stdev(7) << "\t" << cov_stdev(8) << "\t" << cov_stdev(9)
- << "\t" << cov_stdev(10) << "\t" << cov_stdev(11) << "\t" << cov_stdev(12) << "\t" << cov_stdev(13) << "\t" << cov_stdev(14) << "\t" << cov_stdev(15) << std::endl;
- }
- }
-
- //trials to print all factorIMUs' residuals
- Eigen::Matrix<wolf::Scalar,15,1> IMU_residuals;
- Eigen::Vector3s p1(Eigen::Vector3s::Zero());
- Eigen::Vector4s q1_vec(Eigen::Vector4s::Zero());
- Eigen::Map<Quaternions> q1(q1_vec.data());
- Eigen::Vector3s v1(Eigen::Vector3s::Zero());
- Eigen::Vector3s ab1(Eigen::Vector3s::Zero());
- Eigen::Vector3s wb1(Eigen::Vector3s::Zero());
- Eigen::Vector3s p2(Eigen::Vector3s::Zero());
- Eigen::Vector4s q2_vec(Eigen::Vector4s::Zero());
- Eigen::Map<Quaternions> q2(q2_vec.data());
- Eigen::Vector3s v2(Eigen::Vector3s::Zero());
- Eigen::Vector3s ab2(Eigen::Vector3s::Zero());
- Eigen::Vector3s wb2(Eigen::Vector3s::Zero());
-
- for(FrameBasePtr frm_ptr : frame_list)
- {
- if(frm_ptr->isKey())
- {
- FactorBasePtrList fac_list = frm_ptr->getConstrainedByList();
- for(FactorBasePtr fac_ptr : fac_list)
- {
- if(fac_ptr->getTypeId() == FAC_IMU)
- {
- //Eigen::VectorXs prev_KF_state(fac_ptr->getFrameOther()->getState());
- //Eigen::VectorXs curr_KF_state(fac_ptr->getFeature()->getFrame()->getState());
- p1 = fac_ptr->getFrameOther()->getP()->getState();
- q1_vec = fac_ptr->getFrameOther()->getO()->getState();
- v1 = fac_ptr->getFrameOther()->getV()->getState();
- ab1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getAccBias()->getState();
- wb1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getGyroBias()->getState();
-
- p2 = fac_ptr->getFeature()->getFrame()->getP()->getState();
- q2_vec = fac_ptr->getFeature()->getFrame()->getO()->getState();
- v2 = fac_ptr->getFeature()->getFrame()->getV()->getState();
- ab2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getAccBias()->getState();
- wb2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getGyroBias()->getState();
-
- std::static_pointer_cast<FactorIMU>(fac_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
- std::cout << "IMU residuals : " << IMU_residuals.transpose() << std::endl;
- }
- }
- }
- }
-
- debug_results.close();
- WOLF_WARN("WARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)")
-
- #endif
-
- return 0;
-
-}
-
-int _kbhit()
-{
- struct timeval tv;
- fd_set fds;
- tv.tv_sec = 0;
- tv.tv_usec = 0;
- FD_ZERO(&fds);
- FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
- select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
- return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_kf_callback.cpp b/demos/demo_kf_callback.cpp
deleted file mode 100644
index 9e01558e3bc013c65d553d7e213785fdacfaaf96..0000000000000000000000000000000000000000
--- a/demos/demo_kf_callback.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * test_kf_callback.cpp
- *
- * Created on: Nov 6, 2016
- * Author: jsola
- */
-
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
-#include "core/capture/capture_void.h"
-
-int main()
-{
- using namespace wolf;
- using namespace Eigen;
- using namespace std;
-
- ProblemPtr problem = Problem::create("PO", 2);
-
- SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
- ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
- params_odo->max_time_span = 2;
- params_odo->angle_turned = M_PI; // 180 degrees turn
- ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", sen_odo, params_odo);
- prc_odo->setTimeTolerance(0.1);
-
- SensorBasePtr sen_ftr = problem->installSensor ("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),"");
- ProcessorParamsTrackerFeaturePtr params_trk = std::make_shared<ProcessorParamsTrackerFeature>();
- params_trk->max_new_features = 4;
- params_trk->min_features_for_keyframe = 7;
- params_trk->time_tolerance = 0.5;
- shared_ptr<ProcessorTrackerFeatureDummy> prc_ftr = make_shared<ProcessorTrackerFeatureDummy>(params_trk);
- prc_ftr->setName("tracker");
- sen_ftr->addProcessor(prc_ftr);
- prc_ftr->setTimeTolerance(0.1);
-
- cout << "Motion sensor : " << problem->getProcessorMotion()->getSensor()->getName() << endl;
- cout << "Motion processor : " << problem->getProcessorMotion()->getName() << endl;
-
- TimeStamp t(0);
- cout << "=======================\n>> TIME: " << t.get() << endl;
- Vector3s x({0,0,0});
- Matrix3s P; P.setZero();
- problem->setPrior(x, P, t, 0.01);
-
- cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
-
- Vector2s odo_data; odo_data << .1, (M_PI / 10);
-
- problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
-
- Scalar dt = 1;
- for (auto i = 0; i < 4; i++)
- {
-
- cout << "Tracker----------------" << endl;
- sen_ftr->process(make_shared<CaptureVoid>(t, sen_ftr));
- problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
-
- t += dt;
- cout << "=======================\n>> TIME: " << t.get() << endl;
-
- cout << "Motion-----------------" << endl;
- sen_odo->process(make_shared<CaptureMotion>("ODOM 2D", t, sen_odo, odo_data, 3, 3, nullptr));
- cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
- problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
-
- }
-
- return 0;
-}
diff --git a/demos/demo_list_remove.cpp b/demos/demo_list_remove.cpp
deleted file mode 100644
index b0795d0a687e92b774c50de49c2645a488676ced..0000000000000000000000000000000000000000
--- a/demos/demo_list_remove.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * test_list_remove.cpp
- *
- * Created on: Nov 19, 2016
- * Author: jsola
- */
-
-#include <memory>
-#include <list>
-#include <algorithm>
-#include <iostream>
-
-int main()
-{
- using std::list;
- using std::shared_ptr;
- using std::make_shared;
- using std::cout;
- using std::endl;
-
- typedef shared_ptr<int> IntPtr;
-
- list<IntPtr> L;
-
- L.push_back(make_shared<int>(0));
- L.push_back(make_shared<int>(1));
- L.push_back(make_shared<int>(2));
-
- cout << "size: " << L.size() << endl;
-
-// for (auto p : L)
-// {
-// cout << "removing " << *p << endl;
-// L.remove(p);
-// cout << "size: " << L.size() << endl;
-// }
-
-// list<IntPtr>::iterator it = L.begin();
-// while (it != L.end())
-// {
-// cout << "removing " << **it << endl;
-// L.erase(it);
-// it = L.begin();
-// cout << "size: " << L.size() << endl;
-// }
-
- list<IntPtr> L_black;
- for (auto p : L)
- {
- cout << "marking " << *p << endl;
- L_black.push_back(p);
- cout << "size: " << L.size() << endl;
- }
- for (auto p : L_black)
- {
- cout << "removing " << *p << endl;
- L.remove(p);
- cout << "size: " << L.size() << endl;
- }
- return 0;
-}
diff --git a/demos/demo_map_yaml.cpp b/demos/demo_map_yaml.cpp
deleted file mode 100644
index e509c94e2116b2f7bc08e70d866eafb5ed5a9bfb..0000000000000000000000000000000000000000
--- a/demos/demo_map_yaml.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-/**
- * \file test_map_yaml.cpp
- *
- * Created on: Jul 27, 2016
- * \author: jsola
- */
-
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_polyline_2D.h"
-#include "core/landmark/landmark_AHP.h"
-#include "core/state_block/state_block.h"
-#include "core/yaml/yaml_conversion.h"
-
-#include <iostream>
-using namespace wolf;
-
-void print(MapBase& _map)
-{
- for (auto lmk_ptr : _map.getLandmarkList())
- {
- std::cout << "Lmk ID: " << lmk_ptr->id();
- std::cout << "\nLmk type: " << lmk_ptr->getType();
- if (lmk_ptr->getType() == "POLYLINE 2D")
- {
- LandmarkPolyline2DPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
- std::cout << "\npos: " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
- std::cout << "\nori: " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
- std::cout << "\nn points: " << poly_ptr->getNPoints();
- std::cout << "\nFirst idx: " << poly_ptr->getFirstId();
- std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
- std::cout << "\nLast def: " << poly_ptr->isLastDefined();
- for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
- std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
- break;
- }
- else if (lmk_ptr->getType() == "AHP")
- {
- LandmarkAHPPtr ahp_ptr = std::static_pointer_cast<LandmarkAHP>(lmk_ptr);
- std::cout << "\npos: " << ahp_ptr->getP()->getState().transpose() << " -- fixed: " << ahp_ptr->getP()->isFixed();
- std::cout << "\ndescript: " << ahp_ptr->getCvDescriptor().t();
- break;
- }
- else
- break;
-
- std::cout << std::endl;
- }
-}
-
-int main()
-{
- using namespace Eigen;
-
- std::cout << "\nTesting Lmk creator and node saving..." << std::endl;
- Vector4s v;
- v << 1, 2, 3, 4;
- cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
- LandmarkAHP lmk_1(v, nullptr, nullptr, d);
- std::cout << "Pos 1 = " << lmk_1.getP()->getState().transpose() << std::endl;
- std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
-
- YAML::Node n = lmk_1.saveToYaml();
- std::cout << "Pos n = " << n["position"].as<VectorXs>().transpose() << std::endl;
- std::cout << "Des n = " << n["descriptor"].as<VectorXs>().transpose() << std::endl;
-
- LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
- std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
- std::cout << "Des 2 = " << lmk_2.getCvDescriptor().t() << std::endl;
-
- std::string filename;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string wolf_config = wolf_root + "/src/examples";
- std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
-
- ProblemPtr problem = Problem::create("PO", 2);
- filename = wolf_config + "/map_polyline_example.yaml";
- std::cout << "Reading map from file: " << filename << std::endl;
- problem->loadMap(filename);
-
- std::cout << "Printing map..." << std::endl;
- print(*(problem->getMap()));
-
- filename = wolf_config + "/map_polyline_example_write.yaml";
- std::cout << "Writing map to file: " << filename << std::endl;
- std::string thisfile = __FILE__;
- problem->getMap()->save(filename, "Example generated by test file " + thisfile);
-
- std::cout << "Clearing map... " << std::endl;
- problem->getMap()->getLandmarkList().clear();
-
- std::cout << "Re-reading map from file: " << filename << std::endl;
- problem->loadMap(filename);
-
- std::cout << "Printing map..." << std::endl;
- print(*(problem->getMap()));
-
- return 0;
-}
diff --git a/demos/demo_matrix_prod.cpp b/demos/demo_matrix_prod.cpp
deleted file mode 100644
index b03068283ac33b65a430534a3deb8cf0856195ed..0000000000000000000000000000000000000000
--- a/demos/demo_matrix_prod.cpp
+++ /dev/null
@@ -1,187 +0,0 @@
-/**
- * \file test_matrix_prod.cpp
- *
- * Created on: May 26, 2016
- * \author: jsola
- */
-
-#include "eigen3/Eigen/Dense"
-
-//std includes
-#include <ctime>
-#include <iostream>
-#include <iomanip>
-
-#include <eigen3/Eigen/StdVector>
-using namespace Eigen;
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,1,1,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,2,2,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,3,3,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,4,4,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,5,5,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,6,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,7,7,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,8,8,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,9,RowMajor>)
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,1,1,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,2,2,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,3,3,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,4,4,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,5,5,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,6,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,7,7,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,8,8,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,9,ColMajor>)
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,2,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,3,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,4,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,5,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,7,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,8,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,1>)
-
-#define DECLARE_MATRICES(s) \
- Matrix<double, s, s, RowMajor> R1, R2, Ro; \
- Matrix<double, s, s, ColMajor> C1, C2, Co;
-
-#define INIT_MATRICES(s) \
- R1.setRandom(s, s);\
- R2.setRandom(s, s);\
- C1.setRandom(s, s);\
- C2.setRandom(s, s);\
- Ro.setRandom(s, s);\
- Co.setRandom(s, s);
-
-#define LOOP_MATRIX(N,Mo,M1,M2) \
- for (int i = 0; i < N; i++) \
- { \
- Mo = M1 * M2; \
- M1(2,2) = Mo(2,2); \
- }
-
-#define EVALUATE_MATRIX(N,Mo,M1,M2) \
- t0 = clock(); \
- LOOP_MATRIX(N,Mo,M1,M2) \
- t1 = clock(); \
- std::cout << std::setw(15) << Mo(2,2) << "\t";
-
-#define EVALUATE_ALL \
- EVALUATE_MATRIX(N, Ro, R1, R2)\
- std::cout << "Time Ro = R * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
- EVALUATE_MATRIX(N, Ro, R1, C2)\
- std::cout << "Time Ro = R * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
- EVALUATE_MATRIX(N, Ro, C1, R2)\
- std::cout << "Time Ro = C * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
- EVALUATE_MATRIX(N, Ro, C1, C2)\
- std::cout << "Time Ro = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
- EVALUATE_MATRIX(N, Co, R1, R2)\
- std::cout << "Time Co = R * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
- EVALUATE_MATRIX(N, Co, R1, C2)\
- std::cout << "Time Co = R * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
- EVALUATE_MATRIX(N, Co, C1, R2)\
- std::cout << "Time Co = C * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
- EVALUATE_MATRIX(N, Co, C1, C2)\
- std::cout << "Time Co = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N \
- << "ns <-- this is the Eigen default!" << std::endl;
-
-/**
- * We multiply matrices and see how long it takes.
- * We compare different combinations of row-major and column-major to see which one is the fastest.
- * We can select the matrix size.
- */
-int main()
-{
- using namespace Eigen;
-
- int N = 100*1000;
- const int S = 6;
- Matrix<double, 16, S - 3 + 1> results;
- clock_t t0, t1;
-
- // All dynamic sizes
- {
- Matrix<double, Dynamic, Dynamic, RowMajor> R1, R2, Ro;
- Matrix<double, Dynamic, Dynamic, ColMajor> C1, C2, Co;
-
- for (int s = 3; s <= S; s++)
- {
- std::cout << "Timings for dynamic matrix product. R: row major matrix. C: column major matrix. " << s << "x"
- << s << " matrices." << std::endl;
-
- INIT_MATRICES(s)
- EVALUATE_ALL
-
- }
- }
- // Statics, one by one
- {
- const int s = 3;
- std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
- << " matrices." << std::endl;
-
- DECLARE_MATRICES(s)
- INIT_MATRICES(s)
- EVALUATE_ALL
- }
- {
- const int s = 4;
- std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
- << " matrices." << std::endl;
-
- DECLARE_MATRICES(s)
- INIT_MATRICES(s)
- EVALUATE_ALL
- }
- {
- const int s = 5;
- std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
- << " matrices." << std::endl;
-
- DECLARE_MATRICES(s)
- INIT_MATRICES(s)
- EVALUATE_ALL
- }
- {
- const int s = 6;
- std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
- << " matrices." << std::endl;
-
- DECLARE_MATRICES(s)
- INIT_MATRICES(s)
- EVALUATE_ALL
- }
-
- std::cout << "Test q and R rotations" << std::endl;
- Eigen::Quaterniond q(Eigen::Vector4d::Random().normalized());
- Eigen::Matrix3d R = q.matrix();
- Eigen::Vector3d v0; v0.setRandom(); v0.normalize(); double v0n = v0.norm();
- Eigen::Vector3d v;
-
- N *= 100;
-
- v = v0;
- t0 = clock();
- for (int i = 0; i < N; i++)
- {
- v = R * v;
- }
- t1 = clock();
- std::cout << "Time w = R * v: " << (double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;
- std::cout << "v norm change: " << 10*logl((long double)v.norm()/(long double)v0n) << " dB" << std::endl;
-
- v = v0;
- t0 = clock();
- for (int i = 0; i < N; i++)
- {
- v = q * v;
- }
- t1 = clock();
- std::cout << "Time w = q * v: " << (double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;
- std::cout << "v norm change: " << 10*logl((long double)v.norm()/(long double)v0n) << " dB" << std::endl;
- return 0;
-}
-
diff --git a/demos/demo_mpu.cpp b/demos/demo_mpu.cpp
deleted file mode 100644
index f2d5bbade62637c592b5f2dd0cf2341d825dded8..0000000000000000000000000000000000000000
--- a/demos/demo_mpu.cpp
+++ /dev/null
@@ -1,233 +0,0 @@
-/**
- * \file test_mpu.cpp
- *
- * Created on: Oct 4, 2016
- * \author: AtDinesh
- */
-
- //Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-#include <termios.h>
-#include <fcntl.h>
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-
-#define DEBUG_RESULTS
-#define FROM_FILE
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- #ifdef FROM_FILE
- std::ifstream data_file;
- const char * filename;
-
- if (argc < 2)
- {
- std::cout << "Missing input argument! : needs 1 argument (path to data file)." << std::endl;
- return 1;
- }
- else
- {
- filename = argv[1];
- data_file.open(filename);
- std::cout << "file: " << filename << std::endl;
-
- std::string dummy;
- getline(data_file, dummy);
-
- if(!data_file.is_open()){
- std::cerr << "Failed to open data files... Exiting" << std::endl;
- return 1;
- }
- }
- #else
- int fd,n;
- ///prepare MPU here
- if (argc < 2)
- {
- std::cout << "Missing input argument! : needs 1 argument : way to MPU device. (usually /dev/ttyACM#)\n Please make sure that you have rights to access the device and that your user belongs to the dialout group." << std::endl;
- return 1;
- }
- unsigned char buf[64] = {0};
- wolf::Scalar gravity = 9.81;
- wolf::Scalar sec_to_rad = 3.14159265359/180.0;
- wolf::Scalar accel_LSB = 1.0/8192.0; // = 4.0/32768.0
- wolf::Scalar gyro_LSB = 1.0/131.0; // = 250.0/32768.0
- wolf::Scalar accel_LSB_g = accel_LSB * gravity;
- wolf::Scalar gyro_LSB_rad = gyro_LSB * sec_to_rad;
- //wolf::Scalar Ax, Ay, Az, Gx, Gy, Gz;
-
- struct termios toptions;
- //open serial port
- std::cout << "open port...\n" << std::endl;
- fd = open(argv[1], O_RDWR | O_NOCTTY);
- if (fd != -1)
- std::cout << "MPU openned successfully! \n" << std::endl;
- else
- std::cout << "MPU could not be openned... \n" << std::endl;
-
- //configuring termios
- tcgetattr(fd, &toptions);
- cfsetispeed(&toptions, B1000000);
- cfsetospeed(&toptions, B1000000);
- toptions.c_cflag |= (CLOCAL | CREAD);
- toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
- toptions.c_oflag &= ~OPOST;
- toptions.c_cc[VMIN] = 0;
- toptions.c_cc[VTIME] = 10;
- tcsetattr(fd, TCSANOW, &toptions);
- #endif
-
- #ifdef DEBUG_RESULTS
- std::ofstream debug_results;
- debug_results.open("debug_results.dat");
- if(debug_results)
- debug_results << "%%TimeStamp\t"
- << "dp_x\t" << "dp_y\t" << "dp_z\t" << "dv_x\t" << "dv_y\t" << "dv_z\t" << "dq_x\t" << "dq_y\t" << "dq_z\t" << "dq_w\t"
- << "Dp_x\t" << "Dp_y\t" << "Dp_z\t" << "Dv_x\t" << "Dv_y\t" << "Dv_z\t" << "Dq_x\t" << "Dq_y\t" << "Dq_z\t" << "Dq_w\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << std::endl;
- #endif
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, IntrinsicsBase());
- wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
- // Time and data variables
- TimeStamp t;
- Eigen::Vector6s data_;
- Scalar mpu_clock = 0;
-
- t.set(mpu_clock * 0.0001); // clock in 0,1 ms ticks
-
- // Set the origin
- Eigen::VectorXs x0(16);
- x0 << 0,0,0, 0,0,0, 1,0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
- wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_) );
-
- // main loop
- using namespace std;
- clock_t begin = clock();
- std::cout << "\n\t\t\t\tENTERING MAIN LOOP - Please press ENTER to exit loop\n" << std::endl;
-
- #ifdef FROM_FILE
- while(!data_file.eof()){
- // read new data
- data_file >> mpu_clock >> data_[0] >> data_[1] >> data_[2] >> data_[3] >> data_[4] >> data_[5];
- t.set(mpu_clock); //
- #else
- while(!_kbhit()){
- // read new data
- do n = read(fd, buf, 1);//READ IT
- while (buf[0]!=0x47); //control character has been found
- n = read(fd, buf, 12);//read the data
- if (n>3){ //construct data_ from IMU input
- data_(0) = (wolf::Scalar)((int16_t)((buf[1]<<8)|buf[0]))*accel_LSB_g;
- data_(1) = (wolf::Scalar)((int16_t)((buf[3]<<8)|buf[2]))*accel_LSB_g;
- data_(2) = (wolf::Scalar)((int16_t)((buf[5]<<8)|buf[4]))*accel_LSB_g;
- data_(3) = (wolf::Scalar)((int16_t)((buf[7]<<8)|buf[6]))*gyro_LSB_rad;
- data_(4) = (wolf::Scalar)((int16_t)((buf[9]<<8)|buf[8]))*gyro_LSB_rad;
- data_(5) = (wolf::Scalar)((int16_t)((buf[11]<<8)|buf[10]))*gyro_LSB_rad;
- mpu_clock += 0.001;
- t.set(mpu_clock);
- }
- #endif
-
- // assign data to capture
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sensor_ptr->process(imu_ptr);
-
- #ifdef DEBUG_RESULTS
-
- Eigen::VectorXs delta_debug;
- Eigen::VectorXs delta_integr_debug;
- Eigen::VectorXs x_debug;
- TimeStamp ts;
-
- delta_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_;
- delta_integr_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
- x_debug = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
- ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-
- if(debug_results)
- debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
- << delta_debug(5) << "\t" << delta_debug(6) << "\t" << delta_debug(7) << "\t" << delta_debug(8) << "\t" << delta_debug(9) << "\t"
- << delta_integr_debug(0) << "\t" << delta_integr_debug(1) << "\t" << delta_integr_debug(2) << "\t" << delta_integr_debug(3) << "\t" << delta_integr_debug(4) << "\t"
- << delta_integr_debug(5) << "\t" << delta_integr_debug(6) << "\t" << delta_integr_debug(7) << "\t" << delta_integr_debug(8) << "\t" << delta_integr_debug(9) << "\t"
- << x_debug(0) << "\t" << x_debug(1) << "\t" << x_debug(2) << "\t" << x_debug(3) << "\t" << x_debug(4) << "\t"
- << x_debug(5) << "\t" << x_debug(6) << "\t" << x_debug(7) << "\t" << x_debug(8) << "\t" << x_debug(9) << "\n";
- #endif
- }
-
- clock_t end = clock();
- double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
- // Final state
- std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
- std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x0.head(16).transpose() << std::endl;
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
- std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
- // Print statistics
- std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
- std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
-#ifdef DEBUG_RESULTS
- std::cout << "\t\tWARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)" << std::endl;
- debug_results.close();
-#endif
-
- TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
- std::cout << "t0 : " << t0.get() << " s" << std::endl;
- std::cout << "tf : " << tf.get() << " s" << std::endl;
- std::cout << "duration : " << tf-t0 << " s" << std::endl;
- std::cout << "N samples : " << N << std::endl;
- std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
- std::cout << "CPU time : " << elapsed_secs << " s" << std::endl;
- std::cout << "s/integr : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
- std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
- return 0;
-
-}
-
-int _kbhit()
-{
- struct timeval tv;
- fd_set fds;
- tv.tv_sec = 0;
- tv.tv_usec = 0;
- FD_ZERO(&fds);
- FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
- select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
- return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp
deleted file mode 100644
index ec77aedeee3242daffe10d89c1444fa9ea1bf1f9..0000000000000000000000000000000000000000
--- a/demos/demo_processor_imu.cpp
+++ /dev/null
@@ -1,214 +0,0 @@
-/**
- * \file test_processor_imu.cpp
- *
- * Created on: Apr 12, 2016
- * \author: dtsbourg
- */
-
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::cout << std::endl << "==================== processor IMU test ======================" << std::endl;
-
- //load files containing accelerometer and gyroscope data
- std::ifstream data_file_acc;
- std::ifstream data_file_gyro;
- const char * filename_acc;
- const char * filename_gyro;
-
- //prepare creation of file if DEBUG_RESULTS activated
-#ifdef DEBUG_RESULTS
- std::ofstream debug_results;
- debug_results.open("debug_results.dat");
- if(debug_results)
- debug_results << "%%TimeStamp\t"
- << "dp_x\t" << "dp_y\t" << "dp_z\t" << "dq_x\t" << "dq_y\t" << "dq_z\t" << "dq_w\t" << "dv_x\t" << "dv_y\t" << "dv_z\t"
- << "Dp_x\t" << "Dp_y\t" << "Dp_z\t" << "Dq_x\t" << "Dq_y\t" << "Dq_z\t" << "Dq_w\t" << "Dv_x\t" << "Dv_y\t" << "Dv_z\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t" << std::endl;
-#endif
-
- if (argc < 3)
- {
- std::cout << "Missing input argument! : needs 2 arguments (path to accelerometer file and path to gyroscope data)." << std::endl;
- }
- else
- {
- filename_acc = argv[1];
- filename_gyro = argv[2];
- data_file_acc.open(filename_acc);
- data_file_gyro.open(filename_gyro);
- std::cout << "Acc file: " << filename_acc << std::endl;
- std::cout << "Gyro file: " << filename_gyro << std::endl;
-
- std::string dummy;
- getline(data_file_acc, dummy); getline(data_file_gyro, dummy);
-
- if(!data_file_acc.is_open() || !data_file_gyro.is_open()){
- std::cerr << "Failed to open data files... Exiting" << std::endl;
- return 1;
- }
- }
-
- // Wolf problem
- ProblemPtr problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs extrinsics(7);
- extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBasePtr sensor_ptr = problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, IntrinsicsBasePtr());
- problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
- // Time and data variables
- TimeStamp t;
- Scalar mti_clock, tmp;
- Eigen::Vector6s data;
- Eigen::Matrix6s data_cov;
- data_cov.setIdentity();
- data_cov.topLeftCorner(3,3) *= 0.01;
- data_cov.bottomRightCorner(3,3) *= 0.01;
-
- // Get initial data
- data_file_acc >> mti_clock >> data[0] >> data[1] >> data[2];
- data_file_gyro >> tmp >> data[3] >> data[4] >> data[5];
- t.set(mti_clock * 0.0001); // clock in 0,1 ms ticks
-
- // Set the origin
- Eigen::VectorXs x0(16);
- x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases
- problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- CaptureIMUPtr imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
-
-// problem_ptr_->print();
-
- std::cout << "Main loop -----------" << std::endl;
-
- // main loop
- using namespace std;
- clock_t begin = clock();
- int n = 1;
- while(!data_file_acc.eof() && n < 5000){
- n++;
-
- // read new data
- data_file_acc >> mti_clock >> data[0] >> data[1] >> data[2];
- data_file_gyro >> tmp >> data[3] >> data[4] >> data[5];
- t.set(mti_clock * 0.0001); // clock in 0,1 ms ticks
-
-// data.setZero();
-// data(2) = 9.8;
-
- // assign data to capture
- imu_ptr->setData(data);
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sensor_ptr->process(imu_ptr);
-
-#ifdef DEBUG_RESULTS
-
- // --- print to screen ----
-
- std::cout << "Current data : " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
- << data.transpose() << std::endl;
-
- std::cout << "Current delta: " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
- << problem_ptr_->getProcessorMotion()->getMotion().delta_.transpose() << std::endl;
-
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
-
- Eigen::VectorXs x = problem_ptr_->getProcessorMotion()->getCurrentState();
-
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x.head(10).transpose() << std::endl;
-
- std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
- std::cout << std::endl;
-
-//#ifdef DEBUG_RESULTS
- // ----- dump to file -----
-
- Eigen::VectorXs delta_debug;
- Eigen::VectorXs delta_integr_debug;
- Eigen::VectorXs x_debug;
- TimeStamp ts;
-
- delta_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_;
- delta_integr_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
- x_debug = problem_ptr_->getProcessorMotion()->getCurrentState();
- ts = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-
- if(debug_results)
- debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
- << delta_debug(5) << "\t" << delta_debug(6) << "\t" << delta_debug(7) << "\t" << delta_debug(8) << "\t" << delta_debug(9) << "\t"
- << delta_integr_debug(0) << "\t" << delta_integr_debug(1) << "\t" << delta_integr_debug(2) << "\t" << delta_integr_debug(3) << "\t" << delta_integr_debug(4) << "\t"
- << delta_integr_debug(5) << "\t" << delta_integr_debug(6) << "\t" << delta_integr_debug(7) << "\t" << delta_integr_debug(8) << "\t" << delta_integr_debug(9) << "\t"
- << x_debug(0) << "\t" << x_debug(1) << "\t" << x_debug(2) << "\t" << x_debug(3) << "\t" << x_debug(4) << "\t"
- << x_debug(5) << "\t" << x_debug(6) << "\t" << x_debug(7) << "\t" << x_debug(8) << "\t" << x_debug(9) << "\n";
-#endif
-
- }
- clock_t end = clock();
- double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
- // Final state
- std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
- std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x0.head(16).transpose() << std::endl;
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
-// std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
-// << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
- // Print statistics
- std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
- std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
-#ifdef DEBUG_RESULTS
- std::cout << "\t\tWARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)" << std::endl;
- debug_results.close();
-#endif
-
- TimeStamp t0, tf;
- t0 = problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
- tf = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- int N = problem_ptr_->getProcessorMotion()->getBuffer().get().size();
- std::cout << "t0 : " << t0.get() << " s" << std::endl;
- std::cout << "tf : " << tf.get() << " s" << std::endl;
- std::cout << "duration : " << tf-t0 << " s" << std::endl;
- std::cout << "N samples : " << N << std::endl;
- std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
- std::cout << "CPU time : " << elapsed_secs << " s" << std::endl;
- std::cout << "s/integr : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
- std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
- // close data files
- data_file_acc.close(); // no impact on leaks
- data_file_gyro.close();
-
- return 0;
-}
diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp
deleted file mode 100644
index 988244d1598d3a8a9a1bdebf0144266b6744044b..0000000000000000000000000000000000000000
--- a/demos/demo_processor_imu_jacobians.cpp
+++ /dev/null
@@ -1,418 +0,0 @@
-/**
- * \file test_processor_imu_jacobians.cpp
- *
- * Created on: Sep 26, 2016
- * \author: AtDinesh
- */
-
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include <test/processor_IMU_UnitTester.h>
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-using namespace wolf;
-
-void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0);
-void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place );
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << std::endl << "==================== processor IMU : Checking Jacobians ======================" << std::endl;
-
- TimeStamp t;
- Eigen::Vector6s data_;
- wolf::Scalar deg_to_rad = 3.14159265359/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("PQVBB 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- //SensorBase* sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, nullptr);
- //wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
- // Set the origin
- t.set(0.0001); // clock in 0,1 ms ticks
- Eigen::VectorXs x0(16);
- x0 << 0,1,0, 0,0,0,1, 1,0,0, 0,0,.000, 0,0,.000; // P Q V B B
-
- //wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
-
- //CaptureIMU* imu_ptr;
-
- ProcessorIMU_UnitTester processor_imu;
- //processor_imu.setOrigin(x0, t);
- wolf::Scalar ddelta_bias = 0.00000001;
- wolf::Scalar 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;
- //Delta0 << 0,0,0, 0,0,0,1, 0,0,0;
- 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;
-
- struct IMU_jac_bias bias_jac = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
-
- Eigen::Map<Eigen::Quaternions> Dq0(NULL);
- Eigen::Map<Eigen::Quaternions> dq0(NULL);
- Eigen::Map<Eigen::Quaternions> Dq_noisy(NULL);
- Eigen::Map<Eigen::Quaternions> dq_noisy(NULL);
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
-
- /* 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
- */
-
- Eigen::Matrix3s dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dab, dDq_dwb;
-
- /*
- 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
- */
-
- std::cout << "\n input data : \n" << data_ << std::endl;
-
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
- 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;
- dDv_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
- dDp_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
- dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
- 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;
-
- 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;
- //std::cout << "matrix operation result :" << i << "\n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
- //std::cout << "matrix operation result to vector :" << i << "\n" << R2v( q_in_1.matrix().transpose() * q_in_2.matrix()) << std::endl;
- }
-/* Delta1 = bias_jac.Deltas_noisy_vect_(3);
- Delta2 = bias_jac.Deltas_noisy_vect_(4);
- Delta3 = bias_jac.Deltas_noisy_vect_(5);
-
- if( (Delta1 == Delta2 || Delta1 == Delta3 ))
- std::cout << "\n problem" << std::endl;
- else
- std::cout << "\n no problem" << std::endl;
- if(Delta2 == Delta3)
- std::cout << "\n problem" << std::endl;
- else
- std::cout << "\n no problem" << std::endl;*/
-
- //Check the jacobians wrt to bias using finite difference
-
- if(dDp_dab.isApprox(bias_jac.dDp_dab_, wolf::Constants::EPS) )
- std::cout<< "dDp_dab_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dab_ jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dab : \n" << dDp_dab << "\n bias_jac.dDp_dab_ :\n" << bias_jac.dDp_dab_ << "\n" << std::endl;
- std::cout << "dDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << "\n" << std::endl;
- }
-
- if(dDv_dab.isApprox(bias_jac.dDv_dab_, wolf::Constants::EPS) )
- std::cout<< "dDv_dab_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dab_ jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dab_ : \n" << dDv_dab << "\n bias_jac.dDv_dab_ :\n" << bias_jac.dDv_dab_ << "\n" << std::endl;
- std::cout << "dDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << "\n" << std::endl;
- }
-
- if(dDp_dwb.isApprox(bias_jac.dDp_dwb_, wolf::Constants::EPS) )
- std::cout<< "dDp_dwb_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dwb_ jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dwb_ : \n" << dDp_dwb << "\n bias_jac.dDp_dwb_ :\n" << bias_jac.dDp_dwb_ << "\n" << std::endl;
- std::cout << "dDp_dwb_a - dDp_dwb_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << "\n" << std::endl;
- }
-
- if(dDv_dwb.isApprox(bias_jac.dDv_dwb_, wolf::Constants::EPS) )
- std::cout<< "dDv_dwb_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dwb_ jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dwb_ : \n" << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n" << bias_jac.dDv_dwb_ << "\n" << std::endl;
- std::cout << "dDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << "\n" << std::endl;
- }
-
- if(dDq_dwb.isApprox(bias_jac.dDq_dwb_, wolf::Constants::EPS) )
- std::cout<< "dDq_dwb_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDq_dwb_ jacobian is not correct ..." << std::endl;
- std::cout << "dDq_dwb_ : \n" << dDq_dwb << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_ << "\n" << std::endl;
- std::cout << "dDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb << "\n" << std::endl;
- }
-
- //Check jacobians that are supposed to be Zeros just in case
- if(dDq_dab.isZero(wolf::Constants::EPS))
- std::cout<< "dDq_dab_ jacobian is correct (Zero) !" << std::endl;
- else
- std::cout<< "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
-
- /* 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
- */
-
- //taking care of noise now
- Eigen::Matrix<wolf::Scalar,9,1> Delta_noise;
- Eigen::Matrix<wolf::Scalar,9,1> delta_noise;
-
- 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 = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
-
- /* 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
- */
-
- Eigen::Matrix3s dDp_dP, dDp_dV, dDp_dO, dDv_dP, dDv_dV, dDv_dO, dDo_dP, dDo_dV, dDo_dO;
- Eigen::Matrix3s dDp_dp, dDp_dv, dDp_do, dDv_dp, dDv_dv, dDv_do, dDo_dp, dDo_dv, dDo_do;
-
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-
- for(int i = 0; i < 3; i++){
-
- //dDp_dPx = ((P + dPx) - P)/dPx
- dDp_dP.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3))/Delta_noise(i);
- //Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
- 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);
- //dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
- 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);
-
- //dDv_dP = [0, 0, 0]
- //dDv_dVx = ((V + dVx) - V)/dVx
- 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);
- //dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
- 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);
-
- //dDo_dP = dDo_dV = [0, 0, 0]
- //dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
- dDo_dO.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix()) )/Delta_noise(i+3);
-
- //dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
- 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);
- //dDp_dv = dDp_do = [0, 0, 0]
-
- //dDv_dp = [0, 0, 0]
- //dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
- 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);
- //dDv_do = [0, 0, 0]
-
- //dDo_dp = dDo_dv = [0, 0, 0]
- //dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
- dDo_do.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq0.matrix() * dq_noisy.matrix()) )/Delta_noise(i+3);
- }
-
- /* jacobians wrt deltas have PVQ form :
- dDp_dP = deltas_jac.jacobian_delta_preint_.block(0,0,3,3); dDp_dV = deltas_jac.jacobian_delta_preint_.block(0,3,3,3); dDp_dO = deltas_jac.jacobian_delta_preint_.block(0,6,3,3);
- dDv_dP = deltas_jac.jacobian_delta_preint_.block(3,0,3,3); dDv_dV = deltas_jac.jacobian_delta_preint_.block(3,3,3,3); dDv_dO = deltas_jac.jacobian_delta_preint_.block(3,6,3,3);
- dDo_dP = deltas_jac.jacobian_delta_preint_.block(6,0,3,3); dDo_dV = deltas_jac.jacobian_delta_preint_.block(6,3,3,3); dDo_dO = deltas_jac.jacobian_delta_preint_.block(6,6,3,3);
- */
-
- if(dDp_dP.isApprox(deltas_jac.jacobian_delta_preint_.block(0,0,3,3), wolf::Constants::EPS) )
- std::cout<< "dDp_dP jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dP jacobian is not correct ..." << std::endl;
- std::cout << "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) << "\n" << std::endl;
- std::cout << "dDp_dP_a - dDp_dP : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << "\n" << std::endl;
- }
-
- if(dDp_dV.isApprox(deltas_jac.jacobian_delta_preint_.block(0,6,3,3), wolf::Constants::EPS) )
- std::cout<< "dDp_dV jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dV jacobian is not correct ..." << std::endl;
- std::cout << "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) << "\n" << std::endl;
- std::cout << "dDp_dV_a - dDp_dV : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << "\n" << std::endl;
- }
-
- if(dDp_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(0,3,3,3), wolf::Constants::EPS) )
- std::cout<< "dDp_dO jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dO jacobian is not correct ..." << std::endl;
- std::cout << "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) << "\n" << std::endl;
- std::cout << "dDp_dO_a - dDp_dO : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << "\n" << std::endl;
- }
-
- if(dDv_dV.isApprox(deltas_jac.jacobian_delta_preint_.block(6,6,3,3), wolf::Constants::EPS) )
- std::cout<< "dDv_dV jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dV jacobian is not correct ..." << std::endl;
- std::cout << "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) << "\n" << std::endl;
- std::cout << "dDv_dV_a - dDv_dV : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << "\n" << std::endl;
- }
-
- if(dDv_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(6,3,3,3), wolf::Constants::EPS) )
- std::cout<< "dDv_dO jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dO jacobian is not correct ..." << std::endl;
- std::cout << "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) << "\n" << std::endl;
- std::cout << "dDv_dO_a - dDv_dO : \n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) - dDv_dO << "\n" << std::endl;
- }
-
- if(dDo_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(3,3,3,3), wolf::Constants::EPS) )
- std::cout<< "dDo_dO jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDo_dO jacobian is not correct ..." << std::endl;
- std::cout << "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) << "\n" << std::endl;
- std::cout << "dDo_dO_a - dDo_dO : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << "\n" << std::endl;
- }
-
- Eigen::Matrix3s dDp_dp_a, dDv_dv_a, dDo_do_a;
- dDp_dp_a = deltas_jac.jacobian_delta_.block(0,0,3,3);
- dDv_dv_a = deltas_jac.jacobian_delta_.block(6,6,3,3);
- dDo_do_a = deltas_jac.jacobian_delta_.block(3,3,3,3);
-
- if(dDp_dp.isApprox(dDp_dp_a, wolf::Constants::EPS) )
- std::cout<< "dDp_dp jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dp jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dp : \n" << dDv_dp << "\n dDp_dp_a :\n" << dDp_dp_a << "\n" << std::endl;
- std::cout << "dDp_dp_a - dDp_dp : \n" << dDp_dp_a - dDv_dp << "\n" << std::endl;
- }
-
- if(dDv_dv.isApprox(dDv_dv_a, wolf::Constants::EPS) )
- std::cout<< "dDv_dv jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dv jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dv : \n" << dDv_dv << "\n dDv_dv_a :\n" << dDv_dv_a << "\n" << std::endl;
- std::cout << "dDv_dv_a - dDv_dv : \n" << dDv_dv_a - dDv_dv << "\n" << std::endl;
- }
-
- if(dDo_do.isApprox(dDo_do_a, wolf::Constants::EPS) )
- std::cout<< "dDo_do jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDo_do jacobian is not correct ..." << std::endl;
- std::cout << "dDo_do : \n" << dDo_do << "\n dDo_do_a :\n" << dDo_do_a << "\n" << std::endl;
- std::cout << "dDo_do_a - dDo_do : \n" << dDo_do_a - dDo_do << "\n" << std::endl;
- }
-
- return 0;
-}
-
-using namespace wolf;
-
-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);
-}
diff --git a/demos/demo_processor_odom_3D.cpp b/demos/demo_processor_odom_3D.cpp
deleted file mode 100644
index 7cfae793d291d83364261c8996f78d0b64fa3d13..0000000000000000000000000000000000000000
--- a/demos/demo_processor_odom_3D.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/**
- * \file test_processor_odom_3D.cpp
- *
- * Created on: Oct 7, 2016
- * \author: jsola
- */
-
-#include "core/capture/capture_IMU.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_base.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-#include <cstdlib>
-
-using namespace wolf;
-using std::cout;
-using std::endl;
-using Eigen::Vector3s;
-using Eigen::Vector6s;
-using Eigen::Vector7s;
-using Eigen::Quaternions;
-using Eigen::VectorXs;
-
-int main (int argc, char** argv)
-{
- cout << "\n========= Test ProcessorOdom3D ===========" << endl;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- TimeStamp tf;
- if (argc == 1)
- tf = 1.0;
- else
- {
-
- tf.set(strtod(argv[1],nullptr));
- }
- cout << "Final timestamp tf = " << tf.get() << " s" << endl;
-
- ProblemPtr problem = Problem::create("PO", 3);
- ceres::Solver::Options ceres_options;
-// ceres_options.max_num_iterations = 1000;
-// ceres_options.function_tolerance = 1e-10;
-// ceres_options.gradient_check_relative_precision = 1e-10;
-// ceres_options.gradient_tolerance = 1e-10;
- ceres_options.minimizer_progress_to_stdout = true;
- CeresManager ceres_manager(problem, ceres_options);
-
- SensorBasePtr sen = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
- ProcessorParamsOdom3DPtr proc_params = std::make_shared<ProcessorParamsOdom3D>();
- problem->installProcessor("ODOM 3D", "odometry integrator", sen, proc_params);
-
- // Time and prior
- Scalar dt = 0.1;
-
- problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity() * 1e-8, TimeStamp(0), dt/2);
-
- // Motion data
- Scalar dx = .1;
- Scalar dyaw = 2*M_PI/5;
- Vector6s data((Vector6s() << dx*cos(dyaw/2),dx*sin(dyaw/2),0,0,0,dyaw).finished()); // will integrate this data repeatedly
-
- CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>("ODOM 3D", TimeStamp(0), sen, data, 7, 6, nullptr);
-
- cout << "t: " << std::setprecision(2) << 0 << " \t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
-
- for (TimeStamp t = dt; t < tf+dt/2; t += dt)
- {
- cap_odo->setTimeStamp(t);
- cap_odo->setData(data);
-
- sen->process(cap_odo);
-
- cout << "t: " << std::setprecision(2) << t.get() << " \t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
-
-// ceres::Solver::Summary summary = ceres_manager.solve();
-
-// ceres_manager.computeCovariances(ALL);
-
-// cout << summary.BriefReport() << endl;
-
- }
-
- problem->print(1,0,1,0);
-// for (auto frm : problem->getTrajectory()->getFrameList())
-// {
-// frm->setState(problem->zeroState());
-// }
-// problem->print(1,0,1,0);
- std::string brief_report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);
- std::cout << brief_report << std::endl;
- problem->print(1,0,1,0);
-
- problem.reset();
-
- return 0;
-}
diff --git a/demos/demo_processor_tracker_feature.cpp b/demos/demo_processor_tracker_feature.cpp
deleted file mode 100644
index 74340d9e50d8fb1236f0db02c053f39f81807792..0000000000000000000000000000000000000000
--- a/demos/demo_processor_tracker_feature.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/**
- * \file test_processor_tracker_feature.cpp
- *
- * Created on: Apr 11, 2016
- * \author: jvallve
- */
-
-//std
-#include <iostream>
-
-//Wolf
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
-#include "core/capture/capture_void.h"
-
-int main()
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::cout << std::endl << "==================== processor tracker feature test ======================" << std::endl;
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
- SensorBasePtr sensor_ptr_ = make_shared<SensorBase>("ODOM 2D", 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_trk = std::make_shared<ProcessorParamsTrackerFeature>();
- params_trk->max_new_features = 4;
- params_trk->min_features_for_keyframe = 7;
- params_trk->time_tolerance = 0.25;
- shared_ptr<ProcessorTrackerFeatureDummy> processor_ptr_ = make_shared<ProcessorTrackerFeatureDummy>(params_trk);
-
- wolf_problem_ptr_->addSensor(sensor_ptr_);
- sensor_ptr_->addProcessor(processor_ptr_);
-
- std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
- TimeStamp t(0);
- Scalar dt = 0.5;
- for (auto i = 0; i < 10; i++)
- {
- processor_ptr_->process(make_shared<CaptureVoid>(t, sensor_ptr_));
- t += dt;
- }
-
- wolf_problem_ptr_->print(2);
-
- return 0;
-}
-
diff --git a/demos/demo_processor_tracker_landmark.cpp b/demos/demo_processor_tracker_landmark.cpp
deleted file mode 100644
index 5500fdcbb126b413492620e7ea2828738a89300a..0000000000000000000000000000000000000000
--- a/demos/demo_processor_tracker_landmark.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/**
- * \file test_processor_tracker_landmark.cpp
- *
- * Created on: Apr 12, 2016
- * \author: jvallve
- */
-
-//std
-#include <iostream>
-
-//Wolf
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/processor/processor_tracker_landmark_dummy.h"
-#include "core/capture/capture_void.h"
-
-void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
-{
- using namespace wolf;
- std::cout << "\n-----------\nWolf tree begin" << std::endl;
- std::cout << "Hrd: " << wolf_problem_ptr_->getHardware()->getProblem() << std::endl;
- for (SensorBasePtr sen : wolf_problem_ptr_->getHardware()->getSensorList())
- {
- std::cout << "\tSen: " << sen->getProblem() << std::endl;
- for (ProcessorBasePtr prc : sen->getProcessorList())
- {
- std::cout << "\t\tPrc: " << prc->getProblem() << std::endl;
- }
- }
- std::cout << "Trj: " << wolf_problem_ptr_->getTrajectory()->getProblem() << std::endl;
- for (FrameBasePtr frm : wolf_problem_ptr_->getTrajectory()->getFrameList())
- {
- std::cout << "\tFrm: " << frm->getProblem() << std::endl;
- for (CaptureBasePtr cap : frm->getCaptureList())
- {
- std::cout << "\t\tCap: " << cap->getProblem() << std::endl;
- for (FeatureBasePtr ftr : cap->getFeatureList())
- {
- std::cout << "\t\t\tFtr: " << ftr->getProblem() << std::endl;
- for (FactorBasePtr ctr : ftr->getFactorList())
- {
- std::cout << "\t\t\t\tCtr: " << ctr->getProblem() << std::endl;
- }
- }
- }
- }
- std::cout << "Map: " << wolf_problem_ptr_->getMap()->getProblem() << std::endl;
- for (LandmarkBasePtr lmk : wolf_problem_ptr_->getMap()->getLandmarkList())
- {
- std::cout << "\tLmk: " << lmk->getProblem() << std::endl;
- }
- std::cout << "Wolf tree end\n-----------\n" << std::endl;
-}
-
-int main()
-{
- using namespace wolf;
-
- std::cout << std::endl << "==================== processor tracker landmark test ======================" << std::endl;
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
- // SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", 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);
- auto sensor_ptr_ = SensorBase::emplace<SensorBase>(wolf_problem_ptr_->getHardware(), "ODOM 2D", 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);
- ProcessorParamsTrackerLandmarkPtr params_trk = std::make_shared<ProcessorParamsTrackerLandmark>();
- params_trk->max_new_features = 5;
- params_trk->min_features_for_keyframe = 7;
- params_trk->time_tolerance = 0.25;
- // std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk);
- std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ =
- std::static_pointer_cast<ProcessorTrackerLandmarkDummy>(ProcessorBase::emplace<ProcessorTrackerLandmarkDummy>(sensor_ptr_, params_trk));
- // wolf_problem_ptr_->addSensor(sensor_ptr_);
- // sensor_ptr_->addProcessor(processor_ptr_);
-
- std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
- TimeStamp t(0);
- Scalar dt = 0.5;
- for (auto i = 0; i < 10; i++)
- {
- processor_ptr_->process(std::make_shared<CaptureVoid>(t, sensor_ptr_));
- t += dt;
- }
-
- wolf_problem_ptr_->print(2);
-
- return 0;
-}
-
diff --git a/demos/demo_processor_tracker_landmark_image.cpp b/demos/demo_processor_tracker_landmark_image.cpp
deleted file mode 100644
index e8767c4f69d1ece65c6886cf87a2a405fb0d7ca7..0000000000000000000000000000000000000000
--- a/demos/demo_processor_tracker_landmark_image.cpp
+++ /dev/null
@@ -1,254 +0,0 @@
-//std
-#include <iostream>
-
-#include "core/processor/processor_tracker_landmark_image.h"
-
-//Wolf
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/sensor/sensor_camera.h"
-#include "core/capture/capture_image.h"
-#include "core/capture/capture_pose.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// Vision utils includes
-#include <vision_utils.h>
-#include <sensors.h>
-#include <common_class/buffer.h>
-#include <common_class/frame.h>
-
-using Eigen::Vector3s;
-using Eigen::Vector4s;
-using Eigen::Vector6s;
-using Eigen::Vector7s;
-
-using namespace wolf;
-
-void cleanupMap(const ProblemPtr& _problem, const TimeStamp& _t, Scalar _dt_max,
- SizeEigen _min_factors)
-{
- std::list<LandmarkBasePtr> lmks_to_remove;
- for (auto lmk : _problem->getMap()->getLandmarkList())
- {
- TimeStamp t0 = std::static_pointer_cast<LandmarkAHP>(lmk)->getAnchorFrame()->getTimeStamp();
- if (_t - t0 > _dt_max)
- if (lmk->getConstrainedByList().size() <= _min_factors)
- lmks_to_remove.push_back(lmk);
- }
-
- for (auto lmk : lmks_to_remove)
- {
- WOLF_DEBUG("Clean up L" , lmk->id() );
- lmk->remove();
- }
-}
-
-Eigen::MatrixXs computeDataCovariance(const VectorXs& _data)
-{
- Scalar k = 0.5;
- Scalar dist = _data.head<3>().norm();
- if ( dist == 0 ) dist = 1.0;
- WOLF_DEBUG("dist: ", dist, "; sigma: ", sqrt(k* (dist + 0.1)) );
- return k * (dist + 0.1) * Matrix6s::Identity();
-}
-
-int main(int argc, char** argv)
-{
- std::cout << std::endl << "==================== processor image landmark test ======================" << std::endl;
-
- // Sensor or sensor recording
- vision_utils::SensorCameraPtr sen_ptr = vision_utils::askUserSource(argc, argv);
- if (sen_ptr==NULL)
- return 0;
-
- unsigned int buffer_size = 10;
- vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
-
- unsigned int img_width = frame_buff.back()->getImage().cols;
- unsigned int img_height = frame_buff.back()->getImage().rows;
- std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
-
- //=====================================================
- // Environment variable for configuration files
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::cout << wolf_root << std::endl;
- //=====================================================
-
- //=====================================================
- // Wolf problem
- ProblemPtr problem = Problem::create("PO", 3);
-
- // ODOM SENSOR AND PROCESSOR
- SensorBasePtr sensor_base = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
- SensorOdom3DPtr sensor_odom = std::static_pointer_cast<SensorOdom3D>(sensor_base);
- ProcessorBasePtr prcocessor_base = problem->installProcessor("ODOM 3D", "odometry integrator", "odom", wolf_root + "/src/examples/processor_odom_3D.yaml");
-
- // CAMERA SENSOR AND PROCESSOR
- sensor_base = problem->installSensor("CAMERA", "PinHole", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sensor_base);
- camera->setImgWidth(img_width);
- camera->setImgHeight(img_height);
- ProcessorTrackerLandmarkImagePtr prc_img_ptr = std::static_pointer_cast<ProcessorTrackerLandmarkImage>( problem->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml") );
- //=====================================================
-
- //=====================================================
- // Origin Key Frame is fixed
- TimeStamp t = 0;
- FrameBasePtr origin_frame = problem->emplaceFrame(KEY, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
- problem->getProcessorMotion()->setOrigin(origin_frame);
- origin_frame->fix();
-
- std::cout << "t: " << 0 << " \t\t\t x = ( " << problem->getCurrentState().transpose() << ")" << std::endl;
- std::cout << "--------------------------------------------------------------" << std::endl;
- //=====================================================
-
- //=====================================================
- // running CAPTURES preallocated
- CaptureImagePtr image;
- Vector6s data(Vector6s::Zero()); // will integrate this data repeatedly
- CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>("IMAGE", t, sensor_odom, data, 7, 6, nullptr);
- //=====================================================
-
- //=====================================================
- // Ceres wrapper
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- ceres_options.max_num_iterations = 10;
- google::InitGoogleLogging(argv[0]);
-
- CeresManager ceres_manager(problem, ceres_options);
- //=====================================================
-
- //=====================================================
- // graphics
- cv::namedWindow("Feature tracker"); // Creates a window for display.
- cv::moveWindow("Feature tracker", 0, 0);
- cv::startWindowThread();
- //=====================================================
-
- //=====================================================
- // main loop
- unsigned int number_of_KFs = 0;
- Scalar dt = 0.04;
-
- for(int frame_count = 0; frame_count<10000; ++frame_count)
- {
- t += dt;
-
- // Image ---------------------------------------------
-
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
-
- // Preferred method with factory objects:
- image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
-
- WOLF_DEBUG(__LINE__);
-
- /* process */
- camera->process(image);
-
- WOLF_DEBUG(__LINE__);
-
- // Odometry --------------------------------------------
-
- cap_odo->setTimeStamp(t);
-
- // previous state
- FrameBasePtr prev_key_fr_ptr = problem->getLastKeyFrame();
-// Eigen::Vector7s x_prev = problem->getCurrentState();
- Eigen::Vector7s x_prev = prev_key_fr_ptr->getState();
-
- // before the previous state
- FrameBasePtr prev_prev_key_fr_ptr = nullptr;
- Vector7s x_prev_prev;
- Vector7s dx;
- for (auto f_it = problem->getTrajectory()->getFrameList().rbegin(); f_it != problem->getTrajectory()->getFrameList().rend(); f_it++)
- if ((*f_it) == prev_key_fr_ptr)
- {
- f_it++;
- if (f_it != problem->getTrajectory()->getFrameList().rend())
- {
- prev_prev_key_fr_ptr = (*f_it);
- }
- break;
- }
-
- // compute delta state, and odometry data
- if (!prev_prev_key_fr_ptr)
- {
- // we have just one state --> odometry data is zero
- data.setZero();
- }
- else
- {
- x_prev_prev = prev_prev_key_fr_ptr->getState();
-
- // define local variables on top of existing vectors to avoid memory allocation
- Eigen::Vector3s p_prev_prev(x_prev_prev.data());
- Eigen::Quaternions q_prev_prev(x_prev_prev.data() + 3);
- Eigen::Vector3s p_prev(x_prev.data());
- Eigen::Quaternions q_prev(x_prev.data() + 3);
- Eigen::Vector3s dp(dx.data());
- Eigen::Quaternions dq(dx.data() + 3);
-
- // delta state PQ
- dp = q_prev_prev.conjugate() * (p_prev - p_prev_prev);
- dq = q_prev_prev.conjugate() * q_prev;
-
- // odometry data
- data.head<3>() = dp;
- data.tail<3>() = q2v(dq);
- }
-
- Matrix6s data_cov = computeDataCovariance(data);
-
- cap_odo->setData(data);
- cap_odo->setDataCovariance(data_cov);
-
- sensor_odom->process(cap_odo);
-
-// problem->print(2,1,0,0);
-
-// std::cout << "prev prev ts: " << t_prev_prev.get() << "; x: " << x_prev_prev.transpose() << std::endl;
-// std::cout << "prev ts: " << t_prev.get() << "; x: " << x_prev.transpose() << std::endl;
-// std::cout << "current ts: " << t.get() << std::endl;
-// std::cout << " dt: " << t_prev - t_prev_prev << "; dx: " << dx.transpose() << std::endl;
-
- // Cleanup map ---------------------------------------
-
- cleanupMap(problem, t, 2, 5); // dt, min_ctr
-
- // Solve -----------------------------------------------
- // solve only when new KFs are added
- if (problem->getTrajectory()->getFrameList().size() > number_of_KFs)
- {
- number_of_KFs = problem->getTrajectory()->getFrameList().size();
- std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
- std::cout << summary << std::endl;
- }
-
- // Finish loop -----------------------------------------
- cv::Mat image_graphics = frame_buff.back()->getImage().clone();
- prc_img_ptr->drawTrackerRoi(image_graphics, cv::Scalar(255.0, 0.0, 255.0)); //tracker roi
- prc_img_ptr->drawRoi(image_graphics, prc_img_ptr->detector_roi_, cv::Scalar(0.0,255.0, 255.0)); //active search roi
- prc_img_ptr->drawLandmarks(image_graphics);
- prc_img_ptr->drawFeaturesFromLandmarks(image_graphics);
- cv::imshow("Feature tracker", image_graphics);
- cv::waitKey(1);
-
- std::cout << "=================================================================================================" << std::endl;
- }
-
- // problem->print(2);
- problem.reset();
-
- return 0;
-}
-
diff --git a/demos/demo_projection_points.cpp b/demos/demo_projection_points.cpp
deleted file mode 100644
index 6d6f943ab4b3ee11ea03965f8a872fc3454a9f7b..0000000000000000000000000000000000000000
--- a/demos/demo_projection_points.cpp
+++ /dev/null
@@ -1,468 +0,0 @@
-
-// Vision utils
-#include <vision_utils/vision_utils.h>
-
-//std includes
-#include <iostream>
-
-//wolf includes
-#include "core/math/pinhole_tools.h"
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << std::endl << " ========= ProjectPoints test ===========" << std::endl << std::endl;
-
- Eigen::MatrixXs points3D(2,3);
- Eigen::Vector3s point3D;
- point3D[0] = 2.0;
- point3D[1] = 5.0;
- point3D[2] = 6.0;
- points3D.row(0)= point3D;
- point3D[0] = 4.0;
- point3D[1] = 2.0;
- point3D[2] = 1.0;
- points3D.row(1)= point3D;
-
- Eigen::Vector3s cam_ext_rot_mat = Eigen::Vector3s::Zero();
- Eigen::Vector3s cam_ext_trans_mat = Eigen::Vector3s::Ones();
-
- Eigen::Matrix3s cam_intr_mat;
- cam_intr_mat = Eigen::Matrix3s::Identity();
- cam_intr_mat(0,2)=2;
- cam_intr_mat(1,2)=2;
-
- Eigen::VectorXs dist_coef(5);
- dist_coef << 0,0,0,0,0;
-
- Eigen::MatrixXs points2D = vision_utils::projectPoints(points3D, cam_ext_rot_mat, cam_ext_trans_mat, cam_intr_mat, dist_coef);
-
- for (int ii = 0; ii < points3D.rows(); ++ii)
- std::cout << "points2D- X: " << points2D(ii,0) << "; Y: " << points2D(ii,1) << std::endl;
-
- std::cout << std::endl << " ========= PinholeTools DUALITY TEST ===========" << std::endl << std::endl;
-
- //================================= projectPoint and backprojectPoint to/from NormalizedPlane
-
- Eigen::Vector3s project_point_normalized_test;
- project_point_normalized_test[0] = 1.06065;
- project_point_normalized_test[1] = 1.06065;
- project_point_normalized_test[2] = 3;
- Eigen::Vector2s project_point_normalized_output;
- Eigen::Vector2s project_point_normalized_output2;
- Scalar project_point_normalized_dist;
-
- Scalar backproject_point_normalized_depth = 3;
- Eigen::Vector3s backproject_point_normalized_output;
-
- project_point_normalized_output = pinhole::projectPointToNormalizedPlane(project_point_normalized_test);
-
- pinhole::projectPointToNormalizedPlane(project_point_normalized_test,
- project_point_normalized_output2,
- project_point_normalized_dist);
-
- backproject_point_normalized_output =
- pinhole::backprojectPointFromNormalizedPlane(project_point_normalized_output,
- backproject_point_normalized_depth);
-
- std::cout << "TEST project and backproject PointToNormalizedPlane" << std::endl;
- std::cout << std:: endl << "Original " << project_point_normalized_test.transpose() << std::endl;
- std::cout << std:: endl << "Project " << project_point_normalized_output.transpose() << std::endl;
- std::cout << std:: endl << "Alternate project" << project_point_normalized_output.transpose() << std::endl;
- std::cout << std:: endl << "Backproject " << backproject_point_normalized_output.transpose() << std::endl;
-
- //================================= projectPoint and backprojectPoint to/from NormalizedPlane WITH JACOBIANS
-
- Eigen::Vector3s pp_normalized_test;
- pp_normalized_test[0] = 3;
- pp_normalized_test[1] = 3;
- pp_normalized_test[2] = 3;
- Eigen::Vector2s pp_normalized_output;
- Eigen::Vector2s pp_normalized_output2;
- Eigen::Matrix<Scalar, 2, 3> pp_normalized_jacobian;
- Eigen::Matrix<Scalar, 2, 3> pp_normalized_jacobian2;
- Scalar pp_normalized_distance;
-
- Scalar bpp_normalized_depth = 3;
- Eigen::Vector3s bpp_normalized_output;
- Eigen::Matrix<Scalar, 3, 2> bpp_normalized_jacobian;
- Eigen::Vector3s bpp_normalized_jacobian_depth;
-
- pinhole::projectPointToNormalizedPlane(pp_normalized_test,
- pp_normalized_output,
- pp_normalized_jacobian);
- pinhole::projectPointToNormalizedPlane(pp_normalized_test,
- pp_normalized_output2,
- pp_normalized_distance,
- pp_normalized_jacobian2);
-
- pinhole::backprojectPointFromNormalizedPlane(pp_normalized_output,
- bpp_normalized_depth,
- bpp_normalized_output,
- bpp_normalized_jacobian,
- bpp_normalized_jacobian_depth);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST project and backproject PointToNormalizedPlane with JACOBIAN" << std::endl;
-
- std::cout << std:: endl << "Original" << pp_normalized_test.transpose() << std::endl;
- std::cout << std:: endl << "Project" << pp_normalized_output.transpose() << std::endl;
- std::cout << "\n--> Jacobian" << std::endl << pp_normalized_jacobian << std::endl;
-
- std::cout << std:: endl << "Alternate project" << pp_normalized_output2.transpose() << "; distance: "
- << pp_normalized_distance << std::endl;
- std::cout << "\n--> Jacobian" << std::endl << pp_normalized_jacobian2 << std::endl;
-
- std::cout << std:: endl << "Backproject" << bpp_normalized_output.transpose()
- << "; depth: " << bpp_normalized_depth << std::endl;
- std::cout << "\n--> Jacobian" << std::endl << bpp_normalized_jacobian << std::endl;
- std::cout << "\n--> Jacobian - depth" << bpp_normalized_jacobian_depth.transpose() << std::endl;
-
- Eigen::Matrix2s test_jacobian; // (2x3) * (3x2) = (2x2)
- test_jacobian = pp_normalized_jacobian * bpp_normalized_jacobian;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl << test_jacobian << std::endl;
-
- //================================= IsInRoi / IsInImage
-
- Eigen::Vector2s pix;
- pix[0] = 40; // x
- pix[1] = 40; // y
-
- int roi_x = 30;
- int roi_y = 30;
- int roi_width = 20;
- int roi_height = 20;
-
- int image_width = 640;
- int image_height = 480;
-
- bool is_in_roi;
- bool is_in_image;
- is_in_roi = pinhole::isInRoi(pix,
- roi_x,
- roi_y,
- roi_width,
- roi_height);
- is_in_image = pinhole::isInImage(pix,
- image_width,
- image_height);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST isInRoi/isInImage" << std::endl;
- std::cout << std::endl << "Pixel " << std::endl;
- std::cout << "x: " << pix[0] << "; y: " << pix[1] << std::endl;
- std::cout << std::endl << "ROI " << std::endl;
- std::cout << "x: " << roi_x << "; y: " << roi_y << "; width: " << roi_width << "; height: " << roi_height << std::endl;
- std::cout << "is_in_roi: " << is_in_roi << std::endl;
- std::cout << std::endl << "Image " << std::endl;
- std::cout << "width: " << image_width << "; height: " << image_height << std::endl;
- std::cout << "is_in_image: " << is_in_image << std::endl;
-
- //================================= computeCorrectionModel
-
- Eigen::Vector2s distortion2;
- distortion2[0] = -0.301701;
- distortion2[1] = 0.0963189;
- Eigen::Vector4s k_test2;
- //k = [u0, v0, au, av]
- k_test2[0] = 516.686; //u0
- k_test2[1] = 355.129; //v0
- k_test2[2] = 991.852; //au
- k_test2[3] = 995.269; //av
-
- Eigen::Vector2s correction_test2;
- pinhole::computeCorrectionModel(k_test2,
- distortion2,
- correction_test2);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST computeCorrectionModel" << std::endl;
- std::cout << std::endl << "distortion" << std::endl;
- std::cout << "d1: " << distortion2[0] << "; d2: " << distortion2[1] << std::endl;
- std::cout << std::endl << "k values" << std::endl;
- std::cout << "u0: " << k_test2[0] << "; v0: " << k_test2[1] << "; au: " << k_test2[2] << "; av: " << k_test2[3] << std::endl;
- std::cout << std::endl << "correction" << std::endl;
- std::cout << "c1: " << correction_test2[0] << "; c2: " << correction_test2[1] << std::endl;
-
- //================================= distortPoint
-
- Eigen::Vector2s distorting_point;
- distorting_point[0] = 0.35355;
- distorting_point[1] = 0.35355;
-
- Eigen::Vector2s distored_point3;
- distored_point3 = pinhole::distortPoint(distortion2,
- distorting_point);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST distortPoint" << std::endl;
- std::cout << std::endl << "Point to be distorted" << std::endl;
- std::cout << "x: " << distorting_point[0] << "; y: " << distorting_point[1] << std::endl;
- std::cout << std::endl << "Distorted point" << std::endl;
- std::cout << "x: " << distored_point3[0] << "; y: " << distored_point3[1] << std::endl;
-
- Eigen::Vector2s corrected_point4;
- corrected_point4 = pinhole::undistortPoint(correction_test2,
- distored_point3);
- std::cout << std::endl << "Corrected point" << std::endl;
- std::cout << "x: " << corrected_point4[0] << "; y: " << corrected_point4[1] << std::endl;
-
- ////
-
- Eigen::Vector2s distored_point4;
- Eigen::Matrix2s distortion_jacobian2;
- pinhole::distortPoint(distortion2,
- distorting_point,
- distored_point4,
- distortion_jacobian2);
-
- std::cout << "\n\nTEST distortPoint, jacobian" << std::endl;
- std::cout << std::endl << "Point to be distorted" << std::endl;
- std::cout << "x: " << distorting_point[0] << "; y: " << distorting_point[1] << std::endl;
- std::cout << std::endl << "Distorted point" << std::endl;
- std::cout << "x: " << distored_point4[0] << "; y: " << distored_point4[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- distortion_jacobian2 << std::endl;
-
- Eigen::Vector2s corrected_point5;
- Eigen::Matrix2s corrected_jacobian2;
- pinhole::undistortPoint(correction_test2,
- distored_point4,
- corrected_point5,
- corrected_jacobian2);
-
- std::cout << std::endl << "Corrected point" << std::endl;
- std::cout << "x: " << corrected_point5[0] << "; y: " << corrected_point5[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- corrected_jacobian2 << std::endl;
-
- Eigen::Matrix2s test_jacobian_distortion;
- test_jacobian_distortion = distortion_jacobian2 * corrected_jacobian2;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
- test_jacobian_distortion << std::endl;
-
- //================================= PixelizePoint
-
- Eigen::Vector2s pixelize_ud;
- pixelize_ud[0] = 45;
- pixelize_ud[1] = 28;
-
- Eigen::Vector2s pixelize_output3;
- pixelize_output3 = pinhole::pixellizePoint(k_test2,
- pixelize_ud);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST pixelizePoint; Eigen::Vector2s" << std::endl;
- std::cout << std::endl << "Original" << std::endl;
- std::cout << "x: " << pixelize_ud[0] << "; y: " << pixelize_ud[1] << std::endl;
- std::cout << std::endl << "Pixelized" << std::endl;
- std::cout << "x: " << pixelize_output3[0] << "; y: " << pixelize_output3[1] << std::endl;
-
- Eigen::Vector2s depixelize_output3;
- depixelize_output3 = pinhole::depixellizePoint(k_test2,
- pixelize_output3);
- std::cout << std::endl << "Depixelized" << std::endl;
- std::cout << "x: " << depixelize_output3[0] << "; y: " << depixelize_output3[1] << std::endl;
-
- ////
-
- Eigen::Vector2s pixelize_output4;
- Eigen::Matrix2s pixelize_jacobian2;
- pinhole::pixellizePoint(k_test2,
- pixelize_ud,
- pixelize_output4,
- pixelize_jacobian2);
-
- std::cout << std::endl << "TEST pixelizePoint; Jacobians" << std::endl;
- std::cout << std::endl << "Original" << std::endl;
- std::cout << "x: " << pixelize_ud[0] << "; y: " << pixelize_ud[1] << std::endl;
- std::cout << std::endl << "Pixelized" << std::endl;
- std::cout << "x: " << pixelize_output4[0] << "; y: " << pixelize_output4[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- pixelize_jacobian2 << std::endl;
-
- Eigen::Vector2s depixelize_output4;
- Eigen::Matrix2s depixelize_jacobian2;
- pinhole::depixellizePoint(k_test2,
- pixelize_output4,
- depixelize_output4,
- depixelize_jacobian2);
-
- std::cout << std::endl << "Depixelized" << std::endl;
- std::cout << "x: " << depixelize_output4[0] << "; y: " << depixelize_output4[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- depixelize_jacobian2 << std::endl;
-
- Eigen::Matrix2s test_jacobian_pix;
- test_jacobian_pix = pixelize_jacobian2 * depixelize_jacobian2;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
- test_jacobian_pix << std::endl;
-
- //================================= projectPoint Complete
-
-// //distortion
-// distortion2;
-
-// //k
-// k_test2;
-
-// //3Dpoint
-// project_point_normalized_test;
-
- Eigen::Vector2s point2D_test5;
- point2D_test5 = pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << std::endl << "TEST projectPoint Complete" << std::endl;
- std::cout << "\nPARAMS" << std::endl;
- std::cout << "\nDistortion:" << std::endl;
- std::cout << "d1: " << distortion2[0] << "; d2: " << distortion2[1] << std::endl << std::endl;
- std::cout << "k values:" << std::endl;
- std::cout << "u0: " << k_test2[0] << "; v0: " << k_test2[1] << "; au: " << k_test2[2] << "; av: " << k_test2[3] << std::endl << std::endl;
- std::cout << "3D Point" << std::endl;
- std::cout << "x: " << project_point_normalized_test[0] << "; y: " << project_point_normalized_test[1]
- << "; z: " << project_point_normalized_test[2] << std::endl;
- std::cout << "\n\n\nFirst function output" << std::endl;
- std::cout << "x: " << point2D_test5[0] << "; y: " << point2D_test5[1] << std::endl;
-
- //distance
- Eigen::Vector2s point2D_test6;
- Scalar distance_test4;
- pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test,
- point2D_test6,
- distance_test4);
-
- std::cout << std::endl << "Second function output" << std::endl;
- std::cout << "x: " << point2D_test6[0] << "; y: " << point2D_test6[1] << "; dist: " << distance_test4 << std::endl;
-
- //jacobian
- Eigen::Vector2s point2D_test7;
- Eigen::MatrixXs jacobian_test3(2,3);
- pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test,
- point2D_test7,
- jacobian_test3);
-
- std::cout << std::endl << "Third function output" << std::endl;
- std::cout << "x: " << point2D_test7[0] << "; y: " << point2D_test7[1] << std::endl;
- std::cout << "\n-->Jacobian" << std::endl <<
- jacobian_test3 << std::endl;
-
- //jacobian and distance
- Eigen::Vector2s point2D_test8;
- Eigen::MatrixXs jacobian_test4(2,3);
- Scalar distance_test3;
- pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test,
- point2D_test8,
- distance_test3,
- jacobian_test4);
-
- std::cout << std::endl << "Fourth function output" << std::endl;
- std::cout << "x: " << point2D_test8[0] << "; y: " << point2D_test8[1] << "; dist: " << distance_test3 << std::endl;
- std::cout << "\n-->Jacobian" << std::endl <<
- jacobian_test4 << std::endl;
-
- /////////////////////////////
-
-// //correction
-// correction_test2
-
-// //2Dpoint
-// point2D_test5
-
- Scalar depth3 = project_point_normalized_test[2];
-
- Eigen::Vector3s point3D_backproj5;
- point3D_backproj5 = pinhole::backprojectPoint(k_test2,
- correction_test2,
- point2D_test5,
- depth3);
-
- std::cout << "\n\nTEST backprojectPoint Complete" << std::endl;
- std::cout << std::endl << "First function output" << std::endl;
- std::cout << "x: " << point3D_backproj5[0] << "; y: " << point3D_backproj5[1] << "; z: " << point3D_backproj5[2] << std::endl;
-
- //jacobian
- Eigen::Vector3s point3D_backproj4;
- Eigen::MatrixXs jacobian_backproj2(3,2);
- Eigen::Vector3s depth_jacobian2;
- pinhole::backprojectPoint(k_test2,
- correction_test2,
- point2D_test7,
- depth3,
- point3D_backproj4,
- jacobian_backproj2,
- depth_jacobian2);
-
- std::cout << std::endl << "Second function output" << std::endl;
- std::cout << "x: " << point3D_backproj4[0] << "; y: " << point3D_backproj4[1] << "; z: " << point3D_backproj4[2] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- jacobian_backproj2 << std::endl;
- std::cout << "\n--> Jacobian - depth" << std::endl <<
- depth_jacobian2[0] << " " << depth_jacobian2[1] << " " << depth_jacobian2[2] << " " << std::endl;
-
- Eigen::Matrix2s test_jacobian_complete;
- test_jacobian_complete = jacobian_test4 * jacobian_backproj2;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
- test_jacobian_complete << std::endl;
-
- /* Test */
- std::cout << "\n\n\n\nTEST\n" << std::endl;
-
- Eigen::Matrix3s K;
- K(0,0) = 830.748734; K(0,1) = 0; K(0,2) = 327.219132;
- K(1,0) = 0; K(1,1) = 831.18208; K(1,2) = 234.720244;
- K(2,0) = 0; K(2,1) = 0; K(2,2) = 1;
-
- Eigen::Vector4s K_params = {327.219132,234.720244, 830.748734,831.18208};
-
- std::cout << "K:\n" << K << std::endl;
-
- Eigen::Vector4s distortion_vector = {0.0006579999999999999, 0.023847, -0.001878, 0.007706999999999999};
-
- std::cout << "\nDistortion vector:\n" << distortion_vector << std::endl;
-
- Eigen::Vector4s correction_vector;
- pinhole::computeCorrectionModel(K_params,
- distortion_vector,
- correction_vector);
-
- std::cout << "\nCorrection vector:\n" << correction_vector << std::endl;
-
- Eigen::Vector3s test_point3D;
- Eigen::Vector2s test_point2D = {123,321};
- std::cout << "\ntest_point2D ORIGINAL:\n" << test_point2D << std::endl;
-
- test_point2D = pinhole::depixellizePoint(K_params,
- test_point2D);
- std::cout << "\ntest_point2D DEPIXELIZED:\n" << test_point2D << std::endl;
- test_point2D = pinhole::undistortPoint(correction_vector,
- test_point2D);
- std::cout << "\ntest_point2D UNDISTORTED:\n" << test_point2D << std::endl;
- test_point3D = pinhole::backprojectPointFromNormalizedPlane(test_point2D,
- 2);
- std::cout << "\ntest_point3D BACKPROJECTED:\n" << test_point3D << std::endl;
-
- test_point2D = pinhole::projectPointToNormalizedPlane(test_point3D);
- std::cout << "\ntest_point2D NORMALIZED:\n" << test_point2D << std::endl;
- test_point2D = pinhole::distortPoint(distortion_vector,
- test_point2D);
- std::cout << "\ntest_point2D DISTORTED:\n" << test_point2D << std::endl;
- test_point2D = pinhole::pixellizePoint(K_params,
- test_point2D);
- std::cout << "\ntest_point2D PIXELIZED:\n" << test_point2D << std::endl;
-
-}
-
diff --git a/demos/demo_sh_ptr.cpp b/demos/demo_sh_ptr.cpp
deleted file mode 100644
index 3a10903c85ee72967dbe8ca23dd1584dbda494b8..0000000000000000000000000000000000000000
--- a/demos/demo_sh_ptr.cpp
+++ /dev/null
@@ -1,795 +0,0 @@
-/**
- * \file test_sh_ptr.cpp
- *
- * Created on: Oct 4, 2016
- * \author: jsola
- *
- * Complete Wolf tree skeleton with smart pointers
- *
- */
-
-// C++11
-#include <memory>
-
-using std::shared_ptr;
-using std::weak_ptr;
-using std::make_shared;
-using std::enable_shared_from_this;
-
-// std
-#include <list>
-#include <vector>
-#include <iostream>
-using std::list;
-using std::vector;
-using std::cout;
-using std::endl;
-
-namespace wolf
-{
-// fwds
-class H; // hardware
-class S; // sensor
-class p; // processor
-class T; // trajectory
-class F; // frame
-class C; // capture
-class f; // feature
-class c; // factor
-class M; // map
-class L; // landmark
-
-//////////////////////////////////////////////////////////////////////////////////
-// DECLARE WOLF TREE
-
-class P : public enable_shared_from_this<P>
-{
- private:
- shared_ptr<H> H_ptr_;
- shared_ptr<T> T_ptr_;
- shared_ptr<M> M_ptr_;
-
- public:
- P();
- ~P(){cout << "destruct P" << endl;}
- void setup();
- shared_ptr<H> getH(){return H_ptr_;}
- shared_ptr<T> getT(){return T_ptr_;}
- shared_ptr<M> getM(){return M_ptr_;}
-};
-
-class H : public enable_shared_from_this<H>
-{
- private:
- weak_ptr<P> P_ptr_;
- list<shared_ptr<S>> S_list_;
-
- public:
- H(){cout << "construct H" << endl;}
- ~H(){cout << "destruct H" << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- return P_sh;
- }
- void setP(const shared_ptr<P> _P){P_ptr_ = _P;}
- list<shared_ptr<S>>& getSlist() {return S_list_;}
- shared_ptr<S> add_S(shared_ptr<S> _S);
-};
-
-class S : public enable_shared_from_this<S>
-{
- private:
- weak_ptr<P> P_ptr_;
- weak_ptr<H> H_ptr_;
- list<shared_ptr<p>> p_list_;
- static int id_count_;
-
- // list<shared_ptr<C>> C_list_; // List of all captures
-
- public:
- int id;
- S():id(++id_count_){cout << "construct + S" << id << endl;}
- ~S(){cout << "destruct + S" << id << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- if (!P_sh)
- P_sh = getH()->getP();
- return P_sh;
- }
- void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
- shared_ptr<H> getH(){
- shared_ptr<H> H_sh = H_ptr_.lock();
- return H_sh;
- }
- void setH(const shared_ptr<H> _H){H_ptr_ = _H;}
- list<shared_ptr<p>>& getplist() {return p_list_;}
- shared_ptr<p> add_p(shared_ptr<p> _p);
-};
-
-class p : public enable_shared_from_this<p>
-{
- private:
- weak_ptr<P> P_ptr_;
- weak_ptr<S> S_ptr_;
- static int id_count_;
-
- public:
- int id;
- p():id(++id_count_){cout << "construct + p" << id << endl;}
- ~p(){cout << "destruct + p" << id << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- if (!P_sh)
- P_sh = getS()->getP();
- return P_sh;
- }
- void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
- shared_ptr<S> getS(){
- shared_ptr<S> S_sh = S_ptr_.lock();
- return S_sh;
- }
- void setS(const shared_ptr<S> _S){S_ptr_ = _S;}
-};
-
-class T : public enable_shared_from_this<T>
-{
- private:
- weak_ptr<P> P_ptr_;
- list<shared_ptr<F>> F_list_;
-
- public:
- T(){cout << "construct T" << endl;}
- ~T(){cout << "destruct T" << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- return P_sh;
- }
- void setP(const shared_ptr<P> _P){P_ptr_ = _P;}
- list<shared_ptr<F>>& getFlist() {return F_list_;}
- shared_ptr<F> add_F(shared_ptr<F> _F);
-};
-
-class F : public enable_shared_from_this<F>
-{
- private:
- weak_ptr<P> P_ptr_;
- weak_ptr<T> T_ptr_;
- list<shared_ptr<C>> C_list_;
-
- list<shared_ptr<c>> c_by_list; // list of factors to this frame
-
- static int id_count_;
- bool is_removing;
-
- public:
- int id;
- F() :is_removing(false),id(++id_count_){cout << "construct + F" << id << endl;}
- ~F(){cout << "destruct + F" << id << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- if (!P_sh)
- P_sh = getT()->getP();
- return P_sh;
- }
- void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
- shared_ptr<T> getT(){
- shared_ptr<T> T_sh = T_ptr_.lock();
- return T_sh;
- }
- void setT(const shared_ptr<T> _T){T_ptr_ = _T;}
- list<shared_ptr<C>>& getClist() {return C_list_;}
- list<shared_ptr<c>>& getCbyList(){return c_by_list;}
- shared_ptr<C> add_C(shared_ptr<C> _C);
- void add_c_by(shared_ptr<c> _cby)
- {
- c_by_list.push_back(_cby);
- }
- void remove();
-
-};
-
-class C : public enable_shared_from_this<C>
-{
- private:
- weak_ptr<P> P_ptr_;
- weak_ptr<F> F_ptr_;
- list<shared_ptr<f>> f_list_;
-
- weak_ptr<S> S_ptr_; // sensor
- static int id_count_;
- bool is_deleting;
-
- public:
- int id;
- C():is_deleting(false),id(++id_count_){cout << "construct + C" << id << endl;}
- ~C(){cout << "destruct + C" << id << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- if (!P_sh)
- P_sh = getF()->getP();
- return P_sh;
- }
- void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
- shared_ptr<F> getF(){
- shared_ptr<F> F_sh = F_ptr_.lock();
- return F_sh;
- }
- void setF(const shared_ptr<F> _F){F_ptr_ = _F;}
- list<shared_ptr<f>>& getflist() {return f_list_;}
- shared_ptr<f> add_f(shared_ptr<f> _f);
- void remove();
-
-};
-
-class f : public enable_shared_from_this<f>
-{
- private:
- weak_ptr<P> P_ptr_;
- weak_ptr<C> C_ptr_;
- list<shared_ptr<c>> c_list_;
-
- list<shared_ptr<c>> c_by_list; // list of factors to this feature
-
- static int id_count_;
- bool is_deleting;
-
- public:
- int id;
- f():is_deleting(false),id(++id_count_){cout << "construct + f" << id << endl;}
- ~f(){cout << "destruct + f" << id << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- if (!P_sh)
- P_sh = getC()->getP();
- return P_sh;
- }
- void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
- shared_ptr<C> getC(){
- shared_ptr<C> C_sh = C_ptr_.lock();
- return C_sh;
- }
- void setC(const shared_ptr<C> _C){C_ptr_ = _C;}
- list<shared_ptr<c>>& getclist() {return c_list_;}
- list<shared_ptr<c>>& getCbyList(){return c_by_list;}
- shared_ptr<c> add_c(shared_ptr<c> _c);
- void add_c_by(shared_ptr<c> _cby)
- {
- c_by_list.push_back(_cby);
- }
- void remove();
-};
-
-class c : public enable_shared_from_this<c>
-{
- private:
- weak_ptr<P> P_ptr_;
- weak_ptr<f> f_ptr_; // change this to shared??
-
- // can we have just one pointer? Derive 3 classes from c?
- weak_ptr<F> F_other_ptr_; // change this to shared?
- weak_ptr<f> f_other_ptr_; // change this to shared?
- weak_ptr<L> L_other_ptr_; // change this to shared?
-
- static int id_count_;
- bool is_deleting;
-
- public:
- int id;
- enum{c0, cF, cf, cL} type;
- c():is_deleting(false),id(++id_count_){type = c0; cout << "construct + c" << id << endl;}
- c(shared_ptr<F> _F_other);
- c(shared_ptr<f> _f_other);
- c(shared_ptr<L> _L_other);
- ~c(){cout << "destruct + c" << id << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- if (!P_sh)
- P_sh = getf()->getP();
- return P_sh;
- }
- void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
- shared_ptr<f> getf(){
- shared_ptr<f> f_sh = f_ptr_.lock();
- return f_sh;
- }
- void setf(const shared_ptr<f> _f){f_ptr_ = _f;}
- shared_ptr<F> getFother(){return F_other_ptr_.lock();}
- shared_ptr<f> getfother(){return f_other_ptr_.lock();}
- shared_ptr<L> getLother(){return L_other_ptr_.lock();}
- void remove();
-};
-
-class M : public enable_shared_from_this<M>
-{
- private:
- weak_ptr<P> P_ptr_;
- list<shared_ptr<L>> L_list_;
-
- public:
- M(){cout << "construct M" << endl;}
- ~M(){cout << "destruct M" << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- return P_sh;
- }
- void setP(const shared_ptr<P> _P){P_ptr_ = _P;}
- list<shared_ptr<L>>& getLlist() {return L_list_;}
- shared_ptr<L> add_L(shared_ptr<L> _L);
-};
-
-class L : public enable_shared_from_this<L>
-{
- private:
- weak_ptr<P> P_ptr_;
- weak_ptr<M> M_ptr_;
-
- list<shared_ptr<c>> c_by_list; // list of factors to this landmark
-
- static int id_count_;
- bool is_deleting;
-
- public:
- int id;
- L():is_deleting(false),id(++id_count_){cout << "construct + L" << id << endl;}
- ~L(){cout << "destruct + L" << id << endl;}
- shared_ptr<P> getP(){
- shared_ptr<P> P_sh = P_ptr_.lock();
- if (!P_sh)
- P_sh = getM()->getP();
- return P_sh;
- }
- void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
- shared_ptr<M> getM(){
- shared_ptr<M> M_sh = M_ptr_.lock();
- return M_sh;
- }
- void setM(const shared_ptr<M> _M){M_ptr_ = _M;}
- list<shared_ptr<c>>& getCbyList(){return c_by_list;}
- void add_c_by(shared_ptr<c> _cby)
- {
- c_by_list.push_back(_cby);
- }
- void remove();
-};
-
-//////////////////////////////////////////////////////////////////////////////////
-// IMPLEMENTATION of some methods
-
-P::P(){
- cout << "construct P" << endl;
- H_ptr_ = make_shared<H>();
- T_ptr_ = make_shared<T>();
- M_ptr_ = make_shared<M>();
- cout << "P is constructed" << endl;
-}
-
-void P::setup()
-{
- H_ptr_->setP(shared_from_this());
- T_ptr_->setP(shared_from_this());
- M_ptr_->setP(shared_from_this());
- cout << "P is set up" << endl;
-}
-
-shared_ptr<S> H::add_S(shared_ptr<S> _S)
-{
- S_list_.push_back(_S);
- _S->setH(shared_from_this());
- _S->setP(getP());
- return _S;
-}
-
-shared_ptr<p> S::add_p(shared_ptr<p> _p)
-{
- p_list_.push_back(_p);
- _p->setS(shared_from_this());
- _p->setP(getP());
- return _p;
-}
-
-shared_ptr<F> T::add_F(shared_ptr<F> _F)
-{
- F_list_.push_back(_F);
- _F->setT(shared_from_this());
- _F->setP(getP());
- return _F;
-}
-
-shared_ptr<C> F::add_C(shared_ptr<C> _C)
-{
- C_list_.push_back(_C);
- _C->setF(shared_from_this());
- _C->setP(getP());
- return _C;
-}
-void F::remove()
-{
- if (!is_removing)
- {
- is_removing = true;
- cout << "Removing F" << id << endl;
- shared_ptr<F> this_F = shared_from_this(); // keep this alive while removing it
- getT()->getFlist().remove(this_F); // remove from upstream
- while (!C_list_.empty())
- C_list_.front()->remove(); // remove downstream
- while (!c_by_list.empty())
- c_by_list.front()->remove(); // remove constrained
- }
-}
-
-shared_ptr<f> C::add_f(shared_ptr<f> _f)
-{
- f_list_.push_back(_f);
- _f->setC(shared_from_this());
- _f->setP(getP());
- return _f;
-}
-void C::remove()
-{
- if (!is_deleting)
- {
- is_deleting = true;
- cout << "Removing C" << id << endl;
- shared_ptr<C> this_C = shared_from_this(); // keep this alive while removing it
- getF()->getClist().remove(this_C); // remove from upstream
- if (getF()->getClist().empty() && getF()->getCbyList().empty())
- getF()->remove(); // remove upstream
- while (!f_list_.empty())
- f_list_.front()->remove(); // remove downstream
- }
-}
-
-shared_ptr<c> f::add_c(shared_ptr<c> _c)
-{
- c_list_.push_back(_c);
- _c->setf(shared_from_this());
- _c->setP(getP());
- return _c;
-}
-void f::remove()
-{
- if (!is_deleting)
- {
- is_deleting = true;
- cout << "Removing f" << id << endl;
- shared_ptr<f> this_f = shared_from_this(); // keep this alive while removing it
- getC()->getflist().remove(this_f); // remove from upstream
- if (getC()->getflist().empty())
- getC()->remove(); // remove upstream
- while (!c_list_.empty())
- c_list_.front()->remove(); // remove downstream
- while (!c_by_list.empty())
- c_by_list.front()->remove(); // remove constrained
- }
-}
-
-c::c(shared_ptr<F> _F_other):is_deleting(false),id(++id_count_)
-{
- cout << "construct + c" << id << " -> F" << _F_other->id << endl;
- type = cF;
- F_other_ptr_ = _F_other;
-}
-
-c::c(shared_ptr<f> _f_other):is_deleting(false),id(++id_count_)
-{
- cout << "construct + c" << id << " -> f" << _f_other->id << endl;
- type = cf;
- f_other_ptr_ = _f_other;
-}
-
-c::c(shared_ptr<L> _L_other):is_deleting(false),id(++id_count_)
-{
- cout << "construct + c" << id << " -> L" << _L_other->id << endl;
- type = cL;
- L_other_ptr_ = _L_other;
-}
-
-void c::remove()
-{
- if (!is_deleting)
- {
- is_deleting = true;
- cout << "Removing c" << id << endl;
- shared_ptr<c> this_c = shared_from_this(); // keep this alive while removing it
- getf()->getclist().remove(this_c); // remove from upstream
- if (getf()->getclist().empty() && getf()->getCbyList().empty())
- getf()->remove(); // remove upstream
-
- // remove other: {Frame, feature, Landmark}
- switch (type)
- {
- case c::cF:
- getFother()->getCbyList().remove(this_c);
- if (getFother()->getCbyList().empty() && getFother()->getClist().empty())
- getFother()->remove();
- break;
- case c::cf:
- getfother()->getCbyList().remove(this_c);
- if (getfother()->getCbyList().empty() && getfother()->getclist().empty())
- getfother()->remove();
- break;
- case c::cL:
- getLother()->getCbyList().remove(this_c);
- if (getLother()->getCbyList().empty())
- getLother()->remove();
- break;
- default:
- break;
- }
- }
-}
-
-shared_ptr<L> M::add_L(shared_ptr<L> _L)
-{
- L_list_.push_back(_L);
- _L->setM(shared_from_this());
- _L->setP(getP());
- return _L;
-}
-
-void L::remove()
-{
- if (!is_deleting)
- {
- is_deleting = true;
- cout << "Removing L" << id << endl;
- shared_ptr<L> this_L = shared_from_this(); // keep this alive while removing it
- getM()->getLlist().remove(this_L); // remove from upstream
- while (!c_by_list.empty())
- c_by_list.front()->remove(); // remove constrained
- }
-}
-
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// HELPERS
-
-using namespace wolf;
-
-void print_cF(const shared_ptr<P>& Pp)
-{
- cout << "Frame factors" << endl;
- for (auto Fp : Pp->getT()->getFlist())
- {
- cout << "F" << Fp->id << " @ " << Fp.get() << endl;
- for (auto cp : Fp->getCbyList())
- {
- cout << " -> c" << cp->id << " @ " << cp.get()
- << " -> F" << cp->getFother()->id << " @ " << cp->getFother().get() << endl;
- }
- }
-}
-
-void print_cf(const shared_ptr<P>& Pp)
-{
- cout << "Feature factors" << endl;
- for (auto Fp : Pp->getT()->getFlist())
- {
- for (auto Cp : Fp->getClist())
- {
- for (auto fp : Cp->getflist())
- {
- cout << "f" << fp->id << " @ " << fp.get() << endl;
- for (auto cp : fp->getCbyList())
- {
- cout << " -> c" << cp->id << " @ " << cp.get()
- << " -> f" << cp->getfother()->id << " @ " << cp->getfother().get() << endl;
- }
- }
- }
- }
-}
-
-void print_cL(const shared_ptr<P>& Pp)
-{
- cout << "Landmark factors" << endl;
- for (auto Lp : Pp->getM()->getLlist())
- {
- cout << "L" << Lp->id << " @ " << Lp.get() << endl;
- for (auto cp : Lp->getCbyList())
- {
- cout << " -> c" << cp->id << " @ " << cp.get()
- << " -> L" << cp->getLother()->id << " @ " << cp->getLother().get() << endl;
- }
- }
-}
-
-void print_c(const shared_ptr<P>& Pp)
-{
- cout << "All factors" << endl;
- for (auto Fp : Pp->getT()->getFlist())
- {
- for (auto Cp : Fp->getClist())
- {
- for (auto fp : Cp->getflist())
- {
- for (auto cp : fp->getclist())
- {
- if (cp)
- {
- switch (cp->type)
- {
- case c::cF:
- cout << "c" << cp->id << " @ " << cp.get()
- << " -> F" << cp->getFother()->id << " @ " << cp->getFother() << endl;
- break;
- case c::cf:
- cout << "c" << cp->id << " @ " << cp.get()
- << " -> f" << cp->getfother()->id << " @ " << cp->getfother() << endl;
- break;
- case c::cL:
- cout << "c" << cp->id << " @ " << cp.get()
- << " -> L" << cp->getLother()->id << " @ " << cp->getLother() << endl;
- break;
- default:
- cout << "Bad factor" << endl;
- break;
- }
- }
- }
- }
- }
- }
-}
-
-/** Build problem
- * See the problem built here:
- * https://docs.google.com/drawings/d/1vYmhBjJz7AHxOdy0gV-77hqsOYfGVuvUmnRVB-Zfp_Q/edit
- */
-shared_ptr<P> buildProblem(int N)
-{
- shared_ptr<P> Pp = make_shared<P>();
- Pp->setup();
- // H
- for (int Si = 0; Si < 2; Si++)
- {
- shared_ptr<S> Sp = Pp->getH()->add_S(make_shared<S>());
- for (int pi = 0; pi < 2; pi++)
- {
- shared_ptr<p> pp = Sp->add_p(make_shared<p>());
- }
- }
- // M
- for (int Li = 0; Li < 2; Li++)
- {
- shared_ptr<L> Lp = Pp->getM()->add_L(make_shared<L>());
- }
- // T
- list<shared_ptr<L> >::iterator Lit = Pp->getM()->getLlist().begin();
- vector<weak_ptr<F> > Fvec(N);
- for (int Fi = 0; Fi < N; Fi++)
- {
- shared_ptr<F> Fp = Pp->getT()->add_F(make_shared<F>());
- Fvec.at(Fi) = Fp;
- for (int Ci = 0; Ci < 2; Ci++)
- {
- shared_ptr<C> Cp = Fp->add_C(make_shared<C>());
- for (int fi = 0; fi < 1; fi++)
- {
- shared_ptr<f> fp = Cp->add_f(make_shared<f>());
- if (Ci || !Fi) // landmark factor
- {
- shared_ptr<c> cp = fp->add_c(make_shared<c>(*Lit));
- (*Lit)->add_c_by(cp);
- Lit++;
- if (Lit == Pp->getM()->getLlist().end())
- Lit = Pp->getM()->getLlist().begin();
- }
- else // motion factor
- {
- shared_ptr<F> Fp = Fvec.at(Fi-1).lock();
- if (Fp)
- {
- shared_ptr<c> cp = fp->add_c(make_shared<c>(Fp));
- Fp->add_c_by(cp);
- }
- else
- cout << "Could not constrain Frame" << endl;
- }
- }
- }
- }
- return Pp;
-}
-
-// init ID factories
-int S::id_count_ = 0;
-int p::id_count_ = 0;
-int F::id_count_ = 0;
-int C::id_count_ = 0;
-int f::id_count_ = 0;
-int c::id_count_ = 0;
-int L::id_count_ = 0;
-
-// tests
-void removeFactors(const shared_ptr<P>& Pp)
-{
- cout << "Removing factor type L ----------" << endl;
- Pp->getT()->getFlist().front()->getClist().front()->getflist().front()->getclist().front()->remove();
- cout << "Removing factor type L ----------" << endl;
- Pp->getT()->getFlist().front()->getClist().front()->getflist().front()->getclist().front()->remove();
- cout << "Removing factor type F ----------" << endl;
- Pp->getT()->getFlist().back()->getClist().front()->getflist().front()->getclist().front()->remove();
- cout << "Removing factor type L ----------" << endl;
- Pp->getT()->getFlist().back()->getClist().back()->getflist().front()->getclist().front()->remove();
- cout << "Removing factor type F ----------" << endl;
- Pp->getT()->getFlist().back()->getClist().front()->getflist().front()->getclist().front()->remove();
-}
-
-void removeLandmarks(const shared_ptr<P>& Pp)
-{
- cout << "Removing landmark ----------" << endl;
- Pp->getM()->getLlist().front()->remove();
- cout << "Removing landmark ----------" << endl;
- Pp->getM()->getLlist().front()->remove();
-}
-
-void removeFrames(const shared_ptr<P>& Pp)
-{
- cout << "Removing frame ----------" << endl;
- Pp->getT()->getFlist().back()->remove();
- cout << "Removing frame ----------" << endl;
- Pp->getT()->getFlist().front()->remove();
- cout << "Removing frame ----------" << endl;
- Pp->getT()->getFlist().back()->remove();
-}
-
-void removeLmksAndFrames(const shared_ptr<P>& Pp)
-{
- cout << "Removing landmark ----------" << endl;
- Pp->getM()->getLlist().front()->remove();
- cout << "Removing frame ----------" << endl;
- Pp->getT()->getFlist().front()->remove();
- cout << "Removing frame ----------" << endl;
- Pp->getT()->getFlist().back()->remove();
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// MAIN
-int main()
-{
- int N = 3;
-
- shared_ptr<P> Pp = buildProblem(N);
- cout << "Wolf tree created ----------------------------" << endl;
-
- cout << "\nShowing factors --------------------------" << endl;
- cout<<endl;
- print_cF(Pp);
- cout<<endl;
- print_cf(Pp);
- cout<<endl;
- print_cL(Pp);
- cout<<endl;
- print_c(Pp);
-
- //------------------------------------------------------------------
- // Several tests. Uncomment the desired test.
- // Run only one test at a time, otherwise you'll get segfaults!
-
-// cout << "\nRemoving factors -------------------------" << endl;
-// removeFactors(Pp);
-
-// cout << "\nRemoving landmarks ---------------------------" << endl;
-// removeLandmarks(Pp);
-
-// cout << "\nRemoving frames ------------------------------" << endl;
-// removeFrames(Pp);
-
- cout << "\nRemoving lmks and frames ------------------------------" << endl;
- removeLmksAndFrames(Pp);
-
-// cout << "\nRemoving problem ---------------------------" << endl;
-// Pp.reset();
-
-// cout << "\nRebuilding problem ---------------------------" << endl;
-// Pp = buildProblem(N);
-
- //------------------------------------------------------------------
-
- cout << "\nExiting main() -------------------------------" << endl;
-
- return 1;
-}
-
diff --git a/demos/demo_simple_AHP.cpp b/demos/demo_simple_AHP.cpp
deleted file mode 100644
index f4316cb071860fc7340606e9ef0521d0b9a3aea4..0000000000000000000000000000000000000000
--- a/demos/demo_simple_AHP.cpp
+++ /dev/null
@@ -1,257 +0,0 @@
-/**
- * \file test_simple_AHP.cpp
- *
- * Created on: 2 nov. 2016
- * \author: jtarraso
- */
-
-#include "core/common/wolf.h"
-#include "core/frame/frame_base.h"
-#include "core/math/pinhole_tools.h"
-#include "core/sensor/sensor_camera.h"
-#include "core/math/rotations.h"
-#include "core/capture/capture_image.h"
-#include "core/landmark/landmark_AHP.h"
-#include "core/factor/factor_AHP.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// Vision utils
-#include <vision_utils/vision_utils.h>
-
-/**
- * This test simulates the following situation:
- * - A kf at the origin, we use it as anchor: kf1
- * - A kf at the origin, we use it to project lmks onto the anchor frame: kf2
- * - A kf at 1m to the right of the origin, kf3
- * - A kf at 1m to the left of the origin, kf4
- * - A lmk at 1m distance in front of the anchor: L1
- * - we use this lmk to project it onto kf2, kf3 and kf4 and obtain measured pixels p0, p1 and p2
- * - A lmk initialized at kf1, with measurement p0, at a distance of 2m: L2
- * - we project the pixels p3 and p4: we observe that they do not match p1 and p2
- * - we use the measurements p1 and p2 to solve the optimization problem on L2: L2 should converge to L1.
- * - This is a sketch of the situation:
- * - X, Y are the axes in world frame
- * - x, z are the axes in anchor camera frame. We have that X=z, Y=-x.
- * - Axes Z and y are perpendicular to the drawing; they have no effect.
- *
- * X,z
- * ^
- * |
- * L2 * 2
- * .|.
- * . | .
- * . | .
- * . | .
- * . L1 * 1 .
- * . . | . .
- * . . | . .
- * p4 . . | . . p3
- * .. p2 | p1 ..
- * Y <--+---------+---------+
- * -x +1 0 -1
- * kf4 kf1 kf3
- * kf2
- *
- * camera: (uo,vo) = (320,240)
- * (au,av) = (320,320)
- *
- * projection geometry:
- *
- * 1:1 2:1 1:0 2:1 1:1
- * 0 160 320 480 640
- * +----+----+----+----+
- * |
- * | 320
- * |
- * *
- *
- * projected pixels:
- * p0 = (320,240) // at optical axis or relation 1:0
- * p1 = ( 0 ,240) // at 45 deg or relation 1:1
- * p2 = (640,240) // at 45 deg or relation 1:1
- * p3 = (160,240) // at a relation 2:1
- * p4 = (480,240) // at a relation 2:1
- *
- */
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using namespace Eigen;
-
- /* 1. crear 2 kf, fixed
- * 2. crear 1 sensor
- * 3. crear 1 lmk1
- * 4. projectar lmk sobre sensor a fk1
- * 5. projectar lmk sobre sensor a kf4
- * 6. // esborrar lmk lmk_ptr->remove() no cal
- * 7. crear nous kf
- * 8. crear captures
- * 9. crear features amb les mesures de 4 i 5
- * 10. crear lmk2 des de kf3
- * 11. crear factor des del kf4 a lmk2, amb ancora al kf3
- * 12. solve
- * 13. lmk1 == lmk2 ?
- */
-
- // ============================================================================================================
- /* 1 */
- ProblemPtr problem = Problem::create("PO", 3);
- // One anchor frame to define the lmk, and a copy to make a factor
- FrameBasePtr kf_1 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
- // and two other frames to observe the lmk
- FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
-
- kf_1->fix();
- kf_2->fix();
- kf_3->fix();
- kf_4->fix();
- // ============================================================================================================
-
- // ============================================================================================================
- /* 2 */
- std::string wolf_root = _WOLF_ROOT_DIR;
- SensorBasePtr sensor_ptr = problem->installSensor("CAMERA", "PinHole", (Vector7s()<<0,0,0,-0.5,0.5,-0.5,0.5).finished(), wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sensor_ptr);
- // ============================================================================================================
-
- // ============================================================================================================
- /* 3 */
- Eigen::Vector3s lmk_dir = {0,0,1}; // in the optical axis of the anchor camera at kf1
- std::cout << std::endl << "lmk: " << lmk_dir.transpose() << std::endl;
- lmk_dir.normalize();
- Eigen::Vector4s lmk_hmg_c;
- Scalar distance = 1.0; // from anchor at kf1
- lmk_hmg_c = {lmk_dir(0),lmk_dir(1),lmk_dir(2),(1/distance)};
-// std::cout << "lmk hmg in C frame: " << lmk_hmg_c.transpose() << std::endl;
- // ============================================================================================================
-
- // Captures------------------
- cv::Mat cv_image;
- cv_image.zeros(2,2,0);
- CaptureImagePtr image_0 = std::make_shared<CaptureImage>(TimeStamp(0), camera, cv_image);
- CaptureImagePtr image_1 = std::make_shared<CaptureImage>(TimeStamp(1), camera, cv_image);
- CaptureImagePtr image_2 = std::make_shared<CaptureImage>(TimeStamp(2), camera, cv_image);
- kf_2->addCapture(image_0);
- kf_3->addCapture(image_1);
- kf_4->addCapture(image_2);
-
- // Features-----------------
- cv::Mat desc;
-
- cv::KeyPoint kp_0;
- FeaturePointImagePtr feat_0 = std::make_shared<FeaturePointImage>(kp_0, 0, desc, Eigen::Matrix2s::Identity());
- image_0->addFeature(feat_0);
-
- cv::KeyPoint kp_1;
- FeaturePointImagePtr feat_1 = std::make_shared<FeaturePointImage>(kp_1, 0, desc, Eigen::Matrix2s::Identity());
- image_1->addFeature(feat_1);
-
- cv::KeyPoint kp_2;
- FeaturePointImagePtr feat_2 = std::make_shared<FeaturePointImage>(kp_2, 0, desc, Eigen::Matrix2s::Identity());
- image_2->addFeature(feat_2);
-
- // Landmark--------------------
- LandmarkAHPPtr lmk_1 = std::make_shared<LandmarkAHP>(lmk_hmg_c, kf_1, camera, desc);
- problem->addLandmark(lmk_1);
- lmk_1->fix();
- std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
-
- // Factors------------------
- FactorAHPPtr fac_0 = FactorAHP::create(feat_0, lmk_1, nullptr);
- feat_0->addFactor(fac_0);
- FactorAHPPtr fac_1 = FactorAHP::create(feat_1, lmk_1, nullptr);
- feat_1->addFactor(fac_1);
- FactorAHPPtr fac_2 = FactorAHP::create(feat_2, lmk_1, nullptr);
- feat_2->addFactor(fac_2);
-
- // Projections----------------------------
- Eigen::VectorXs pix_0 = fac_0->expectation();
- kp_0 = cv::KeyPoint(pix_0(0), pix_0(1), 0);
- feat_0->setKeypoint(kp_0);
-
- Eigen::VectorXs pix_1 = fac_1->expectation();
- kp_1 = cv::KeyPoint(pix_1(0), pix_1(1), 0);
- feat_1->setKeypoint(kp_1);
-
- Eigen::VectorXs pix_2 = fac_2->expectation();
- kp_2 = cv::KeyPoint(pix_2(0), pix_2(1), 0);
- feat_2->setKeypoint(kp_2);
-
- std::cout << "pixel 0: " << pix_0.transpose() << std::endl;
- std::cout << "pixel 1: " << pix_1.transpose() << std::endl;
- std::cout << "pixel 2: " << pix_2.transpose() << std::endl;
- //
- //======== up to here the initial projections ==============
-
- std::cout << "\n";
-
- //======== now we want to estimate a new lmk ===============
- //
- // Features -----------------
- FeaturePointImagePtr feat_3 = std::make_shared<FeaturePointImage>(kp_1, 0, desc, Eigen::Matrix2s::Identity());
- image_1->addFeature(feat_3);
-
- FeaturePointImagePtr feat_4 = std::make_shared<FeaturePointImage>(kp_2, 0, desc, Eigen::Matrix2s::Identity());
- image_2->addFeature(feat_4);
-
- // New landmark with measured pixels from kf2 (anchor) kf3 and kf4 (measurements)
- Scalar unknown_distance = 2; // the real distance is 1
- Matrix3s K = camera->getIntrinsicMatrix();
- Vector3s pix_0_hmg;
- pix_0_hmg << pix_0, 1;
- Eigen::Vector3s dir_0 = K.inverse() * pix_0_hmg;
- Eigen::Vector4s pnt_hmg_0;
- pnt_hmg_0 << dir_0, 1/unknown_distance;
- LandmarkAHPPtr lmk_2( std::make_shared<LandmarkAHP>(pnt_hmg_0, kf_2, camera, desc) );
- problem->addLandmark(lmk_2);
- std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
-
- // New factors from kf3 and kf4
- FactorAHPPtr fac_3 = FactorAHP::create(feat_3, lmk_2, nullptr);
- feat_3->addFactor(fac_3);
- FactorAHPPtr fac_4 = FactorAHP::create(feat_4, lmk_2, nullptr);
- feat_4->addFactor(fac_4);
-
- Eigen::Vector2s pix_3 = fac_3->expectation();
- Eigen::Vector2s pix_4 = fac_4->expectation();
-
- std::cout << "pix 3: " << pix_3.transpose() << std::endl;
- std::cout << "pix 4: " << pix_4.transpose() << std::endl;
-
- // Wolf tree status ----------------------
- problem->print(3);
-// problem->check();
-
- // ========== solve ==================================================================================================
- /* 12 */
- // Ceres wrapper
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- google::InitGoogleLogging(argv[0]);
-
- CeresManager ceres_manager(problem, ceres_options);
-
- std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
- std::cout << summary << std::endl;
-
- // Test of convergence over the lmk params
- bool pass = (lmk_2->point() - lmk_1->point()).isMuchSmallerThan(1,Constants::EPS);
-
- std::cout << "Landmark 2 below should have converged to Landmark 1:" << std::endl;
- std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
- std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
- std::cout << "Landmark convergence test " << (pass ? "PASSED" : "FAILED") << std::endl;
- std::cout << std::endl;
-
- if (!pass)
- return -1;
- return 0;
-
-}
-
diff --git a/demos/demo_sort_keyframes.cpp b/demos/demo_sort_keyframes.cpp
deleted file mode 100644
index a1f225eddd560462b6af13e7898990cdaea6d4dc..0000000000000000000000000000000000000000
--- a/demos/demo_sort_keyframes.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * \file test_sort_keyframes.cpp
- *
- * Created on: May 24, 2016
- * \author: jvallve
- */
-
-// Wolf includes
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/frame/frame_base.h"
-#include "core/trajectory/trajectory_base.h"
-
-// STL includes
-#include <list>
-#include <memory>
-
-// General includes
-#include <iostream>
-
-using namespace wolf;
-
-void printFrames(ProblemPtr _problem_ptr)
-{
- std::cout << "TRAJECTORY:" << std::endl;
- for (auto frm : _problem_ptr->getTrajectory()->getFrameList())
- std::cout << "\t " << (frm->isKey() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl;
-}
-
-int main()
-{
- ProblemPtr problem_ptr = Problem::create(FRM_PO_2D);
-
- problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
- FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
- FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
- problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
- FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
- FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl;
- frm5->setEstimated();
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl;
- frm2->setEstimated();
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm3->id() << " new TimeStamp:" << 0.45 << std::endl;
- frm3->setTimeStamp(TimeStamp(0.45));
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm3->id() << " set KEY" << std::endl;
- frm3->setEstimated();
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl;
- frm6->setEstimated();
-
- printFrames(problem_ptr);
-
- FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
- std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
-
- printFrames(problem_ptr);
-
- FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
- std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
-
- printFrames(problem_ptr);
-
- return 0;
-}
diff --git a/demos/demo_sparsification.cpp b/demos/demo_sparsification.cpp
deleted file mode 100644
index a521cabd1b5b1276846573df70b53d2b70b6d537..0000000000000000000000000000000000000000
--- a/demos/demo_sparsification.cpp
+++ /dev/null
@@ -1,314 +0,0 @@
-// Sparsification example creating wolf tree from imported graph from .txt file
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//std includes
-#include <cstdlib>
-#include <fstream>
-#include <string>
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "core/capture/capture_void.h"
-#include "core/feature/feature_odom_2D.h"
-#include "core/factor/factor_base.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// EIGEN
-//#include <Eigen/CholmodSupport>
-#include <Eigen/StdVector> // Eigen in std vector
-
-namespace wolf{
-// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-
-void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
-{
- for (int k=0; k<ins.outerSize(); ++k)
- for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
- original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
-
- original.makeCompressed();
-}
-
-void decodeEdge(const std::string& buffer, unsigned int& edge_from, unsigned int& edge_to, Eigen::Vector3s& measurement, Eigen::Matrix3s& covariance)
-{
- std::string str_num;
-
- unsigned int i = 0;
-
- // only decode edges
- if (buffer.at(0) == 'E')
- {
- //skip rest of EDGE word
- while (buffer.at(i) != ' ') i++;
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // FROM ID
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- edge_from = atoi(str_num.c_str())+1;
- str_num.clear();
-
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // TO ID
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- edge_to = atoi(str_num.c_str())+1;
- str_num.clear();
-
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // MEASUREMENT
- // X
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- measurement(0) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // Y
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- measurement(1) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // THETA
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- measurement(2) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // INFORMATION
- Eigen::Matrix3s information;
- // XX
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- information(0,0) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // XY
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- information(0,1) = atof(str_num.c_str());
- information(1,0) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // YY
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- information(1,1) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // THETATHETA
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- information(2,2) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // XTHETA
- while (buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- information(0,2) = atof(str_num.c_str());
- information(2,0) = atof(str_num.c_str());
- str_num.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // YTHETA
- while (i < buffer.size() && buffer.at(i) != ' ')
- str_num.push_back(buffer.at(i++));
- information(1,2) = atof(str_num.c_str());
- information(2,1) = atof(str_num.c_str());
- str_num.clear();
-
- // COVARIANCE
- covariance = information.inverse();
- }
- else
- {
- edge_from = 0;
- edge_to = 0;
- }
-}
-
-}
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- //Welcome message
- std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
-
- bool wrong_input = false;
- if (argc < 3)
- wrong_input = true;
- else if (argc > 4)
- wrong_input = true;
- else if (argc > 2 && (atoi(argv[2]) < 2 || atoi(argv[2]) > 5))
- wrong_input = true;
- else if (argc > 3 && atoi(argv[3]) < 0 )
- wrong_input = true;
-
- if (wrong_input)
- {
- std::cout << "Please call me with: [./test_wolf_imported_graph DATASET T (MAX_VERTICES)], where:" << std::endl;
- std::cout << " DATASET: manhattan, killian or intel" << std::endl;
- std::cout << " T keep one node each T: 2, 3, 4 or 5" << std::endl;
- std::cout << " optional: MAX_VERTICES max edges to be loaded" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- // input variables
- char const * dataset_path = std::getenv("DATASET_PATH");
- unsigned int pruning_T = atoi(argv[2]);
- std::string file_path(dataset_path);
- file_path = file_path + "/graphs/redirected_" + std::to_string(pruning_T) + "_" + argv[1] + ".graph";
- unsigned int MAX_VERTEX = 1e9;
- if (argc > 3 && atoi(argv[3]) != 0)
- MAX_VERTEX = atoi(argv[3]);
-
- // Wolf problem
- FrameBasePtr last_frame_ptr, frame_from_ptr, frame_to_ptr;
- ProblemPtr bl_problem_ptr = Problem::create("PO_2D");
- SensorBasePtr sensor_ptr = bl_problem_ptr->installSensor("ODOM 2D", "Odometry", Eigen::VectorXs::Zero(3), IntrinsicsBasePtr());
-
- // Ceres wrapper
- std::string bl_summary, sp_summary;
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 10;
- CeresManagerPtr bl_ceres_manager = std::make_shared<CeresManager>(bl_problem_ptr, ceres_options);
-
- // load graph from .txt
- std::ifstream graph_file;
- graph_file.open(file_path.c_str(), std::ifstream::in);
- if (!graph_file.is_open())
- {
- printf("\nError opening file: %s\n",file_path.c_str());
- return -1;
- }
-
- // auxiliar variables
- std::string line_string;
- unsigned int edge_from, edge_to;
- Eigen::Vector3s meas;
- Eigen::Matrix3s meas_cov;
- Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
- //clock_t t1;
-
- // ------------------------ START EXPERIMENT ------------------------
- // First frame FIXED
- last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0));
- last_frame_ptr->fix();
- bl_problem_ptr->print(4, true, false, true);
-
- while (std::getline(graph_file, line_string) && last_frame_ptr->id() <= MAX_VERTEX)
- {
- std::cout << "new line:" << line_string << std::endl;
- decodeEdge(line_string, edge_from, edge_to, meas, meas_cov);
-
- // only factors
- if (edge_from != 0)
- {
-
- // ODOMETRY -------------------
- if (edge_to > last_frame_ptr->id())
- {
- frame_from_ptr = last_frame_ptr;
-
- // NEW KEYFRAME
- Eigen::Vector3s from_pose = frame_from_ptr->getState();
- R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix();
- Eigen::Vector3s new_frame_pose = from_pose + R*meas;
- last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to)));
-
- frame_to_ptr = last_frame_ptr;
-
- std::cout << "NEW FRAME " << last_frame_ptr->id() << " - ts = " << last_frame_ptr->getTimeStamp().get() << std::endl;
-
- // REMOVE PREVIOUS NODES
- }
- // LOOP CLOSURE ---------------
- else
- {
- if (edge_from == last_frame_ptr->id())
- frame_from_ptr = last_frame_ptr;
- else
- for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
- if ((*frm_rit)->id() == edge_from)
- {
- frame_from_ptr = *frm_rit;
- break;
- }
- if (edge_to == last_frame_ptr->id())
- frame_to_ptr = last_frame_ptr;
- else
- for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
- if ((*frm_rit)->id() == edge_to)
- {
- frame_to_ptr = *frm_rit;
- break;
- }
- }
-// std::cout << "frame_from " << frame_from_ptr->id() << std::endl;
-// std::cout << "edge_from " << edge_from << std::endl;
-// std::cout << "frame_to " << frame_to_ptr->id() << std::endl;
-// std::cout << "edge_to " << edge_to << std::endl;
-
- assert(frame_from_ptr->id() == edge_from && "frame from id and edge from idx must be the same");
- assert(frame_to_ptr->id() == edge_to && "frame to id and edge to idx must be the same");
-
- // CAPTURE
- CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- frame_from_ptr->addCapture(capture_ptr);
-
- // FEATURE
- FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(meas, meas_cov);
- capture_ptr->addFeature(feature_ptr);
-
- // CONSTRAINT
- FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, frame_to_ptr);
- feature_ptr->addFactor(factor_ptr);
- frame_to_ptr->addConstrainedBy(factor_ptr);
-
- // SOLVE
- // solution
- bl_summary = bl_ceres_manager->solve(SolverManager::ReportVerbosity::FULL);
- std::cout << bl_summary << std::endl;
-
- // covariance
- bl_ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);//ALL_MARGINALS
-
- // t1 = clock();
- // double t_sigma_manual = 0;
- // t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- }
- }
-
- //bl_problem_ptr->print(4, true, false, true);
-
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_state_quaternion.cpp b/demos/demo_state_quaternion.cpp
deleted file mode 100644
index 6528a2b3935e488c7534c44f031a7cff22a112ed..0000000000000000000000000000000000000000
--- a/demos/demo_state_quaternion.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * \file test_state_quaternion.cpp
- *
- * Created on: Mar 7, 2016
- * \author: jsola
- */
-
-#include "core/frame/frame_base.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/common/time_stamp.h"
-
-#include <iostream>
-
-int main (void)
-{
- using namespace wolf;
-
- // 3D
- StateBlockPtr pp = std::make_shared<StateBlock>(3);
- StateQuaternionPtr op = std::make_shared<StateQuaternion>();
- StateBlockPtr vp = std::make_shared<StateBlock>(3);
-
- TimeStamp t;
-
- FrameBase pqv(t,pp,op,vp);
-
- std::cout << "P local param: " << pqv.getP()->getLocalParametrization() << std::endl;
- std::cout << "Q local param: " << pqv.getO()->getLocalParametrization() << std::endl;
- std::cout << "V local param: " << pqv.getV()->getLocalParametrization() << std::endl;
-
- // delete pp;
- // delete op;
- // delete vp;
- // Note: Deleting the StateBlock pointers will be done at the destruction of FrameBase.
-
- std::cout << "Done" << std::endl;
-
- return 1;
-}
diff --git a/demos/demo_tracker_ORB.cpp b/demos/demo_tracker_ORB.cpp
deleted file mode 100644
index 89ea1800adf53cd76e7e858ab8c3c1fe1e0fb9ed..0000000000000000000000000000000000000000
--- a/demos/demo_tracker_ORB.cpp
+++ /dev/null
@@ -1,267 +0,0 @@
-//std includes
-#include <iostream>
-
-// Vision utils
-#include <vision_utils.h>
-#include <sensors.h>
-#include <common_class/buffer.h>
-#include <common_class/frame.h>
-#include <detectors/orb/detector_orb.h>
-#include <descriptors/orb/descriptor_orb.h>
-#include <matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h>
-
-//Wolf
-#include "core/processor/processor_tracker_landmark_image.h"
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << std::endl << "==================== tracker ORB test ======================" << std::endl;
-
- //=====================================================
- // Environment variable for configuration files
- std::string wolf_root = _WOLF_ROOT_DIR;
- //=====================================================
-
- //=====================================================
-
- // Sensor or sensor recording
- vision_utils::SensorCameraPtr sen_ptr = vision_utils::askUserSource(argc, argv);
- if (sen_ptr==NULL)
- return 0;
-
- // Detector
- vision_utils::DetectorParamsORBPtr params_det = std::make_shared<vision_utils::DetectorParamsORB>();
-
- params_det->nfeatures = 500; // The maximum number of features to retain.
- params_det->scaleFactor = 2; // Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.
- params_det->nlevels = 8; // The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).
- params_det->edgeThreshold = 16; // This is size of the border where the features are not detected. It should roughly match the patchSize parameter.
- params_det->firstLevel = 0; // It should be 0 in the current implementation.
- params_det->WTA_K = 2; // The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).
- params_det->scoreType = cv::ORB::HARRIS_SCORE; //#enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- params_det->patchSize = 31;
-
- vision_utils::DetectorBasePtr det_b_ptr = vision_utils::setupDetector("ORB", "ORB detector", params_det);
- vision_utils::DetectorORBPtr det_ptr = std::static_pointer_cast<vision_utils::DetectorORB>(det_b_ptr);
-
- // Descriptor
- vision_utils::DescriptorParamsORBPtr params_des = std::make_shared<vision_utils::DescriptorParamsORB>();
-
- params_des->nfeatures = 500; // The maximum number of features to retain.
- params_des->scaleFactor = 2; // Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.
- params_des->nlevels = 8; // The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).
- params_des->edgeThreshold = 16; // This is size of the border where the features are not detected. It should roughly match the patchSize parameter.
- params_des->firstLevel = 0; // It should be 0 in the current implementation.
- params_des->WTA_K = 2; // The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).
- params_des->scoreType = cv::ORB::HARRIS_SCORE; //#enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- params_des->patchSize = 31;
-
- vision_utils::DescriptorBasePtr des_b_ptr = vision_utils::setupDescriptor("ORB", "ORB descriptor", params_des);
- vision_utils::DescriptorORBPtr des_ptr = std::static_pointer_cast<vision_utils::DescriptorORB>(des_b_ptr);
-
- // Matcher
- vision_utils::MatcherParamsBRUTEFORCE_HAMMING_2Ptr params_mat = std::make_shared<vision_utils::MatcherParamsBRUTEFORCE_HAMMING_2>();
- vision_utils::MatcherBasePtr mat_b_ptr = vision_utils::setupMatcher("BRUTEFORCE_HAMMING_2", "BRUTEFORCE_HAMMING_2 matcher", params_mat);
- vision_utils::MatcherBRUTEFORCE_HAMMING_2Ptr mat_ptr = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_b_ptr);
-
- //=====================================================
-
- unsigned int buffer_size = 8;
- vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
-
- unsigned int img_width = frame_buff.back()->getImage().cols;
- unsigned int img_height = frame_buff.back()->getImage().rows;
- std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
-
- cv::namedWindow("Feature tracker"); // Creates a window for display.
- cv::moveWindow("Feature tracker", 0, 0);
- cv::startWindowThread();
- cv::imshow("Feature tracker", frame_buff.back()->getImage());
- cv::waitKey(1);
-
- KeyPointVector target_keypoints;
- KeyPointVector tracked_keypoints_;
- KeyPointVector tracked_keypoints_2;
- KeyPointVector current_keypoints;
- cv::Mat target_descriptors;
- cv::Mat tracked_descriptors;
- cv::Mat tracked_descriptors2;
- cv::Mat current_descriptors;
- cv::Mat image_original = frame_buff.back()->getImage();
- cv::Mat image_graphics;
-
- unsigned int roi_width = 200;
- unsigned int roi_heigth = 200;
-
- int n_first_1 = 0;
- int n_second_1 = 0;
-
- // Initial detection
- target_keypoints = det_ptr->detect(image_original);
- target_descriptors = des_ptr->getDescriptor(image_original, target_keypoints);
-
- for (unsigned int f_num=0; f_num < 1000; ++f_num)
- {
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), f_num) );
-
- KeyPointVector keypoints;
- cv::Mat descriptors;
- DMatchVector cv_matches;
- cv::Mat image = frame_buff.back()->getImage();
- image_graphics = image.clone();
- bool matched = false;
- n_first_1 = n_second_1 = 0;
-
- unsigned int tracked_keypoints = 0;
-
- for(unsigned int target_idx = 0; target_idx < target_keypoints.size(); target_idx++)
- {
- std::cout << "\npixel: " << target_keypoints[target_idx].pt << std::endl;
- std::cout << "target_descriptor[" << target_idx << "]:\n" << target_descriptors.row(target_idx) << std::endl;
-
- matched = false;
-
- cv::Rect roi = vision_utils::setRoi(target_keypoints[target_idx].pt.x, target_keypoints[target_idx].pt.y, roi_width, roi_heigth);
-
- cv::Point2f roi_up_left_corner;
- roi_up_left_corner.x = roi.x;
- roi_up_left_corner.y = roi.y;
-
- for(unsigned int fr = 0; fr < 2; fr++)
- {
- keypoints = det_ptr->detect(image, roi);
- descriptors = des_ptr->getDescriptor(image, keypoints);
-
- cv::Mat target_descriptor; //B(cv::Rect(0,0,vec_length,1));
- target_descriptor = target_descriptors(cv::Rect(0,target_idx,target_descriptors.cols,1));
-
- if(keypoints.size() != 0)
- {
- mat_ptr->match(target_descriptor, descriptors, des_ptr->getSize(), cv_matches);
- Scalar normalized_score = 1 - (Scalar)(cv_matches[0].distance)/(des_ptr->getSize()*8);
- std::cout << "pixel: " << keypoints[cv_matches[0].trainIdx].pt + roi_up_left_corner << std::endl;
- std::cout << "normalized score: " << normalized_score << std::endl;
- if(normalized_score < 0.8)
- {
- std::cout << "not tracked" << std::endl;
- }
- else
- {
- std::cout << "tracked" << std::endl;
-
- matched = true;
-
- cv::Point2f point,t_point;
- point.x = keypoints[cv_matches[0].trainIdx].pt.x;
- point.y = keypoints[cv_matches[0].trainIdx].pt.y;
- t_point.x = target_keypoints[target_idx].pt.x;
- t_point.y = target_keypoints[target_idx].pt.y;
-
- cv::circle(image_graphics, t_point, 4, cv::Scalar(51.0, 51.0, 255.0), -1, 3, 0);
- cv::circle(image_graphics, point, 2, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0);
- cv::putText(image_graphics, std::to_string(target_idx), point, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 255.0, 0.0));
-
- //introduce in list - tracked point
- cv::KeyPoint tracked_kp = keypoints[cv_matches[0].trainIdx];
- tracked_kp.pt.x = tracked_kp.pt.x + roi.x;
- tracked_kp.pt.y = tracked_kp.pt.y + roi.y;
- if(fr==0)
- tracked_keypoints_.push_back(tracked_kp);
- else
- tracked_keypoints_2.push_back(tracked_kp);
-
- cv::Mat tracked_desc;
- tracked_desc = descriptors(cv::Rect(0,cv_matches[0].trainIdx,target_descriptors.cols,1));
- if(fr==0)
- tracked_descriptors.push_back(tracked_desc);
- else
- tracked_descriptors2.push_back(tracked_desc);
-
- //introduce in list - target point
- if(fr==0)
- {
- current_keypoints.push_back(target_keypoints[target_idx]);
- current_descriptors.push_back(target_descriptor);
- }
-
- if (fr == 0 && normalized_score == 1)n_first_1++;
- if (fr == 1 && normalized_score == 1)n_second_1++;
- }
- }
- else
- std::cout << "not tracked" << std::endl;
-
- }
- if (matched) tracked_keypoints++;
- }
-
- std::cout << "\ntracked keypoints: " << tracked_keypoints << "/" << target_keypoints.size() << std::endl;
- std::cout << "percentage first: " << ((float)((float)tracked_keypoints/(float)target_keypoints.size()))*100 << "%" << std::endl;
- std::cout << "Number of perfect first matches: " << n_first_1 << std::endl;
- std::cout << "Number of perfect second matches: " << n_second_1 << std::endl;
-
- if(tracked_keypoints == 0)
- {
- target_keypoints = det_ptr->detect(image);
- target_descriptors = des_ptr->getDescriptor(image, target_keypoints);
- std::cout << "number of new keypoints to be tracked: " << target_keypoints.size() << std::endl;
- }
- else
- {
- std::cout << "\n\nADVANCE" << std::endl;
-// for(unsigned int i = 0; i < target_keypoints.size(); i++)
-// {
-// std::cout << "\ntarget keypoints";
-// std::cout << target_keypoints[i].pt;
-// }
-// for(unsigned int i = 0; i < current_keypoints.size(); i++)
-// {
-// std::cout << "\ncurrent keypoints";
-// std::cout << current_keypoints[i].pt;
-// }
-// for( int i = 0; i < target_descriptors.rows; i++)
-// {
-// std::cout << "\ntarget descriptors";
-// std::cout << target_descriptors.row(i);
-// }
-// for( int i = 0; i < current_descriptors.rows; i++)
-// {
-// std::cout << "\ncurrent descriptors";
-// std::cout << current_descriptors.row(i);
-// }
-// for( int i = 0; i < current_descriptors2.rows; i++)
-// {
-// std::cout << "\ncurrent descriptors2";
-// std::cout << current_descriptors2.row(i);
-// }
-
- //target_keypoints.clear();
- target_keypoints = current_keypoints;
- current_descriptors.copyTo(target_descriptors);
- current_keypoints.clear();
- current_descriptors.release();
-
- std::cout << "\nAFTER THE ADVANCE" << std::endl;
-// for(unsigned int i = 0; i < target_keypoints.size(); i++)
-// {
-// std::cout << "target keypoints";
-// std::cout << target_keypoints[i].pt << "\t" ;
-// }
-// for( int i = 0; i < target_descriptors.rows; i++)
-// {
-// std::cout << "\ntarget descriptors";
-// std::cout << target_descriptors.row(i);
-// }
-
- std::cout << "\nEND OF ADVANCE\n";
-
- }
-
- tracked_keypoints = 0;
- cv::imshow("Feature tracker", image_graphics);
- cv::waitKey(1);
- }
-}
diff --git a/demos/demo_virtual_hierarchy.cpp b/demos/demo_virtual_hierarchy.cpp
deleted file mode 100644
index fb1b248ce8bae0371579391351daec340fa2c8c5..0000000000000000000000000000000000000000
--- a/demos/demo_virtual_hierarchy.cpp
+++ /dev/null
@@ -1,354 +0,0 @@
-/*
- * test_virtual_hierarchy.cpp
- *
- * Created on: Sep 8, 2014
- * Author: jsola
- */
-
-#include <list>
-
-namespace wolf{
-
-// BASE CLASSES
-
-/**
- * \brief Node class.
- *
- * The Node class has only an ID and an ID factory built in the constructor.
- */
-class N
-{
- private:
- unsigned int id_;
- static unsigned int id_count_;
- protected:
- N() : id_(++id_count_) { }
- virtual ~N() { }
- public:
- unsigned int id() { return id_; }
-};
-
-/**
- * \brief Base class for children.
- *
- * It has a pointer to parent.
- *
- * NOTE: The virtual inheritance solves the "diamond of death" problem.
- */
-template<class Parent>
-class ChildOf : virtual public N
-{
- private:
- Parent* up_ptr_;
- protected:
- ChildOf(Parent* _up_ptr) : up_ptr_(_up_ptr) { }
- virtual ~ChildOf() { }
- Parent* up() { return up_ptr_; }
-};
-
-/**
- * \brief Base class for parents
- *
- * It has a list of pointers to children, and a dumy method 'print' that is recursive to all children.
- *
- * NOTE: The virtual inheritance solves the "diamond of death" problem.
- */
-template<class Child>
-class ParentOf : virtual public N
-{
- private:
- std::list<Child*> down_list_;
- protected:
- ParentOf() { }
- virtual ~ParentOf() { }
- public:
- void addToList(Child* _down_ptr) { down_list_.push_back(_down_ptr); }
- std::list<Child*> downList() { return down_list_; }
- virtual void print();
-};
-
-///*
-// * Virtual inheritance solves the "diamond of death" problem.
-// */
-//template<class Sibling>
-//class SiblingOf : virtual public N
-//{
-// private:
-// std::list<Sibling*> side_list_;
-// protected:
-// SiblingOf() { }
-// virtual ~SiblingOf() { }
-// public:
-// void addToList(Sibling* _side_ptr) { side_list_.push_back(_side_ptr); }
-// std::list<Sibling*> sideList() { return side_list_; }
-//};
-
-/**
- * \brief Base class for bottom nodes.
- *
- * This class is for children that are no parents - they are bottom nodes
- *
- * It overloads the dummy 'print' function so that this is is not recursive any more.
- *
- * NOTE: The virtual inheritance solves the "diamond of death" problem.
- */
-class Bot : virtual public N
-{
- protected:
- virtual ~Bot() { }
- public:
- virtual void print();
-};
-
-//template<class Child>
-//class ExplorerOf : public ParentOf<Child>
-//{
-// protected:
-// ExplorerOf() { }
-// virtual ~ExplorerOf() { }
-// public:
-// virtual void explore();
-//};
-
-//class Explored : public Bot
-//{
-// protected:
-// virtual ~Explored() { }
-// public:
-// virtual void explore() { }
-//};
-
-// DERIVED ISOLATED CLASSES
-
-// a bunch of fwd_decs
-class VehNode;
-class FrmNode;
-class CapNode;
-class FeaNode;
-class CorNode;
-class TrSNode;
-class SenNode;
-
-class Veh
-{
- public:
- Veh() : v_(1){}
- void duplicate(){v_ *= 2;}
- double v(){return v_;}
- private:
- double v_;
-};
-class Frm
-{
- public:
- Frm() : f_(1){}
- void duplicate(){f_ *= 2;}
- double f(){return f_;}
- private:
- double f_;
-};
-class Cap
-{
- public:
- Cap() : c_(1){}
- void duplicate(){c_ *= 2;}
- double c(){return c_;}
- private:
- double c_;
-};
-class Fea
-{
- public:
- Fea() : ft_(1){}
- void duplicate(){ft_ *= 2;}
- double ft(){return ft_;}
- private:
- double ft_;
-};
-class Cor
-{
-public:
- Cor() : co_(1){}
- void duplicate(){co_ *= 2;}
- double co(){return co_;}
-private:
- double co_;
-};
-class Sen
-{
- public:
- Sen() : s_(1){}
- void duplicate(){s_ *= 2;}
- double s(){return s_;}
- private:
- double s_;
-};
-
-// Derived classes for all levels of the tree
-
-class VehNode : public Veh, public ParentOf<FrmNode>, public ParentOf<SenNode>
-{
- public:
- VehNode() { }
- virtual ~VehNode() { }
- void print(); // Overload because I am Top and have both Down and Explorer children.
-};
-class FrmNode : public Frm, public ChildOf<VehNode>, public ParentOf<CapNode>
-{
- public:
- FrmNode(VehNode* _veh_ptr) : ChildOf<VehNode>(_veh_ptr) { }
- virtual ~FrmNode() { }
-};
-class CapNode : public Cap, public ChildOf<FrmNode>, public ParentOf<FeaNode>//, public SiblingOf<TrSNode>
-{
- public:
- CapNode(FrmNode* _frm_ptr) : ChildOf<FrmNode>(_frm_ptr) { }
- virtual ~CapNode() { }
- void explore(); // Overload because I have both Explorer and Side children
-};
-class FeaNode : public Fea, public ChildOf<CapNode>, public ParentOf<CorNode>
-{
- public:
- FeaNode(CapNode* _cap_ptr) : ChildOf<CapNode>(_cap_ptr) { }
- virtual ~FeaNode() { }
-};
-class CorNode : public Cor, public ChildOf<FeaNode>, public Bot//, public Explored
-{
- public:
- CorNode(FeaNode* _fea_ptr) : ChildOf<FeaNode>(_fea_ptr) { }
- virtual ~CorNode() { }
-};
-//class TrSNode : public virtual N
-//{
-// public:
-// TrSNode() { }
-// virtual ~TrSNode() { }
-//};
-class SenNode : public Sen, public ChildOf<VehNode>, public Bot
-{
- public:
- SenNode(VehNode* _veh_ptr) : ChildOf<VehNode>(_veh_ptr) { }
- virtual ~SenNode() { }
-};
-
-} // namespace wolf
-
-/////////////////////
-// IMPLEMENTATIONS, here to avoid incomplete types and unwanted #includes
-/////////////////////
-
-#include <iostream>
-
-namespace wolf {
-using namespace std;
-
-template<class Child>
-void ParentOf<Child>::print()
-{
- cout << this->id() << ":( ";
- for (auto const & it_ptr : this->downList())
- cout << it_ptr->id() << " ";
- cout << ")" << endl;
- for (auto const & it_ptr : this->downList())
- it_ptr->print();
-}
-
-//template<class Child>
-//void ExplorerOf<Child>::explore()
-//{
-// cout << this->id() << ":( "; // Yes I look sad but I'm OK.
-// for (auto const & it_ptr : ParentOf<Child>::downList())
-// cout << it_ptr->id() << " ";
-// cout << ")" << endl;
-// for (auto const & it_ptr : ParentOf<Child>::downList())
-// it_ptr->explore();
-//}
-
-void Bot::print(){
- cout << this->id() << ":( Bottom )" << endl;
-}
-
-void VehNode::print()
-{
- cout << "Vehicle Own Field: v_ = " << v() << endl;
- cout << "Vehicle Hardware:" << endl;
- ParentOf < SenNode > ::print();
- cout << "Vehicle Data:" << endl;
- ParentOf < FrmNode > ::print();
-}
-
-//void CapNode::explore()
-//{
-// cout << this->id() << ":( "; // Yes I look sad but I'm OK.
-// for (auto const & it_ptr : ExplorerOf<FeaNode>::downList())
-// cout << it_ptr->id() << " ";
-// cout << "/ ";
-// for (auto const & it_ptr : SiblingOf<TrSNode>::sideList())
-// cout << it_ptr->id() << " ";
-// cout << ")" << endl;
-// for (auto const & it_ptr : ExplorerOf<FeaNode>::downList())
-// it_ptr->explore();
-//}
-
-///////////////////////
-// START APPLICATION
-///////////////////////
-
-unsigned int N::id_count_ = 0;
-
-} // namespace wolf
-
-int main()
-{
- using namespace wolf;
-
- // Create all nodes with up-pointers already set up
- VehNode V;
- SenNode S0(&V), S1(&V);
- FrmNode F0(&V), F1(&V);
- CapNode C00(&F0), C01(&F0), C10(&F1), C11(&F1);
- FeaNode f000(&C00), f001(&C00), f010(&C01), f011(&C01), f100(&C10), f101(&C10), f110(&C11), f111(&C11);
-// TrSNode T0001, T0010, T0011, T0110, T0111, T1011;
-
- // Add sensors to vehicle
- V.ParentOf < SenNode > ::addToList(&S0);
- V.ParentOf < SenNode > ::addToList(&S1);
-
- // Add frames to vehicle
- V.ParentOf < FrmNode > ::addToList(&F0);
- V.ParentOf < FrmNode > ::addToList(&F1);
-
- // Add captures to frames
- F0.ParentOf<CapNode>::addToList(&C00);
- F0.ParentOf<CapNode>::addToList(&C01);
- F1.ParentOf<CapNode>::addToList(&C10);
- F1.ParentOf<CapNode>::addToList(&C11);
-
- // Add features to captures
- C00.ParentOf<FeaNode>::addToList(&f000);
- C00.ParentOf<FeaNode>::addToList(&f001);
- C01.ParentOf<FeaNode>::addToList(&f010);
- C01.ParentOf<FeaNode>::addToList(&f011);
- C10.ParentOf<FeaNode>::addToList(&f100);
- C10.ParentOf<FeaNode>::addToList(&f101);
- C11.ParentOf<FeaNode>::addToList(&f110);
- C11.ParentOf<FeaNode>::addToList(&f111);
-
-// // Add trans-sensors to captures
-// C00.SiblingOf<TrSNode>::addToList(&T0001);
-// C00.SiblingOf<TrSNode>::addToList(&T0010);
-// C00.SiblingOf<TrSNode>::addToList(&T0011);
-// C01.SiblingOf<TrSNode>::addToList(&T0110);
-// C01.SiblingOf<TrSNode>::addToList(&T0111);
-// C10.SiblingOf<TrSNode>::addToList(&T1011);
-
- // explore() : means we are calling advanced functionality from Explorer classes. Here, we just fake.
- // print() : means we just print linkage info.
- cout << "V.explore():" << endl;
-// V.explore();
- V.duplicate();
- cout << "V.print():" << endl;
- V.print();
-
- return 0;
-}
diff --git a/demos/demo_wolf_autodiffwrapper.cpp b/demos/demo_wolf_autodiffwrapper.cpp
deleted file mode 100644
index 8d0897956275c596d47782117311784b8ea2828e..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_autodiffwrapper.cpp
+++ /dev/null
@@ -1,316 +0,0 @@
-// Testing creating wolf tree from imported .graph file
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "wolf_manager.h"
-#include "core/capture/capture_void.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- //Welcome message
- std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
-
- if (argc != 3 || atoi(argv[2]) < 0 )
- {
- std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
- std::cout << " FILE_PATH is the .graph file path" << std::endl;
- std::cout << " MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- // auxiliar variables
- std::string file_path_ = argv[1];
- unsigned int MAX_VERTEX = atoi(argv[2]);
- if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
- std::ifstream offLineFile_;
- clock_t t1;
- ceres::Solver::Summary summary_ceres_diff, summary_wolf_diff;
-
- // loading variables
- std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_ceres_diff;
- std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_wolf_diff;
- std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_wolf_diff;
-
- // Wolf problem
- ProblemPtr wolf_problem_ceres_diff = new Problem(FRM_PO_2D);
- ProblemPtr wolf_problem_wolf_diff = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", 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);
-
- // Ceres wrappers
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_ceres_diff = new CeresManager(wolf_problem_ceres_diff, ceres_options, false);
- CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_wolf_diff, ceres_options, true);
-
- // load graph from .txt
- offLineFile_.open(file_path_.c_str(), std::ifstream::in);
- if (offLineFile_.is_open())
- {
- std::string buffer;
- unsigned int j = 0;
- // Line by line
- while (std::getline(offLineFile_, buffer))
- {
- //std::cout << "new line:" << buffer << std::endl;
- std::string bNum;
- unsigned int i = 0;
-
- // VERTEX
- if (buffer.at(0) == 'V')
- {
- //skip rest of VERTEX word
- while (buffer.at(i) != ' ') i++;
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- //vertex index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- unsigned int vertex_index = atoi(bNum.c_str());
- bNum.clear();
-
- if (vertex_index <= MAX_VERTEX+1)
- {
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // vertex pose
- Eigen::Vector3s vertex_pose;
- // x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- vertex_pose(0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- vertex_pose(1) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // theta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- vertex_pose(2) = atof(bNum.c_str());
- bNum.clear();
-
- // add frame to problem
- FrameBasePtr vertex_frame_ptr_ceres_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_wolf_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_ceres_diff->getTrajectory()->addFrame(vertex_frame_ptr_ceres_diff);
- wolf_problem_wolf_diff->getTrajectory()->addFrame(vertex_frame_ptr_wolf_diff);
- // store
- index_2_frame_ptr_ceres_diff[vertex_index] = vertex_frame_ptr_ceres_diff;
- index_2_frame_ptr_wolf_diff[vertex_index] = vertex_frame_ptr_wolf_diff;
- frame_ptr_2_index_wolf_diff[vertex_frame_ptr_wolf_diff] = vertex_index;
-
- //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_wolf_diff->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
- }
- }
- // EDGE
- else if (buffer.at(0) == 'E')
- {
- j++;
- //skip rest of EDGE word
- while (buffer.at(i) != ' ') i++;
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- //from
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- unsigned int edge_old = atoi(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- //to index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- unsigned int edge_new = atoi(bNum.c_str());
- bNum.clear();
-
- if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
- {
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // edge vector
- Eigen::Vector3s edge_vector;
- // x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_vector(0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_vector(1) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // theta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_vector(2) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // edge covariance
- Eigen::Matrix3s edge_information;
- // xx
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // xy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,1) = atof(bNum.c_str());
- edge_information(1,0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // yy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,1) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // thetatheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(2,2) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // xtheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,2) = atof(bNum.c_str());
- edge_information(2,0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // ytheta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,2) = atof(bNum.c_str());
- edge_information(2,1) = atof(bNum.c_str());
- bNum.clear();
-
- // add capture, feature and factor to problem
- FeatureBasePtr feature_ptr_ceres_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
- FeatureBasePtr feature_ptr_wolf_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
- CaptureVoid* capture_ptr_ceres_diff = new CaptureVoid(TimeStamp(0), sensor);
- CaptureVoid* capture_ptr_wolf_diff = new CaptureVoid(TimeStamp(0), sensor);
- assert(index_2_frame_ptr_ceres_diff.find(edge_old) != index_2_frame_ptr_ceres_diff.end() && "edge from vertex not added!");
- assert(index_2_frame_ptr_wolf_diff.find(edge_old) != index_2_frame_ptr_wolf_diff.end() && "edge from vertex not added!");
- FrameBasePtr frame_old_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_old];
- FrameBasePtr frame_old_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_old];
- assert(index_2_frame_ptr_ceres_diff.find(edge_new) != index_2_frame_ptr_ceres_diff.end() && "edge to vertex not added!");
- assert(index_2_frame_ptr_wolf_diff.find(edge_new) != index_2_frame_ptr_wolf_diff.end() && "edge to vertex not added!");
- FrameBasePtr frame_new_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_new];
- FrameBasePtr frame_new_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_new];
- frame_new_ptr_ceres_diff->addCapture(capture_ptr_ceres_diff);
- frame_new_ptr_wolf_diff->addCapture(capture_ptr_wolf_diff);
- capture_ptr_ceres_diff->addFeature(feature_ptr_ceres_diff);
- capture_ptr_wolf_diff->addFeature(feature_ptr_wolf_diff);
- FactorOdom2D* factor_ptr_ceres_diff = new FactorOdom2D(feature_ptr_ceres_diff, frame_old_ptr_ceres_diff);
- FactorOdom2D* factor_ptr_wolf_diff = new FactorOdom2D(feature_ptr_wolf_diff, frame_old_ptr_wolf_diff);
- feature_ptr_ceres_diff->addFactor(factor_ptr_ceres_diff);
- feature_ptr_wolf_diff->addFactor(factor_ptr_wolf_diff);
- //std::cout << "Added edge! " << factor_ptr_wolf_diff->id() << " from vertex " << factor_ptr_wolf_diff->getCapture()->getFrame()->id() << " to " << factor_ptr_wolf_diff->getFrameTo()->id() << std::endl;
- //std::cout << "vector " << factor_ptr_wolf_diff->getMeasurement().transpose() << std::endl;
- //std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << factor_ptr_wolf_diff->getMeasurementCovariance() << std::endl;
- }
- }
- else
- assert("unknown line");
- }
- printf("\nGraph loaded!\n");
- }
- else
- printf("\nError opening file\n");
-
- // PRIOR
- FrameBasePtr first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectory()->getFrameList().front();
- CaptureFix* initial_covariance_ceres_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_ceres_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
- CaptureFix* initial_covariance_wolf_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_wolf_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
- first_frame_ceres_diff->addCapture(initial_covariance_ceres_diff);
- first_frame_wolf_diff->addCapture(initial_covariance_wolf_diff);
- initial_covariance_ceres_diff->process();
- initial_covariance_wolf_diff->process();
- //std::cout << "initial covariance: factor " << initial_covariance_wolf_diff->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_wolf_diff->getFeatureList().front()->getMeasurementCovariance() << std::endl;
-
- // COMPUTE COVARIANCES
- std::cout << "computing covariances..." << std::endl;
- t1 = clock();
- ceres_manager_ceres_diff->computeCovariances(ALL);//ALL_MARGINALS
- double t_sigma_ceres = ((double) clock() - t1) / CLOCKS_PER_SEC;
- t1 = clock();
- ceres_manager_wolf_diff->computeCovariances(ALL);//ALL_MARGINALS
- double t_sigma_wolf = ((double) clock() - t1) / CLOCKS_PER_SEC;
- std::cout << "computed!" << std::endl;
-
- // SOLVING PROBLEMS
- std::cout << "solving..." << std::endl;
- Eigen::VectorXs prev_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
- summary_ceres_diff = ceres_manager_ceres_diff->solve();
- Eigen::VectorXs post_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
- //std::cout << summary_ceres_diff.BriefReport() << std::endl;
- Eigen::VectorXs prev_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
- summary_wolf_diff = ceres_manager_wolf_diff->solve();
- Eigen::VectorXs post_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
- //std::cout << summary_wolf_diff.BriefReport() << std::endl;
- std::cout << "solved!" << std::endl;
-
- std::cout << "CERES AUTODIFF:" << std::endl;
- std::cout << "Covariance computation: " << t_sigma_ceres << "s" << std::endl;
- std::cout << "Solving: " << summary_ceres_diff.total_time_in_seconds << "s" << std::endl;
- std::cout << "Prev: " << prev_ceres_state.transpose() << std::endl;
- std::cout << "Post: " << post_ceres_state.transpose() << std::endl;
-
- std::cout << std::endl << "WOLF AUTODIFF:" << std::endl;
- std::cout << "Covariance computation: " << t_sigma_wolf << "s" << std::endl;
- std::cout << "Solving: " << summary_wolf_diff.total_time_in_seconds << "s" << std::endl;
- std::cout << "Prev: " << prev_wolf_state.transpose() << std::endl;
- std::cout << "Post: " << post_wolf_state.transpose() << std::endl;
-
- delete wolf_problem_ceres_diff; //not necessary to delete anything more, wolf will do it!
- delete wolf_problem_wolf_diff; //not necessary to delete anything more, wolf will do it!
- std::cout << "wolf_problem_ deleted!" << std::endl;
- delete ceres_manager_ceres_diff;
- delete ceres_manager_wolf_diff;
- std::cout << "ceres_manager deleted!" << std::endl;
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_wolf_factories.cpp b/demos/demo_wolf_factories.cpp
deleted file mode 100644
index ce7db15c8ee5a986d8d4ed469efd9ee57dbf3351..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_factories.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-/**
- * \file test_wolf_factories.cpp
- *
- * Created on: Apr 25, 2016
- * \author: jsola
- */
-
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_GPS_fix.h"
-#include "core/hardware/hardware_base.h"
-#include "core/sensor/sensor_camera.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "../sensor_imu.h"
-//#include "../sensor_gps.h"
-
-#include "core/processor/processor_odom_2D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "../processor_image_feature.h"
-
-#include "core/problem/problem.h"
-
-#include "core/common/factory.h"
-
-#include <iostream>
-#include <iomanip>
-#include <cstdlib>
-
-int main(void)
-{
- using namespace wolf;
- using namespace std;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- //==============================================================================================
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string wolf_config = wolf_root + "/src/examples";
- std::cout << "\nwolf directory for configuration files: " << wolf_config << std::endl;
- //==============================================================================================
-
- cout << "\n====== Registering creators in the Wolf Factories =======" << endl;
-
- cout << "If you look above, you see the registered creators.\n"
- "There is only one attempt per class, and it is successful!\n"
- "We do this by registering in the class\'s .cpp file.\n"
- "\n"
- "- See \'" << wolf_root << "/src/examples/test_wolf_factories.cpp\'\n"
- " for the way to install sensors and processors to wolf::Problem." << endl;
-
- // Start creating the problem
-
- ProblemPtr problem = Problem::create(FRM_PO_3D);
-
- // define some useful parameters
- Eigen::VectorXs pq_3d(7), po_2d(3), p_3d(3);
- shared_ptr<IntrinsicsOdom2D> intr_odom2d_ptr = nullptr;
-
- cout << "\n================== Intrinsics Factory ===================" << endl;
-
- // Use params factory for camera intrinsics
- IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", wolf_config + "/camera_params_ueye_sim.yaml");
- ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE FEATURE", wolf_config + "/processor_image_feature.yaml");
-
- cout << "CAMERA with intrinsics : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model_raw.transpose() << endl;
-// cout << "Processor IMAGE image width : " << (static_pointer_cast<ProcessorParamsImage>(params_ptr))->image.width << endl;
-
- cout << "\n==================== Install Sensors ====================" << endl;
-
- // Install sensors
- problem->installSensor("CAMERA", "front left camera", pq_3d, intr_cam_ptr);
- problem->installSensor("CAMERA", "front right camera", pq_3d, wolf_config + "/camera_params_ueye_sim.yaml");
- problem->installSensor("ODOM 2D", "main odometer", po_2d, intr_odom2d_ptr);
- problem->installSensor("GPS FIX", "GPS fix", p_3d);
- problem->installSensor("IMU", "inertial", pq_3d);
-// problem->installSensor("GPS", "GPS raw", p_3d);
- problem->installSensor("ODOM 2D", "aux odometer", po_2d, intr_odom2d_ptr);
- problem->installSensor("CAMERA", "rear camera", pq_3d, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
-
- // print available sensors
- for (auto sen : problem->getHardware()->getSensorList())
- {
- cout << "Sensor " << setw(2) << left << sen->id()
- << " | type " << setw(8) << sen->getType()
- << " | name: " << sen->getName() << endl;
- }
-
- cout << "\n=================== Install Processors ===================" << endl;
-
- // Install processors and bind them to sensors -- by sensor name!
- problem->installProcessor("ODOM 2D", "main odometry", "main odometer");
- problem->installProcessor("ODOM 2D", "sec. odometry", "aux odometer");
- problem->installProcessor("IMU", "pre-integrated", "inertial");
- problem->installProcessor("IMAGE FEATURE", "ORB", "front left camera", wolf_config + "/processor_image_feature.yaml");
-// problem->createProcessor("GPS", "GPS pseudoranges", "GPS raw");
-
- // print installed processors
- for (auto sen : problem->getHardware()->getSensorList())
- for (auto prc : sen->getProcessorList())
- cout << "Processor " << setw(2) << left << prc->id()
- << " | type : " << setw(8) << prc->getType()
- << " | name: " << setw(17) << prc->getName()
- << " | bound to sensor " << setw(2) << prc->getSensor()->id() << ": " << prc->getSensor()->getName() << endl;
-
- return 0;
-}
-
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index b87d3c17ec366f3d868aafb4bf4397b17ba92f45..386d803138f33b89bee401aceda047fe2199f104 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -10,20 +10,20 @@
#include <cmath>
#include <queue>
+#include "../include/core/ceres_wrapper/solver_ceres.h"
//Wolf includes
#include "wolf_manager.h"
#include "core/capture/capture_void.h"
-#include "core/ceres_wrapper/ceres_manager.h"
// EIGEN
//#include <Eigen/CholmodSupport>
namespace wolf{
// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
{
for (int k=0; k<ins.outerSize(); ++k)
- for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
+ for (Eigen::SparseMatrix<double>::InnerIterator iti(ins,k); iti; ++iti)
original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
original.makeCompressed();
@@ -52,8 +52,8 @@ int main(int argc, char** argv)
std::ifstream offLineFile_;
clock_t t1;
ceres::Solver::Summary summary_full, summary_prun;
- Eigen::MatrixXs Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
- Scalar xi, yi, thi, si, ci, xj, yj;
+ Eigen::MatrixXd Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
+ double xi, yi, thi, si, ci, xj, yj;
// loading variables
std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_full;
@@ -61,19 +61,19 @@ int main(int argc, char** argv)
std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
// Wolf problem
- ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
- ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
+ ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
- Eigen::SparseMatrix<Scalar> Lambda(0,0);
+ Eigen::SparseMatrix<double> Lambda(0,0);
// Ceres wrapper
ceres::Solver::Options ceres_options;
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options);
- CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun, ceres_options);
+ SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options);
+ SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options);
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
@@ -108,7 +108,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// vertex pose
- Eigen::Vector3s vertex_pose;
+ Eigen::Vector3d vertex_pose;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -130,8 +130,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
@@ -173,7 +173,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge vector
- Eigen::Vector3s edge_vector;
+ Eigen::Vector3d edge_vector;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -197,7 +197,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge covariance
- Eigen::Matrix3s edge_information;
+ Eigen::Matrix3d edge_information;
// xx
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -259,8 +259,8 @@ int main(int argc, char** argv)
frame_new_ptr_prun->addCapture(capture_ptr_prun);
capture_ptr_full->addFeature(feature_ptr_full);
capture_ptr_prun->addFeature(feature_ptr_prun);
- FactorOdom2D* factor_ptr_full = new FactorOdom2D(feature_ptr_full, frame_old_ptr_full);
- FactorOdom2D* factor_ptr_prun = new FactorOdom2D(feature_ptr_prun, frame_old_ptr_prun);
+ FactorOdom2d* factor_ptr_full = new FactorOdom2d(feature_ptr_full, frame_old_ptr_full);
+ FactorOdom2d* factor_ptr_prun = new FactorOdom2d(feature_ptr_prun, frame_old_ptr_prun);
feature_ptr_full->addFactor(factor_ptr_full);
feature_ptr_prun->addFactor(factor_ptr_prun);
//std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() << std::endl;
@@ -268,14 +268,14 @@ int main(int argc, char** argv)
//std::cout << "information " << std::endl << edge_information << std::endl;
//std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
- Scalar xi = *(frame_old_ptr_prun->getP()->get());
- Scalar yi = *(frame_old_ptr_prun->getP()->get()+1);
- Scalar thi = *(frame_old_ptr_prun->getO()->get());
- Scalar si = sin(thi);
- Scalar ci = cos(thi);
- Scalar xj = *(frame_new_ptr_prun->getP()->get());
- Scalar yj = *(frame_new_ptr_prun->getP()->get()+1);
- Eigen::MatrixXs Ji(3,3), Jj(3,3);
+ double xi = *(frame_old_ptr_prun->getP()->get());
+ double yi = *(frame_old_ptr_prun->getP()->get()+1);
+ double thi = *(frame_old_ptr_prun->getO()->get());
+ double si = sin(thi);
+ double ci = cos(thi);
+ double xj = *(frame_new_ptr_prun->getP()->get());
+ double yj = *(frame_new_ptr_prun->getP()->get()+1);
+ Eigen::MatrixXd Ji(3,3), Jj(3,3);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
0, 0, -1;
@@ -284,7 +284,7 @@ int main(int argc, char** argv)
0, 0, 1;
//std::cout << "Ji" << std::endl << Ji << std::endl;
//std::cout << "Jj" << std::endl << Jj << std::endl;
- Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
+ Eigen::SparseMatrix<double> DeltaLambda(Lambda.rows(), Lambda.cols());
insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3);
insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3);
insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3);
@@ -301,17 +301,17 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
- CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
- CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front();
+ CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
+ CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
first_frame_prun->addCapture(initial_covariance_prun);
initial_covariance_full->process();
initial_covariance_prun->process();
//std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
- Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
- insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
+ Eigen::SparseMatrix<double> DeltaLambda(Lambda.rows(), Lambda.cols());
+ insertSparseBlock((Eigen::Matrix3d::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
Lambda = Lambda + DeltaLambda;
// COMPUTE COVARIANCES
@@ -319,8 +319,8 @@ int main(int argc, char** argv)
wolf_problem_prun->getTrajectory()->getFactorList(factors);
// Manual covariance computation
t1 = clock();
- Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A
- Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols()));
+ Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> chol(Lambda); // performs a Cholesky factorization of A
+ Eigen::MatrixXd Sigma = chol.solve(Eigen::MatrixXd::Identity(Lambda.rows(), Lambda.cols()));
double t_sigma_manual = ((double) clock() - t1) / CLOCKS_PER_SEC;
//std::cout << "Lambda" << std::endl << Lambda << std::endl;
//std::cout << "Sigma" << std::endl << Sigma << std::endl;
@@ -384,7 +384,7 @@ int main(int argc, char** argv)
//std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
// Information gain
- Scalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
+ double IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
//std::cout << "IG = " << IG << std::endl;
if (IG < 2)
diff --git a/demos/demo_wolf_logging.cpp b/demos/demo_wolf_logging.cpp
deleted file mode 100644
index ee3b9b5763037355fcf24ec813a79199067eb227..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_logging.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/**
- * \file test_wolf_logging.cpp
- *
- * Created on: Oct 28, 2016
- * \author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "core/utils/logging.h"
-
-int main(int, char*[])
-{
- WOLF_INFO("test info ", 5, " ", 0.123);
-
- WOLF_WARN("test warn ", 5, " ", 0.123);
-
- WOLF_ERROR("test error ", 5, " ", 0.123);
-
- WOLF_TRACE("test trace ", 5, " ", 0.123);
-
- WOLF_DEBUG("test debug ", 5, " ", 0.123);
-
- return 0;
-}
diff --git a/demos/demo_wolf_prunning.cpp b/demos/demo_wolf_prunning.cpp
deleted file mode 100644
index 99567b5c3fb991e7664b255fc3893df9f027e1c8..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_prunning.cpp
+++ /dev/null
@@ -1,566 +0,0 @@
-// Testing creating wolf tree from imported .graph file
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "wolf_manager.h"
-#include "core/capture/capture_void.h"
-#include "core/factor/factor_base.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// EIGEN
-//#include <Eigen/CholmodSupport>
-#include <Eigen/StdVector> // Eigen in std vector
-
-namespace wolf{
-// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-
-void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
-{
- for (int k=0; k<ins.outerSize(); ++k)
- for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
- original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
-
- original.makeCompressed();
-}
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- //Welcome message
- std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
-
- if (argc != 3 || atoi(argv[2]) < 0 )
- {
- std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
- std::cout << " FILE_PATH is the .graph file path" << std::endl;
- std::cout << " MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- // auxiliar variables
- std::string file_path_ = argv[1];
- unsigned int MAX_VERTEX = atoi(argv[2]);
- if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
- std::ifstream offLineFile_;
- clock_t t1;
- ceres::Solver::Summary summary_full, summary_prun;
- Eigen::MatrixXs Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
- Eigen::MatrixXs Sigma_11(2,2), Sigma_12(2,1), Sigma_13(2,2), Sigma_14(2,1),
- Sigma_22(1,1), Sigma_23(1,2), Sigma_24(1,1),
- Sigma_33(2,2), Sigma_34(2,1),
- Sigma_44(1,1);
-
- std::vector<Eigen::MatrixXs> jacobians;
- jacobians.push_back(Eigen::MatrixXs::Zero(3,2));
- jacobians.push_back(Eigen::MatrixXs::Zero(3,1));
- jacobians.push_back(Eigen::MatrixXs::Zero(3,2));
- jacobians.push_back(Eigen::MatrixXs::Zero(3,1));
- Scalar xi, yi, thi, si, ci, xj, yj;
- double t_sigma_manual = 0;
-
- // loading variables
- std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_full;
- std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_prun;
- std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
-
- // Wolf problem
- ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
- ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
- Eigen::SparseMatrix<Scalar> Lambda(0,0);
-
- // prunning
- FactorBasePtrList ordered_fac_ptr;
- std::list<Scalar> ordered_ig;
-
- // Ceres wrapper
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options);
- CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun,ceres_options);
-
- // load graph from .txt
- offLineFile_.open(file_path_.c_str(), std::ifstream::in);
- if (offLineFile_.is_open())
- {
- std::string buffer;
- unsigned int j = 0;
- // Line by line
- while (std::getline(offLineFile_, buffer))
- {
- //std::cout << "new line:" << buffer << std::endl;
- std::string bNum;
- unsigned int i = 0;
-
- // VERTEX
- if (buffer.at(0) == 'V')
- {
- //skip rest of VERTEX word
- while (buffer.at(i) != ' ') i++;
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- //vertex index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- unsigned int vertex_index = atoi(bNum.c_str());
- bNum.clear();
-
- if (vertex_index <= MAX_VERTEX+1)
- {
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // vertex pose
- Eigen::Vector3s vertex_pose;
- // x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- vertex_pose(0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- vertex_pose(1) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // theta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- vertex_pose(2) = atof(bNum.c_str());
- bNum.clear();
-
- // add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
- wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
- // store
- index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
- index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
- frame_ptr_2_index_prun[vertex_frame_ptr_prun] = vertex_index;
- // Information matrix
- Lambda.conservativeResize(Lambda.rows()+3,Lambda.cols()+3);
-
- //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_prun->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
- }
- }
- // EDGE
- else if (buffer.at(0) == 'E')
- {
- j++;
- //skip rest of EDGE word
- while (buffer.at(i) != ' ') i++;
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- //from
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- unsigned int edge_old = atoi(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- //to index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- unsigned int edge_new = atoi(bNum.c_str());
- bNum.clear();
-
- if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
- {
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // edge vector
- Eigen::Vector3s edge_vector;
- // x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_vector(0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_vector(1) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // theta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_vector(2) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
-
- // edge covariance
- Eigen::Matrix3s edge_information;
- // xx
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // xy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,1) = atof(bNum.c_str());
- edge_information(1,0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // yy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,1) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // thetatheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(2,2) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // xtheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,2) = atof(bNum.c_str());
- edge_information(2,0) = atof(bNum.c_str());
- bNum.clear();
- //skip white spaces
- while (buffer.at(i) == ' ') i++;
- // ytheta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,2) = atof(bNum.c_str());
- edge_information(2,1) = atof(bNum.c_str());
- bNum.clear();
-
- //std::cout << "Adding edge... " << std::endl;
- // add capture, feature and factor to problem
- FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse());
- FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse());
- CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
- CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
- assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!");
- assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() && "edge from vertex not added!");
- FrameBasePtr frame_old_ptr_full = index_2_frame_ptr_full[edge_old];
- FrameBasePtr frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old];
- assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() && "edge to vertex not added!");
- assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() && "edge to vertex not added!");
- FrameBasePtr frame_new_ptr_full = index_2_frame_ptr_full[edge_new];
- FrameBasePtr frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new];
- frame_new_ptr_full->addCapture(capture_ptr_full);
- frame_new_ptr_prun->addCapture(capture_ptr_prun);
- capture_ptr_full->addFeature(feature_ptr_full);
- capture_ptr_prun->addFeature(feature_ptr_prun);
- FactorOdom2DAnalytic* factor_ptr_full = new FactorOdom2DAnalytic(feature_ptr_full, frame_old_ptr_full);
- FactorOdom2DAnalytic* factor_ptr_prun = new FactorOdom2DAnalytic(feature_ptr_prun, frame_old_ptr_prun);
- feature_ptr_full->addFactor(factor_ptr_full);
- feature_ptr_prun->addFactor(factor_ptr_prun);
- //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameOther()->id() << std::endl;
- //std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl;
- //std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
-
- t1 = clock();
- Scalar xi = *(frame_old_ptr_prun->getP()->get());
- Scalar yi = *(frame_old_ptr_prun->getP()->get()+1);
- Scalar thi = *(frame_old_ptr_prun->getO()->get());
- Scalar si = sin(thi);
- Scalar ci = cos(thi);
- Scalar xj = *(frame_new_ptr_prun->getP()->get());
- Scalar yj = *(frame_new_ptr_prun->getP()->get()+1);
- Eigen::MatrixXs Ji(3,3), Jj(3,3);
- Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
- si,-ci,-(xj-xi)*ci-(yj-yi)*si,
- 0, 0, -1;
- Jj << ci, si, 0,
- -si, ci, 0,
- 0, 0, 1;
- //std::cout << "Ji" << std::endl << Ji << std::endl;
- //std::cout << "Jj" << std::endl << Jj << std::endl;
- Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
- insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3);
- insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3);
- insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3);
- insertSparseBlock((Jj.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_new*3, edge_old*3);
- Lambda = Lambda + DeltaLambda;
- t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
- }
- }
- else
- assert("unknown line");
- }
- printf("\nGraph loaded!\n");
- }
- else
- printf("\nError opening file\n");
-
- // PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
- CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
- CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
- first_frame_full->addCapture(initial_covariance_full);
- first_frame_prun->addCapture(initial_covariance_prun);
- initial_covariance_full->process();
- initial_covariance_prun->process();
- //std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
- t1 = clock();
- Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
- insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
- Lambda = Lambda + DeltaLambda;
- t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // COMPUTE COVARIANCES
- FactorBasePtrList factors;
- wolf_problem_prun->getTrajectory()->getFactorList(factors);
- // Manual covariance computation
- t1 = clock();
- Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A
- Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols()));
- t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
- //std::cout << "Lambda" << std::endl << Lambda << std::endl;
- //std::cout << "Sigma" << std::endl << Sigma << std::endl;
-
- std::cout << " ceres is computing covariances..." << std::endl;
- t1 = clock();
- ceres_manager_prun->computeCovariances(ALL);//ALL_MARGINALS
- double t_sigma_ceres = ((double) clock() - t1) / CLOCKS_PER_SEC;
- std::cout << "computed!" << std::endl;
-
- t1 = clock();
-
- for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++)
- {
- if ((*c_it)->getCategory() != FAC_FRAME) continue;
-
- // Measurement covariance
- Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
- //std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
- //std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
-
- // NEW WAY
- // State covariance
- //11
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_11);
- //12
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_12);
- //13
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_13);
- //14
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_14);
-
- //22
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_22);
- //23
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_23);
- //24
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_24);
-
- //33
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_33);
- //34
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_34);
-
- //44
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_44);
-
-// std::cout << "Sigma_11" << std::endl << Sigma_11 << std::endl;
-// std::cout << "Sigma_12" << std::endl << Sigma_12 << std::endl;
-// std::cout << "Sigma_13" << std::endl << Sigma_13 << std::endl;
-// std::cout << "Sigma_14" << std::endl << Sigma_14 << std::endl;
-// std::cout << "Sigma_22" << std::endl << Sigma_22 << std::endl;
-// std::cout << "Sigma_23" << std::endl << Sigma_23 << std::endl;
-// std::cout << "Sigma_24" << std::endl << Sigma_24 << std::endl;
-// std::cout << "Sigma_33" << std::endl << Sigma_33 << std::endl;
-// std::cout << "Sigma_34" << std::endl << Sigma_34 << std::endl;
-// std::cout << "Sigma_44" << std::endl << Sigma_44 << std::endl;
-
- // jacobians
- ((FactorAnalytic*)(*c_it))->evaluatePureJacobians(jacobians);
- Eigen::MatrixXs& J1 = jacobians[0];
- Eigen::MatrixXs& J2 = jacobians[1];
- Eigen::MatrixXs& J3 = jacobians[2];
- Eigen::MatrixXs& J4 = jacobians[3];
-// std::cout << "J1" << std::endl << J1 << std::endl;
-// std::cout << "J2" << std::endl << J2 << std::endl;
-// std::cout << "J3" << std::endl << J3 << std::endl;
-// std::cout << "J4" << std::endl << J4 << std::endl;
-
- // Information gain
- Scalar IG_new = 0.5 * log( Sigma_z.determinant() /
- ( Sigma_z - (J1 * Sigma_11 * J1.transpose() +
- J1 * Sigma_12 * J2.transpose() +
- J1 * Sigma_13 * J3.transpose() +
- J1 * Sigma_14 * J4.transpose() +
- J2 * Sigma_12.transpose() * J1.transpose() +
- J2 * Sigma_22 * J2.transpose() +
- J2 * Sigma_23 * J3.transpose() +
- J2 * Sigma_24 * J4.transpose() +
- J3 * Sigma_13.transpose() * J1.transpose() +
- J3 * Sigma_23.transpose() * J2.transpose() +
- J3 * Sigma_33 * J3.transpose() +
- J3 * Sigma_34 * J4.transpose() +
- J4 * Sigma_14.transpose() * J1.transpose() +
- J4 * Sigma_24.transpose() * J2.transpose() +
- J4 * Sigma_34.transpose() * J3.transpose() +
- J4 * Sigma_44 * J4.transpose()) ).determinant() );
-
-// std::cout << "part = " << std::endl << (J1 * Sigma_11 * J1.transpose() +
-// J1 * Sigma_12 * J2.transpose() +
-// J1 * Sigma_13 * J3.transpose() +
-// J1 * Sigma_14 * J4.transpose() +
-// J2 * Sigma_12.transpose() * J1.transpose() +
-// J2 * Sigma_22 * J2.transpose() +
-// J2 * Sigma_23 * J3.transpose() +
-// J2 * Sigma_24 * J4.transpose() +
-// J3 * Sigma_13.transpose() * J1.transpose() +
-// J3 * Sigma_23.transpose() * J2.transpose() +
-// J3 * Sigma_33 * J3.transpose() +
-// J3 * Sigma_34 * J4.transpose() +
-// J4 * Sigma_14.transpose() * J1.transpose() +
-// J4 * Sigma_24.transpose() * J2.transpose() +
-// J4 * Sigma_34.transpose() * J3.transpose() +
-// J4 * Sigma_44 * J4.transpose()) << std::endl;
- std::cout << "IG_new = " << IG_new << std::endl;
-
- // OLD WAY
- // ii (old)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
-// std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
-// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
- // jj (new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
-// std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
-// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
- // ij (old-new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
-// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
-// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
-
- //jacobian
- xi = *(*c_it)->getFrameOther()->getP()->get();
- yi = *((*c_it)->getFrameOther()->getP()->get()+1);
- thi = *(*c_it)->getFrameOther()->getO()->get();
- si = sin(thi);
- ci = cos(thi);
- xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
- yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1);
-
- Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
- si,-ci,-(xj-xi)*ci-(yj-yi)*si,
- 0, 0, -1;
- Jj << ci, si, 0,
- -si, ci, 0,
- 0, 0, 1;
-// std::cout << "Ji" << std::endl << Ji << std::endl;
-// std::cout << "Jj" << std::endl << Jj << std::endl;
-
- //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
- // Information gain
- Scalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
-
-// std::cout << "part = " << std::endl << (Ji * Sigma_ii * Ji.transpose() +
-// Jj * Sigma_jj * Jj.transpose() +
-// Ji * Sigma_ij * Jj.transpose() +
-// Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
- std::cout << "IG = " << IG << std::endl;
-
- std::cout << "difference IG = " << std::abs(IG - IG_new) << std::endl;
- assert((std::abs((IG - IG_new)/IG) < 0.1 || std::isnan(IG - IG_new)) && "not equals information gains");
-
- if (IG < 2 && IG > 0 && !std::isnan(IG))
- {
- // Store as a candidate to be prunned, ordered by information gain
- auto ordered_fac_it = ordered_fac_ptr.begin();
- for (auto ordered_ig_it = ordered_ig.begin(); ordered_ig_it != ordered_ig.end(); ordered_ig_it++, ordered_fac_it++ )
- if (IG < (*ordered_ig_it))
- {
- ordered_ig.insert(ordered_ig_it, IG);
- ordered_fac_ptr.insert(ordered_fac_it, (*c_it));
- break;
- }
- ordered_ig.insert(ordered_ig.end(), IG);
- ordered_fac_ptr.insert(ordered_fac_ptr.end(), (*c_it));
- }
- }
-
- // PRUNNING
- std::vector<bool> any_inactive_in_frame(wolf_problem_prun->getTrajectory()->getFrameList().size(), false);
- for (auto c_it = ordered_fac_ptr.begin(); c_it != ordered_fac_ptr.end(); c_it++ )
- {
- // Check heuristic: factor do not share node with any inactive factor
- unsigned int& index_frame = frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()];
- unsigned int& index_frame_other = frame_ptr_2_index_prun[(*c_it)->getFrameOther()];
-
- if (!any_inactive_in_frame[index_frame] && !any_inactive_in_frame[index_frame_other])
- {
- std::cout << "setting inactive" << (*c_it)->id() << std::endl;
- (*c_it)->setStatus(FAC_INACTIVE);
- std::cout << "set!" << std::endl;
- any_inactive_in_frame[index_frame] = true;
- any_inactive_in_frame[index_frame_other] = true;
- }
- }
-
- double t_ig = ((double) clock() - t1) / CLOCKS_PER_SEC;
- std::cout << "manual sigma computation " << t_sigma_manual << "s" << std::endl;
- std::cout << "ceres sigma computation " << t_sigma_ceres << "s" << std::endl;
- std::cout << "information gain computation " << t_ig << "s" << std::endl;
-
- // SOLVING PROBLEMS
- std::cout << "FULL PROBLEM" << std::endl;
- std::cout << "solving..." << std::endl;
- summary_full = ceres_manager_full->solve();
- std::cout << summary_full.FullReport() << std::endl;
- std::cout << "PRUNNED PROBLEM" << std::endl;
- std::cout << "solving..." << std::endl;
- summary_prun = ceres_manager_prun->solve();
- std::cout << summary_prun.FullReport() << std::endl;
-
- delete wolf_problem_full; //not necessary to delete anything more, wolf will do it!
- std::cout << "wolf_problem_ deleted!" << std::endl;
- delete ceres_manager_full;
- delete ceres_manager_prun;
- std::cout << "ceres_manager deleted!" << std::endl;
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/demos/demo_wolf_root.cpp b/demos/demo_wolf_root.cpp
deleted file mode 100644
index 4ea048471753a28281c01dc50f3fcda0316f0bd7..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_root.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-/**
- * \file test_wolf_root.cpp
- *
- * Created on: Apr 12, 2016
- * \author: Jeremie Deray
- */
-
-//Wolf
-#include "core/common/wolf.h"
-
-//std
-#include <iostream>
-
-int main(int /*argc*/, char** /*argv*/)
-{
- std::cout << "Your wolf root directory is (_WOLF_ROOT_DIR) : " << _WOLF_ROOT_DIR << std::endl;
-
- return 1;
-}
diff --git a/demos/demo_wolf_tree.cpp b/demos/demo_wolf_tree.cpp
deleted file mode 100644
index 9a0075a93c089db05f9bb044ec9c5f90f237228f..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_tree.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-// Testing create and delete full wolf tree with odometry captures
-
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "wolf_manager.h"
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- //Welcome message
- std::cout << std::endl << " ========= WOLF TREE test ===========" << std::endl << std::endl;
-
- SensorOdom2D* odom_sensor_ptr_ = new SensorOdom2D(std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
- std::make_shared<StateBlock>(Eigen::Vector1s::Zero()), 0.1, 0.1);
- //std::cout << " odom sensor created!" << std::endl;
-
- WolfManager* wolf_manager_ = new WolfManager(FRM_PO_2D, //frame structure
- odom_sensor_ptr_, //odom sensor
- Eigen::Vector3s::Zero(), //prior
- Eigen::Matrix3s::Identity()*0.01, //prior cov
- 5, //window size
- 1); //time for new keyframe
- //std::cout << " wolf_manager_ created!" << std::endl;
-
- wolf_manager_->addSensor(odom_sensor_ptr_);
- //std::cout << " odom sensor added!" << std::endl;
-
- //main loop
- for (unsigned int ii = 0; ii<1000; ii++)
- {
- // Add sintetic odometry
- wolf_manager_->addCapture(new CaptureOdom2D(TimeStamp(ii*0.1),
- TimeStamp(ii*0.1+0.01),
- odom_sensor_ptr_,
- Eigen::Vector3s(0.1, 0. ,0.05)));
- //std::cout << " capture added!" << std::endl;
-
- // update wolf tree
- wolf_manager_->update();
- //std::cout << " updated!" << std::endl;
- }
- //std::cout << " end for!" << std::endl;
-
- delete wolf_manager_; //not necessary to delete anything more, wolf will do it!
-
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
-
diff --git a/demos/demo_yaml.cpp b/demos/demo_yaml.cpp
deleted file mode 100644
index 5768f4b50ce0963ba4974baa10d27c4ffc1e2382..0000000000000000000000000000000000000000
--- a/demos/demo_yaml.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/**
- * \file yaml-test.cpp
- *
- * Created on: May 1, 2016
- * \author: jsola
- */
-
-#include "core/math/pinhole_tools.h"
-#include "yaml/yaml_conversion.h"
-#include "processor_image_feature.h"
-#include "core/common/factory.h"
-
-#include <yaml-cpp/yaml.h>
-
-#include <eigen3/Eigen/Dense>
-
-#include <iostream>
-#include <fstream>
-
-int main()
-{
-
- //=============================================================================================
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::cout << "\nwolf root directory: " << wolf_root << std::endl;
- //=============================================================================================
-
- using namespace Eigen;
- using namespace wolf;
- using std::string;
- using YAML::Node;
-
- // Camera parameters
-
- YAML::Node camera_config = YAML::LoadFile(wolf_root + "/src/examples/camera.yaml");
-
- if (camera_config["sensor type"])
- {
- std::string sensor_type = camera_config["sensor type"].as<std::string>();
-
- std::string sensor_name = camera_config["sensor name"].as<std::string>();
-
- YAML::Node params = camera_config["intrinsic"];
-
- // convert yaml to Eigen
- Vector3s pos = camera_config["extrinsic"]["position"].as<Vector3s>();
- Vector3s ori = camera_config["extrinsic"]["orientation"].as<Vector3s>() * M_PI / 180;
- Vector2s size = params["image size"].as<Vector2s>();
- Vector4s intrinsic = params["pinhole model"].as<Vector4s>();
- VectorXs distortion = params["distortion"].as<VectorXs>();
-
- // compute correction model
- VectorXs correction(distortion.size());
- pinhole::computeCorrectionModel(intrinsic, distortion, correction);
-
- // output
- std::cout << "sensor type : " << sensor_type << std::endl;
- std::cout << "sensor name : " << sensor_name << std::endl;
- std::cout << "sensor extrinsics : " << std::endl;
- std::cout << "\tposition : " << pos.transpose() << std::endl;
- std::cout << "\torientation : " << ori.transpose() << std::endl;
- std::cout << "sensor parameters : " << std::endl;
- std::cout << "\timage size : " << size.transpose() << std::endl;
- std::cout << "\tpinhole model : " << intrinsic.transpose() << std::endl;
- std::cout << "\tdistoriton : " << distortion.transpose() << std::endl;
- std::cout << "\tcorrection : " << correction.transpose() << std::endl;
- }
- else
- std::cout << "Bad configuration file. No sensor type found." << std::endl;
-
-// // Processor Image parameters
-//
-// ProcessorParamsImage p;
-//
-// Node params = YAML::LoadFile(wolf_root + "/src/examples/processor_image_feature.yaml");
-//
-// if (params["processor type"])
-// {
-// Node as = params["active search"];
-// p.active_search.grid_width = as["grid width"].as<unsigned int>();
-// p.active_search.grid_height = as["grid height"].as<unsigned int>();
-// p.active_search.separation = as["separation"].as<unsigned int>();
-//
-// Node img = params["image"];
-// p.image.width = img["width"].as<unsigned int>();
-// p.image.height = img["height"].as<unsigned int>();
-//
-// Node alg = params["algorithm"];
-// p.max_new_features = alg["maximum new features"].as<unsigned int>();
-// p.min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>();
-// }
-
- return 0;
-}
diff --git a/hello_wolf/CMakeLists.txt b/demos/hello_wolf/CMakeLists.txt
similarity index 60%
rename from hello_wolf/CMakeLists.txt
rename to demos/hello_wolf/CMakeLists.txt
index f8f41bd407e9e4d9fc9eaf71f92b7111b0772feb..eff58ca207ecc5d2603e1839d59c3a69098d88fe 100644
--- a/hello_wolf/CMakeLists.txt
+++ b/demos/hello_wolf/CMakeLists.txt
@@ -7,23 +7,25 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/factor_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/factor_range_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.h
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2d.h
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.h
)
SET(SRCS_HELLOWOLF
-# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp
${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
+ ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2d.cpp
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
)
+
+
add_library(hellowolf SHARED ${SRCS_HELLOWOLF})
+TARGET_LINK_LIBRARIES(hellowolf ${PLUGIN_NAME})
+
ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
-TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME} hellowolf)
-add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
-TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf)
-add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
-TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME} hellowolf)
\ No newline at end of file
+TARGET_LINK_LIBRARIES(hello_wolf ${PLUGIN_NAME} hellowolf)
+
+ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
+TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME} hellowolf)
diff --git a/hello_wolf/capture_range_bearing.cpp b/demos/hello_wolf/capture_range_bearing.cpp
similarity index 73%
rename from hello_wolf/capture_range_bearing.cpp
rename to demos/hello_wolf/capture_range_bearing.cpp
index f2e3ad79dfac128288c65d127b47d4efcd599685..9ead1c183b8b08a64f709faea5236d5dbb056231 100644
--- a/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.cpp
@@ -1,5 +1,5 @@
/*
- * CaptureRangeBearing2D.cpp
+ * CaptureRangeBearing2d.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
@@ -10,8 +10,8 @@
namespace wolf
{
-CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _bearings) :
- CaptureBase("RANGE BEARING", _ts, _scanner),
+CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
+ CaptureBase("CaptureRangeBearing", _ts, _scanner),
ids_(_ids),
ranges_(_ranges),
bearings_(_bearings)
diff --git a/hello_wolf/capture_range_bearing.h b/demos/hello_wolf/capture_range_bearing.h
similarity index 54%
rename from hello_wolf/capture_range_bearing.h
rename to demos/hello_wolf/capture_range_bearing.h
index 49af84cad89fb9befe2bb2d5b7475e999d602341..10fb8fa05f14fc4801effce4fb734c90a9ea2e90 100644
--- a/hello_wolf/capture_range_bearing.h
+++ b/demos/hello_wolf/capture_range_bearing.h
@@ -20,22 +20,22 @@ using namespace Eigen;
class CaptureRangeBearing : public CaptureBase
{
public:
- CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _bearings);
- virtual ~CaptureRangeBearing();
+ CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings);
+ ~CaptureRangeBearing() override;
const VectorXi& getIds () const;
const int& getId (int _i) const;
- const Eigen::VectorXs& getRanges () const;
- const Eigen::VectorXs& getBearings () const;
- const Scalar& getRange (int _i) const;
- const Scalar& getBearing (int _i) const;
- Eigen::Vector2s getRangeBearing (int _i) const;
+ const Eigen::VectorXd& getRanges () const;
+ const Eigen::VectorXd& getBearings () const;
+ const double& getRange (int _i) const;
+ const double& getBearing (int _i) const;
+ Eigen::Vector2d getRangeBearing (int _i) const;
Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const;
private:
VectorXi ids_; // identifiers
- VectorXs ranges_; // ranges
- VectorXs bearings_; // bearings
+ VectorXd ranges_; // ranges
+ VectorXd bearings_; // bearings
};
inline const Eigen::VectorXi& CaptureRangeBearing::getIds() const
@@ -48,34 +48,34 @@ inline const int& CaptureRangeBearing::getId(int _i) const
return ids_(_i);
}
-inline const Eigen::VectorXs& CaptureRangeBearing::getRanges() const
+inline const Eigen::VectorXd& CaptureRangeBearing::getRanges() const
{
return ranges_;
}
-inline const Eigen::VectorXs& CaptureRangeBearing::getBearings() const
+inline const Eigen::VectorXd& CaptureRangeBearing::getBearings() const
{
return bearings_;
}
-inline const Scalar& CaptureRangeBearing::getRange(int _i) const
+inline const double& CaptureRangeBearing::getRange(int _i) const
{
return ranges_(_i);
}
-inline const Scalar& CaptureRangeBearing::getBearing(int _i) const
+inline const double& CaptureRangeBearing::getBearing(int _i) const
{
return bearings_(_i);
}
-inline Eigen::Matrix<Scalar,Dynamic,2> CaptureRangeBearing::getRangeBearing() const
+inline Eigen::Matrix<double,Dynamic,2> CaptureRangeBearing::getRangeBearing() const
{
- return (Matrix<Scalar,Dynamic,2>() << ranges_, bearings_).finished();
+ return (Matrix<double,Dynamic,2>() << ranges_, bearings_).finished();
}
-inline Eigen::Vector2s CaptureRangeBearing::getRangeBearing(int _i) const
+inline Eigen::Vector2d CaptureRangeBearing::getRangeBearing(int _i) const
{
- return Vector2s(ranges_(_i), bearings_(_i));
+ return Vector2d(ranges_(_i), bearings_(_i));
}
} /* namespace wolf */
diff --git a/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
similarity index 63%
rename from hello_wolf/factor_bearing.h
rename to demos/hello_wolf/factor_bearing.h
index 56857fe0a188a5eb3ab30d8f4f6a967102f8b7d0..d3902daadbda071ae5a08e5d74148c877d44b93e 100644
--- a/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
@@ -19,14 +19,14 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
{
public:
FactorBearing(const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", nullptr, nullptr, nullptr,
- _landmark_other_ptr, _processor_ptr,
- _apply_loss_function, _status,
- getCapture()->getFrame()->getP(),
- getCapture()->getFrame()->getO(),
- _landmark_other_ptr->getP())
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, FactorStatus _status) :
+ FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr,
+ _landmark_other_ptr, _processor_ptr,
+ _apply_loss_function, _status,
+ getCapture()->getFrame()->getP(),
+ getCapture()->getFrame()->getO(),
+ _landmark_other_ptr->getP())
{
//
}
@@ -57,7 +57,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
{
// 1. produce a transformation matrix to transform from robot frame to world frame
- Transform<T, 2, Affine> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform
+ Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform
// Map input pointers into meaningful Eigen elements
Map<const Matrix<T, 2, 1>> point_w(_p2);
Map<const Matrix<T, 1, 1>> res(_res);
@@ -68,9 +68,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
// 3. Get the expected bearing of the point
T bearing = atan2(point_r(1), point_r(0));
- // 4. Get the measured range-and-bearing to the point, and its covariance
- Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
- Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance().cast<T>();
+ // 4. Get the measured range-and-bearing to the point
+ Matrix<T, 2, 1> range_bearing = getMeasurement();
// 5. Get the bearing error by comparing against the bearing measurement
T er = range_bearing(1) - bearing;
@@ -80,8 +79,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
er -= T(2*M_PI);
// 6. Compute the residual by scaling according to the standard deviation of the bearing part
- T sigma = sqrt(range_bearing_cov(1,1));
- *_res = er / sigma;
+ *_res = er * T(getMeasurementSquareRootInformationUpper()(1,1));
return true;
}
diff --git a/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
similarity index 84%
rename from hello_wolf/factor_range_bearing.h
rename to demos/hello_wolf/factor_range_bearing.h
index c4a321badc8673aaa8c7ff57c61a90a987d5d831..596278b2f70d362ba3c1debeec12fb9d8830795e 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/demos/hello_wolf/factor_range_bearing.h
@@ -21,12 +21,15 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
{
public:
FactorRangeBearing(const CaptureBasePtr& _capture_own, // own capture's pointer
- const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
- const ProcessorBasePtr& _processor_ptr, // processor having created this
- bool _apply_loss_function, // apply loss function to residual?
- FactorStatus _status) : // active factor?
+ const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
+ const ProcessorBasePtr& _processor_ptr, // processor having created this
+ bool _apply_loss_function, // apply loss function to residual?
+ FactorStatus _status) : // active factor?
FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
"RANGE BEARING", // factor type enum (see wolf.h)
+ TOP_LMK, // factor topology (see factor_base.h)
+ _feature_ptr,
nullptr, // other frame's pointer
nullptr, // other capture's pointer
nullptr, // other feature's pointer
@@ -43,7 +46,7 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
//
}
- virtual ~FactorRangeBearing()
+ ~FactorRangeBearing() override
{
//
}
@@ -97,11 +100,11 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
Map<Matrix<T, 2, 1>> res(_res); // residual
// 1. produce transformation matrices to transform from sensor frame --> to robot frame --> to world frame
- Transform<T, 2, Affine> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
- Transform<T, 2, Affine> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Sensor frame = sensor-to-robot transform
+ Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
+ Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Sensor frame = sensor-to-robot transform
// 2. Transform world-referenced landmark point to sensor-referenced point
- Transform<T, 2, Affine> H_w_s = H_w_r * H_r_s; // world-to-sensor transform
+ Transform<T, 2, Isometry> H_w_s = H_w_r * H_r_s; // world-to-sensor transform
Matrix<T, 2, 1> lmk_s = H_w_s.inverse() * lmk; // point in sensor frame
// 3. Get the expected range-and-bearing of the point
@@ -110,7 +113,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
exp_rb(1) = atan2(lmk_s(1), lmk_s(0)); // bearing
// 4. Get the measured range-and-bearing to the point
- Matrix<T, 2, 1> meas_rb = getMeasurement().cast<T>(); // cast Eigen type vector to have scalar type 'T'
+ auto& meas_rb = getMeasurement();
// 5. Get the error by comparing the expected against the measurement
Matrix<T, 2, 1> err_rb = meas_rb - exp_rb;
@@ -126,7 +129,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
// R^T * R = Omega
// in other words, R is the Cholesky decomposition of Omega.
// NOTE: you get R directly from the Feature with getMeasurementSquareRootInformationUpper()
- res = getMeasurementSquareRootInformationUpper().cast<T>() * err_rb;
+ res = getMeasurementSquareRootInformationUpper() * err_rb;
return true;
}
diff --git a/demos/hello_wolf/feature_range_bearing.cpp b/demos/hello_wolf/feature_range_bearing.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ea584a419aa21c1d7e462824d6872f7d57bd818c
--- /dev/null
+++ b/demos/hello_wolf/feature_range_bearing.cpp
@@ -0,0 +1,25 @@
+/*
+ * FeatureRangeBearing2d.cpp
+ *
+ * Created on: Nov 30, 2017
+ * Author: jsola
+ */
+
+#include "feature_range_bearing.h"
+
+namespace wolf
+{
+
+FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
+ FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
+{
+ //
+}
+
+FeatureRangeBearing::~FeatureRangeBearing()
+{
+ //
+}
+
+} // namespace wolf
+
diff --git a/hello_wolf/feature_range_bearing.h b/demos/hello_wolf/feature_range_bearing.h
similarity index 68%
rename from hello_wolf/feature_range_bearing.h
rename to demos/hello_wolf/feature_range_bearing.h
index b68419434b320aa3c2352e833419cb33dbad14aa..b924d29eea0a22a80267ace90718f9a66f54dac1 100644
--- a/hello_wolf/feature_range_bearing.h
+++ b/demos/hello_wolf/feature_range_bearing.h
@@ -1,5 +1,5 @@
/*
- * FeatureRangeBearing2D.h
+ * FeatureRangeBearing2d.h
*
* Created on: Nov 30, 2017
* Author: jsola
@@ -18,8 +18,8 @@ WOLF_PTR_TYPEDEFS(FeatureRangeBearing);
class FeatureRangeBearing : public FeatureBase
{
public:
- FeatureRangeBearing(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
- virtual ~FeatureRangeBearing();
+ FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+ ~FeatureRangeBearing() override;
};
} // namespace wolf
diff --git a/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
similarity index 78%
rename from hello_wolf/hello_wolf.cpp
rename to demos/hello_wolf/hello_wolf.cpp
index 827d6d77bd4a47f5783f2162e21c90c309ef4709..2b2d03d43f1a7d315efb0ae07a734ec022f9e4c6 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -12,18 +12,17 @@
* \author: jsola
*/
-#include "core/common/wolf.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
+// wolf core includes
+#include "../../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/common/wolf.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/capture/capture_odom_2d.h"
#include "sensor_range_bearing.h"
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
-#include "feature_range_bearing.h"
-#include "factor_range_bearing.h"
-#include "landmark_point_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
int main()
{
@@ -105,16 +104,15 @@ int main()
// Wolf problem and solver
ProblemPtr problem = Problem::create("PO", 2);
- ceres::Solver::Options options;
- options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
- CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
+ SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem);
+ ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
- // sensor odometer 2D
- IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>();
- SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
+ // sensor odometer 2d
+ ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>();
+ SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
- // processor odometer 2D
- ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
+ // processor odometer 2d
+ ParamsProcessorOdom2dPtr params_odo = std::make_shared<ParamsProcessorOdom2d>();
params_odo->voting_active = true;
params_odo->time_tolerance = 0.1;
params_odo->max_time_span = 999;
@@ -122,55 +120,57 @@ int main()
params_odo->angle_turned = 999;
params_odo->cov_det = 999;
params_odo->unmeasured_perturbation_std = 0.001;
- ProcessorBasePtr processor = problem->installProcessor("ODOM 2D", "processor odo", sensor_odo, params_odo);
- ProcessorOdom2DPtr processor_odo = std::static_pointer_cast<ProcessorOdom2D>(processor);
+ ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2d", "processor odo", sensor_odo, params_odo);
// sensor Range and Bearing
- IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
+ ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
intrinsics_rb->noise_range_metres_std = 0.1;
intrinsics_rb->noise_bearing_degrees_std = 1.0;
- SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
+ SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", Vector3d(1,1,0), intrinsics_rb);
// processor Range and Bearing
- ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
+ ParamsProcessorRangeBearingPtr params_rb = std::make_shared<ParamsProcessorRangeBearing>();
params_rb->voting_active = false;
params_rb->time_tolerance = 0.01;
- ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
+ ProcessorBasePtr processor_rb = problem->installProcessor("ProcessorRangeBearing", "processor RB", sensor_rb, params_rb);
+
+ // initialize
+ TimeStamp t(0.0); // t : 0.0
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
+ std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
- // NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
+ // SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
sensor_rb->getO()->unfix();
- // NOTE: SELF-CALIBRATION OF SENSOR POSITION
- // The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
+ // SELF-CALIBRATION OF SENSOR POSITION
+ // The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
// sensor_rb->getP()->unfix();
// CONFIGURE ==========================================================
// Motion data
- Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
- Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
+ Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
+ Matrix2d motion_cov = 0.1 * Matrix2d::Identity();
// landmark observations data
VectorXi ids;
- VectorXs ranges, bearings;
+ VectorXd ranges, bearings;
// SET OF EVENTS =======================================================
std::cout << std::endl;
- WOLF_TRACE("======== BUILD PROBLEM =======")
+ WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======")
// We'll do 3 steps of motion and landmark observations.
// STEP 1 --------------------------------------------------------------
- // initialize
- TimeStamp t(0.0); // t : 0.0
- Vector3s x(0,0,0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
- problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
-
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
@@ -183,7 +183,7 @@ int main()
t += 1.0; // t : 1.0
// motion
- CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
+ CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
// observe lmks
@@ -220,19 +220,7 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto sen : problem->getHardware()->getSensorList())
- for (auto sb : sen->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto kf : problem->getTrajectory()->getFrameList())
- if (kf->isKeyOrAux())
- for (auto sb : kf->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto sb : lmk->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
// SOLVE again
@@ -244,30 +232,29 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectory()->getFrameList())
- if (kf->isKeyOrAux())
- {
- Eigen::MatrixXs cov;
- kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
- }
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- Eigen::MatrixXs cov;
- lmk->getCovariance(cov);
- WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
- }
+ for (auto& kf : *problem->getTrajectory())
+ {
+ Eigen::MatrixXd cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
+ for (auto& lmk : problem->getMap()->getLandmarkList())
+ {
+ Eigen::MatrixXd cov;
+ lmk->getCovariance(cov);
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ }
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
- problem->print(4,1,1,1);
+ problem->print(4,0,1,0);
/*
* ============= FIRST COMMENT ON THE RESULTS ==================
*
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
*
- * L3 POINT 2D <-- c8
+ * L3 POINT 2d <-- c8
* Est, x = ( 3 2 )
* sb: Est
*
@@ -289,10 +276,10 @@ int main()
*
* P: wolf tree status ---------------------
Hardware
- S1 ODOM 2D // Sensor 1, type ODOMETRY 2D.
+ S1 ODOM 2d // Sensor 1, type ODOMETRY 2d.
Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
- pm1 ODOM 2D // Processor 1, type ODOMETRY 2D
+ pm1 ODOM 2d // Processor 1, type ODOMETRY 2d
o: C7 - F3 // origin at Capture 7, Frame 3
l: C10 - F4 // last at Capture 10, frame 4
S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
@@ -307,7 +294,7 @@ int main()
f1 FIX <-- // Feature 1, type Fix
m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
c1 FIX --> A // Factor 1, type FIX, it is Absolute
- CM2 ODOM 2D -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
+ CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
f2 RANGE BEARING <-- // Feature 2, type R+B
m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
@@ -315,10 +302,10 @@ int main()
KF2 <-- c6
Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
sb: Est Est
- CM3 ODOM 2D -> S1 [Sta, Sta] <--
- f3 ODOM 2D <--
+ CM3 ODOM 2d -> S1 [Sta, Sta] <--
+ f3 ODOM 2d <--
m = ( 1 0 0 )
- c3 ODOM 2D --> F1 // Factor 3, type ODOM, against Frame 1
+ c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
C9 RANGE BEARING -> S2 [Sta, Sta] <--
f4 RANGE BEARING <--
m = ( 1.41 2.36 )
@@ -329,10 +316,10 @@ int main()
KF3 <--
Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
sb: Est Est
- CM7 ODOM 2D -> S1 [Sta, Sta] <--
- f6 ODOM 2D <--
+ CM7 ODOM 2d -> S1 [Sta, Sta] <--
+ f6 ODOM 2d <--
m = ( 1 0 0 )
- c6 ODOM 2D --> F2
+ c6 ODOM 2d --> F2
C12 RANGE BEARING -> S2 [Sta, Sta] <--
f7 RANGE BEARING <--
m = ( 1.41 2.36 )
@@ -343,15 +330,15 @@ int main()
F4 <--
Est, ts=2, x = ( 0.11 -0.045 0.26 )
sb: Est Est
- CM10 ODOM 2D -> S1 [Sta, Sta] <--
+ CM10 ODOM 2d -> S1 [Sta, Sta] <--
Map
- L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
+ L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
Est, x = ( 1 2 ) // L4 state is estimated, state vector
sb: Est // L4 has 1 state block estimated
- L2 POINT 2D <-- c5 c7
+ L2 POINT 2d <-- c5 c7
Est, x = ( 2 2 )
sb: Est
- L3 POINT 2D <-- c8
+ L3 POINT 2d <-- c8
Est, x = ( 3 2 )
sb: Est
-----------------------------------------
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edeb6b1f04f1f673ad3af9b94493b4e67e46ab9f
--- /dev/null
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -0,0 +1,368 @@
+/**
+ * \file hello_wolf_autoconf.cpp
+ *
+ * Created on: Jul 16, 2019
+ * \author: jsola
+ */
+
+
+// wolf core includes
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/solver/factory_solver.h"
+#include "core/common/wolf.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/processor/processor_motion.h"
+#include "core/yaml/parser_yaml.h"
+#include "capture_range_bearing.h"
+
+
+int main()
+ {
+ /*
+ * ============= PROBLEM DEFINITION ==================
+ *
+ * We have a planar robot with a range-and-bearing sensor 'S' mounted at its front-left corner, looking forward:
+ *
+ * ^ Y
+ * |
+ * ------------------S-> sensor at location (1,1) and orientation 0 degrees, that is, at pose (1,1,0).
+ * | | |
+ * | +--------|--> X robot axes X, Y
+ * | |
+ * -------------------
+ *
+ * The robot performs a straight trajectory with 3 keyframes 'KF', and observes 3 landmarks 'L'.
+ *
+ * The 3 robot keyframes, the 3 resulting sensor poses, and the 3 landmark positions can be sketched as follows
+ *
+ * (1,2) (2,2) (3,2) landmark positions in world frame
+ * L1 L2 L3 LANDMARKS
+ * | \ | \ |
+ * | \ | \ |
+ * | \ | \ |
+ * (1,1,0) (2,1,0) (3,1,0) sensor poses in world frame
+ * S-> S-> S-> SENSOR
+ * (1,1,0) (1,1,0) (1,1,0) sensor poses in robot frame
+ * / / /
+ * / / /
+ * / / /
+ * KF1->---KF2->---KF3-> KEYFRAMES -- robot poses
+ * (0,0,0) (1,0,0) (2,0,0) keyframe poses in world frame
+ * |
+ * * prior Initial robot pose in world frame
+ * (0,0,0)
+ *
+ * where:
+ * the links '--' are the motion factors,
+ * the links '\' and '|' are the landmark measurement factors, and
+ * the links '/' are the robot-to-sensor transforms.
+ *
+ * That is:
+ * - KFs have ids '1', '2', '3'
+ * - All KFs look East, so all theta = 0
+ * - KFs are at poses (0,0, 0), (1,0, 0), and (2,0, 0)
+ * - We set a prior at (0,0,0) on KF1 to render the system observable
+ * - The range-and-bearing sensor is mounted at pose (1,1, 0) w.r.t. the robot's origin
+ * - Lmks have ids '1', '2', '3'
+ * - Lmks are at positions (1,2), (2,2), (3,2)
+ * - Observations have ranges 1 or sqrt(2)
+ * - Observations have bearings pi/2 or 3pi/4
+ *
+ * The robot starts at (0,0,0) with a map with no previously known landmarks.
+ * At each keyframe, it does:
+ * - Create a motion factor to the previous keyframe
+ * - Measure some landmarks. For each measurement,
+ * - If the landmark is unknown, add it to the map and create an observation factor
+ * - If the landmark is known, create an observation factor
+ *
+ * The landmarks observed at each keyframe are as follows:
+ * - KF1: lmk 1
+ * - KF2: lmks 1 and 2
+ * - KF3: lmks 2 and 3
+ *
+ * After 3 keyframes, the problem is solved and the robot poses and landmark positions are optimized:
+ * - First, using the exact values as initial guess for the solver
+ * - Second, using random values
+ * Both solutions must produce the same exact values as in the sketches above.
+ *
+ * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 136)
+ *
+ * (c) 2017 Joan Sola @ IRI-CSIC
+ */
+
+ // SET PROBLEM =======================================================
+
+ using namespace wolf;
+
+
+ WOLF_TRACE("======== CONFIGURE PROBLEM =======");
+
+ // Config file to parse. Here is where all the problem is defined:
+ std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
+ std::string wolf_path = std::string(_WOLF_ROOT_DIR);
+
+ // parse file into params server: each param will be retrievable from this params server:
+ ParserYaml parser = ParserYaml(config_file, wolf_path);
+ ParamsServer server = ParamsServer(parser.getParams());
+ // Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
+ ProblemPtr problem = Problem::autoSetup(server);
+
+ // Print problem to see its status before processing any sensor data
+ problem->print(4,0,1,0);
+
+ // Solver. Configure a Ceres solver
+ // ceres::Solver::Options options;
+ // options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
+ // SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options);
+ SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
+
+ // recover sensor pointers and other stuff for later use (access by sensor name)
+ SensorBasePtr sensor_odo = problem->getSensor("sen odom");
+ SensorBasePtr sensor_rb = problem->getSensor("sen rb");
+
+ // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
+ TimeStamp t(0.0);
+ FrameBasePtr KF1 = problem->applyPriorOptions(t);
+ std::static_pointer_cast<ProcessorMotion>(problem->getProcessorIsMotion())->setOrigin(KF1);
+
+ // SELF CALIBRATION ===================================================
+ // These few lines control whether we calibrate some sensor parameters or not.
+
+ // SELF-CALIBRATION OF SENSOR ORIENTATION
+ // Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
+ // sensor_rb->getO()->unfix();
+
+ // SELF-CALIBRATION OF SENSOR POSITION
+ // The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
+ // sensor_rb->getP()->unfix();
+
+ // CONFIGURE input data (motion and measurements) ==============================================
+
+ // Motion data
+ Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
+ Matrix2d motion_cov = 0.1 * Matrix2d::Identity();
+
+ // landmark observations data
+ VectorXi ids;
+ VectorXd ranges, bearings;
+
+ // SET OF EVENTS: make things happen =======================================================
+ std::cout << std::endl;
+ WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======")
+
+ // We'll do 3 steps of motion and landmark observations.
+
+ // STEP 1 --------------------------------------------------------------
+
+ // observe lmks
+ ids.resize(1); ranges.resize(1); bearings.resize(1);
+ ids << 1; // will observe Lmk 1
+ ranges << 1.0; // see drawing
+ bearings << M_PI/2;
+ CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb->process(); // L1 : (1,2)
+
+ // STEP 2 --------------------------------------------------------------
+ t += 1.0; // t : 1.0
+
+ // motion
+ CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
+ cap_motion ->process(); // KF2 : (1,0,0)
+
+ // observe lmks
+ ids.resize(2); ranges.resize(2); bearings.resize(2);
+ ids << 1, 2; // will observe Lmks 1 and 2
+ ranges << sqrt(2.0), 1.0; // see drawing
+ bearings << 3*M_PI/4, M_PI/2;
+
+ cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb ->process(); // L1 : (1,2), L2 : (2,2)
+
+ // STEP 3 --------------------------------------------------------------
+ t += 1.0; // t : 2.0
+
+ // motion
+ cap_motion ->setTimeStamp(t);
+ cap_motion ->process(); // KF3 : (2,0,0)
+
+ // observe lmks
+ ids.resize(2); ranges.resize(2); bearings.resize(2);
+ ids << 2, 3; // will observe Lmks 2 and 3
+ ranges << sqrt(2.0), 1.0; // see drawing
+ bearings << 3*M_PI/4, M_PI/2;
+
+ cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb ->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+
+ problem->print(1,0,1,0);
+
+ // SOLVE ================================================================
+
+ // SOLVE with exact initial guess
+ WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
+ std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
+ problem->print(1,0,1,0);
+
+ // PERTURB initial guess
+ WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
+ problem->perturb(0.5); // Perturb all state blocks that are not fixed
+ problem->print(1,0,1,0);
+
+ // SOLVE again
+ WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
+ report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
+ problem->print(1,0,1,0);
+
+ // GET COVARIANCES of all states
+ WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
+ ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ for (auto& kf : *problem->getTrajectory())
+ {
+ Eigen::MatrixXd cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
+ for (auto& lmk : problem->getMap()->getLandmarkList())
+ {
+ Eigen::MatrixXd cov;
+ lmk->getCovariance(cov);
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ }
+ std::cout << std::endl;
+
+ WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
+ problem->print(4,1,1,1);
+
+ /*
+ * ============= FIRST COMMENT ON THE RESULTS ==================
+ *
+ * IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
+ *
+ * Lmk3 LandmarkPoint2d <-- Fac9
+ * P[Est] = ( 1 2 )
+ *
+ * it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) !
+ *
+ * Side notes:
+ *
+ * - Observe that all other KFs and Lmks are correct.
+ *
+ * - Try self-calibrating the sensor orientation by uncommenting line 136 (well, around 136)
+ *
+ */
+
+ /*
+ * ============= DETAILED DESCRIPTION OF THE PRINTED RESULT ==================
+ *
+ * The line problem->print(4,1,1,1) produces a printout of the status of the WOLF problem.
+ * The full message is explained below.
+ *
+ * P: wolf tree status ---------------------
+ Hardware
+ Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
+ sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
+ PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
+ o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
+ l: Cap8 - Frm4 // last at Capture 8, Frame 4
+ Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
+ sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
+ Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
+ Trajectory
+ KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
+ P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
+ O[Est] = ( 2.6e-09 ) // Orientation is estimated
+ Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
+ Ftr1 trk0 prior <-- // Position prior
+ m = ( 0 0 )
+ Fac1 FactorBlockAbsolute --> Abs
+ Ftr2 trk0 prior <-- // Orientation prior
+ m = ( 0 )
+ Fac2 FactorBlockAbsolute --> Abs
+ CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
+ buffer size : 0
+ Cap4 CaptureRangeBearing -> Sen2 <--
+ Ftr3 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac3 RANGE BEARING --> Lmk1
+ KFrm2 <-- Fac7
+ P[Est] = ( 1 5.7e-09 )
+ O[Est] = ( 5.7e-09 )
+ CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
+ // Its origin is at Capture 2; Frame 1
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr4 trk0 ProcessorOdom2d <--
+ m = ( 1 0 0 )
+ Fac4 FactorOdom2d --> Frm1
+ Cap7 CaptureRangeBearing -> Sen2 <--
+ Ftr5 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac5 RANGE BEARING --> Lmk1
+ Ftr6 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac6 RANGE BEARING --> Lmk2
+ KFrm3 <--
+ P[Est] = ( 2 1.2e-08 )
+ O[Est] = ( 6.6e-09 )
+ CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr7 trk0 ProcessorOdom2d <--
+ m = ( 1 0 0 )
+ Fac7 FactorOdom2d --> Frm2
+ Cap9 CaptureRangeBearing -> Sen2 <--
+ Ftr8 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac8 RANGE BEARING --> Lmk2
+ Ftr9 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac9 RANGE BEARING --> Lmk3
+ Frm4 <--
+ P[Est] = ( 2 0 )
+ O[Est] = ( 0 )
+ CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
+ buffer size : 1
+ delta preint : (0 0 0)
+ Map
+ Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
+ P[Est] = ( 1 2 )
+ Lmk2 LandmarkPoint2d <-- Fac6 Fac8
+ P[Est] = ( 2 2 )
+ Lmk3 LandmarkPoint2d <-- Fac9
+ P[Est] = ( 3 2 )
+ -----------------------------------------
+
+ *
+ * ============= GENERAL WOLF NOTES ==================
+ *
+ * Explanatory notes, general to the Wolf architecture design:
+ *
+ * (1): Sensor params (extrinsics and intrinsics) can be declared Dynamic or Static
+ * Static: they do not change with time --> stored in the Sensor
+ * Dynamic: they change with time --> stored in each Capture
+ *
+ * (2): General params can be declared Fixed or Estimated
+ * Fixed: they are used as constant values, never estimated
+ * Estimated: they are estimated by the solver iteratively
+ *
+ * Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This produces 4 combinations:
+ *
+ * 1 Fixed + Static : general case of calibrated sensor.
+ * Example: rigidly fixed sensor with calibrated parameters
+ * This is the case for the current example (the SensorRangeBearing we use is like this)
+ *
+ * 2 Estimated + Static : Wolf performs self-calibration.
+ * Example: extrinsics self-calibration of a camera
+ *
+ * 3 Fixed + Dynamic : calibrated but variable parameters.
+ * Example: pan and tilt camera extrinsics, known at every time through precise encoders
+ *
+ * 4 Estimated + Dynamic : Wolf will track those sensor parameters that evolve with time.
+ * Example: IMU bias, which drift, must be tracked
+ *
+ */
+
+ return 0;
+}
diff --git a/demos/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b2ad4c0e8a9b576f67e9d8d56e41d932abedef8
--- /dev/null
+++ b/demos/hello_wolf/landmark_point_2d.cpp
@@ -0,0 +1,24 @@
+/**
+ * \file landmark_point_2d.cpp
+ *
+ * Created on: Dec 4, 2017
+ * \author: jsola
+ */
+
+#include "landmark_point_2d.h"
+
+namespace wolf
+{
+
+LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
+ LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
+{
+ setId(_id);
+}
+
+LandmarkPoint2d::~LandmarkPoint2d()
+{
+ //
+}
+
+} /* namespace wolf */
diff --git a/demos/hello_wolf/landmark_point_2d.h b/demos/hello_wolf/landmark_point_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..79287af069fb5b5da0ef7186333b90fc18664bbb
--- /dev/null
+++ b/demos/hello_wolf/landmark_point_2d.h
@@ -0,0 +1,27 @@
+/**
+ * \file landmark_point_2d.h
+ *
+ * Created on: Dec 4, 2017
+ * \author: jsola
+ */
+
+#ifndef HELLO_WOLF_LANDMARK_POINT_2d_H_
+#define HELLO_WOLF_LANDMARK_POINT_2d_H_
+
+#include "core/landmark/landmark_base.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(LandmarkPoint2d);
+
+class LandmarkPoint2d : public LandmarkBase
+{
+ public:
+ LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
+ ~LandmarkPoint2d() override;
+};
+
+} /* namespace wolf */
+
+#endif /* HELLO_WOLF_LANDMARK_POINT_2d_H_ */
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7df38e73367b7b45aff81cdbf5f00099d64aa990
--- /dev/null
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -0,0 +1,170 @@
+/*
+ * processor_range_bearing.cpp
+ *
+ * Created on: Nov 30, 2017
+ * Author: jsola
+ */
+
+#include "processor_range_bearing.h"
+#include "capture_range_bearing.h"
+#include "landmark_point_2d.h"
+#include "feature_range_bearing.h"
+#include "factor_range_bearing.h"
+
+namespace wolf
+{
+
+ProcessorRangeBearing::ProcessorRangeBearing(ParamsProcessorBasePtr _params) :
+ ProcessorBase("ProcessorRangeBearing", 2, _params)
+{
+ //
+}
+
+void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
+{
+ if (_capture == nullptr)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+
+ // 1. get KF
+ FrameBasePtr kf(nullptr);
+ if ( !buffer_pack_kf_.empty() )
+ {
+ // KeyFrame Callback received
+ PackKeyFramePtr pack = buffer_pack_kf_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
+
+ if (pack!=nullptr)
+ kf = pack->key_frame;
+
+ buffer_pack_kf_.removeUpTo( _capture->getTimeStamp() );
+
+ assert( kf && "Callback KF is not close enough to _capture!");
+ }
+
+ if (!kf)
+ {
+ // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
+ kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
+ assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
+ }
+
+ // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
+ CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
+ capture_rb->link(kf);
+
+ // 3. explore all observations in the capture
+ for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
+ {
+ // extract info
+ int id = capture_rb->getId (i);
+ double range = capture_rb->getRange (i);
+ double bearing = capture_rb->getBearing(i);
+
+ // 4. create or recover landmark
+ LandmarkPoint2dPtr lmk;
+ auto lmk_it = known_lmks.find(id);
+ if ( lmk_it != known_lmks.end() )
+ {
+ // known landmarks : recover landmark
+ lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second);
+ WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
+ }
+ else
+ {
+ // new landmark:
+ // - create landmark
+ lmk = LandmarkBase::emplace<LandmarkPoint2d>(getProblem()->getMap(), id, invObserve(range, bearing));
+ WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
+ // - add to known landmarks
+ known_lmks.emplace(id, lmk);
+ }
+
+ // 5. create feature
+ Vector2d rb(range,bearing);
+ auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
+ rb,
+ getSensor()->getNoiseCov());
+
+ // 6. create factor
+ auto prc = shared_from_this();
+ auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr,
+ capture_rb,
+ ftr,
+ lmk,
+ prc,
+ false,
+ FAC_ACTIVE);
+ }
+
+}
+
+
+Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const
+{
+ return polar(toSensor(lmk_w));
+}
+
+Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const
+{
+ return fromSensor(rect(r, b));
+}
+
+ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const
+{
+ Trf H = Eigen::Translation<double,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)) ;
+ return H;
+}
+
+ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position,
+ const Eigen::Vector1d& _orientation) const
+{
+ Trf H = Eigen::Translation<double,2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)) ;
+ return H;
+}
+
+Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
+{
+ Trf H_w_r = transform(getProblem()->getState().vector("PO"));
+ return H_w_r * H_r_s * lmk_s;
+}
+
+Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
+{
+// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
+ Trf H_w_r = transform(getProblem()->getState().vector("PO"));
+ return (H_w_r * H_r_s).inverse() * lmk_w;
+}
+
+Eigen::Vector2d ProcessorRangeBearing::polar(const Eigen::Vector2d& rect) const
+{
+ Eigen::Vector2d polar;
+ polar(0) = rect.norm();
+ polar(1) = atan2(rect(1), rect(0));
+ return polar;
+}
+
+Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const
+{
+ return range * (Vector2d() << cos(bearing), sin(bearing)).finished();
+}
+
+bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr)
+{
+ return true;
+}
+bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
+{
+ return false;
+}
+} /* namespace wolf */
+
+// Register in the SensorFactory
+#include "core/processor/factory_processor.h"
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorRangeBearing)
+} // namespace wolf
+
diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
new file mode 100644
index 0000000000000000000000000000000000000000..f086da9071e4956e3c7ef1ab2d59bff81326f594
--- /dev/null
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -0,0 +1,101 @@
+/*
+ * ProcessorRangeBearing.h
+ *
+ * Created on: Nov 30, 2017
+ * Author: jsola
+ */
+
+#ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
+#define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
+
+#include "core/processor/processor_base.h"
+#include "sensor_range_bearing.h"
+#include "Eigen/Geometry"
+
+#include <map>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorRangeBearing);
+
+struct ParamsProcessorRangeBearing : public ParamsProcessorBase
+{
+ // We do not need special parameters, but in case you need they should be defined here.
+ ParamsProcessorRangeBearing()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ParamsProcessorRangeBearing(std::string _unique_name, const ParamsServer& _server) :
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ //
+ }
+ std::string print() const
+ {
+ return ParamsProcessorBase::print();
+ }
+};
+
+using namespace Eigen;
+WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
+
+class ProcessorRangeBearing : public ProcessorBase
+{
+ public:
+ typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
+
+ ProcessorRangeBearing(ParamsProcessorBasePtr _params);
+ ~ProcessorRangeBearing() override {/* empty */}
+ void configure(SensorBasePtr _sensor) override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
+
+ protected:
+ // Implementation of pure virtuals from ProcessorBase
+ void processCapture (CaptureBasePtr _capture) override;
+ void processKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+ bool triggerInCapture (CaptureBasePtr) const override { return true;};
+ bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
+ bool voteForKeyFrame () const override {return false;}
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ bool storeKeyFrame(FrameBasePtr) override;
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ bool storeCapture(CaptureBasePtr) override;
+
+ private:
+ // control variables
+ Trf H_r_s; // transformation matrix, robot to sensor
+ std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
+
+ protected:
+ // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
+ Eigen::Vector2d observe (const Eigen::Vector2d& lmk_w) const;
+ Eigen::Vector2d invObserve (double r, double b) const;
+ private:
+ // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
+ Trf transform (const Eigen::Vector3d& _pose) const;
+ Trf transform (const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
+ Eigen::Vector2d fromSensor (const Eigen::Vector2d& lmk_s) const;
+ Eigen::Vector2d toSensor (const Eigen::Vector2d& lmk_w) const;
+ Eigen::Vector2d polar (const Eigen::Vector2d& rect) const;
+ Eigen::Vector2d rect (double range, double bearing) const;
+};
+
+inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
+{
+ H_r_s = transform(_sensor->getP()->getState(), _sensor->getO()->getState());
+}
+
+} /* namespace wolf */
+
+#endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..54507f9ac53771523b9db8716089113686bc1ca0
--- /dev/null
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -0,0 +1,46 @@
+/*
+ * SensorRangeBearing.cpp
+ *
+ * Created on: Nov 30, 2017
+ * Author: jsola
+ */
+
+#include "sensor_range_bearing.h"
+#include "core/state_block/state_angle.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(SensorRangeBearing);
+
+SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
+ SensorBase("SensorRangeBearing",
+ std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
+ std::make_shared<StateAngle>(_extrinsics(2), true),
+ nullptr,
+ _noise_std)
+{
+ assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2d");
+}
+
+SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) :
+ SensorRangeBearing(_extrinsics, Eigen::Vector2d(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
+{
+ //
+}
+
+SensorRangeBearing::~SensorRangeBearing()
+{
+ //
+}
+
+
+} /* namespace wolf */
+
+// Register in the SensorFactory
+#include "core/sensor/factory_sensor.h"
+namespace wolf
+{
+WOLF_REGISTER_SENSOR(SensorRangeBearing)
+WOLF_REGISTER_SENSOR_AUTO(SensorRangeBearing)
+} // namespace wolf
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
new file mode 100644
index 0000000000000000000000000000000000000000..70684f77951f2681d8c2741d3c5c5b09fc503a12
--- /dev/null
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -0,0 +1,54 @@
+/*
+ * SensorRangeBearing.h
+ *
+ * Created on: Nov 30, 2017
+ * Author: jsola
+ */
+
+#ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_
+#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
+
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+
+namespace wolf
+{
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorRangeBearing);
+
+struct ParamsSensorRangeBearing : public ParamsSensorBase
+{
+ double noise_range_metres_std = 0.05;
+ double noise_bearing_degrees_std = 0.5;
+ ParamsSensorRangeBearing()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ParamsSensorRangeBearing(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ noise_range_metres_std = _server.getParam<double>(prefix + _unique_name + "/noise_range_metres_std");
+ noise_bearing_degrees_std = _server.getParam<double>(prefix + _unique_name + "/noise_bearing_degrees_std");
+ }
+ std::string print() const
+ {
+ return ParamsSensorBase::print() + "\n"
+ + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(SensorRangeBearing)
+
+class SensorRangeBearing : public SensorBase
+{
+ public:
+ SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
+ SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr);
+ WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
+ ~SensorRangeBearing() override;
+
+};
+
+} /* namespace wolf */
+
+#endif /* HELLO_WOLF_SENSOR_RANGE_BEARING_H_ */
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4be898bc1a22d0af902f53e1280856cc76a67d74
--- /dev/null
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -0,0 +1,57 @@
+config:
+
+ problem:
+
+ dimension: 2 # space is 2d
+ frame_structure: "PO" # keyframes have position and orientation
+
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0]
+ O: [0]
+ $sigma:
+ P: [0.31, 0.31]
+ O: [0.31]
+ time_tolerance: 0.1
+
+ tree_manager:
+ type: "none"
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0.2
+ update_immediately: false
+ n_threads: 1
+
+ sensors:
+
+ - type: "SensorOdom2d"
+ plugin: "core"
+ name: "sen odom"
+ extrinsic:
+ pose: [0,0, 0]
+ follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
+
+ - type: "SensorRangeBearing"
+ plugin: "core"
+ name: "sen rb"
+ extrinsic:
+ pose: [1,1, 0]
+ noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
+ follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
+
+ processors:
+
+ - type: "ProcessorOdom2d"
+ plugin: "core"
+ name: "prc odom"
+ sensor_name: "sen odom" # attach processor to this sensor
+ follow: demos/hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
+
+ - type: "ProcessorRangeBearing"
+ plugin: "core"
+ name: "prc rb"
+ sensor_name: "sen rb" # attach processor to this sensor
+ follow: demos/hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..907bbfab3be6e9f66feb6c027f34fa752fd893c4
--- /dev/null
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -0,0 +1,15 @@
+#type: "ProcessorOdom2d"
+
+time_tolerance: 0.1
+unmeasured_perturbation_std: 0.001
+keyframe_vote:
+ voting_active: true
+ max_time_span: 999
+ dist_traveled: 0.95
+ angle_turned: 999
+ max_buff_length: 999
+ cov_det: 999
+apply_loss_function: true
+
+state_getter: true
+state_priority: 1
diff --git a/demos/hello_wolf/yaml/processor_range_bearing.yaml b/demos/hello_wolf/yaml/processor_range_bearing.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c5a5608a42d47e02cfe1504aec59b99fea7e56c7
--- /dev/null
+++ b/demos/hello_wolf/yaml/processor_range_bearing.yaml
@@ -0,0 +1,8 @@
+#type: "ProcessorRangeBearing"
+
+time_tolerance: 0.1
+
+keyframe_vote:
+ voting_active: true
+
+apply_loss_function: true
diff --git a/demos/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0f724c780c41c708b0b5a63a0e429cbd3319cb1e
--- /dev/null
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -0,0 +1,5 @@
+#type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+
+k_disp_to_disp: 0.1 # m^2 / m
+k_rot_to_rot: 0.1 # rad^2 / rad
+apply_loss_function: true
diff --git a/demos/hello_wolf/yaml/sensor_range_bearing.yaml b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7f81091db16c74fb479684a171256a91dd425f66
--- /dev/null
+++ b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
@@ -0,0 +1,5 @@
+#type: "SensorRangeBearing"
+
+noise_range_metres_std: 0.1
+noise_bearing_degrees_std: 0.5
+apply_loss_function: true
diff --git a/demos/input_M3500b_toro.graph b/demos/input_M3500b_toro.graph
deleted file mode 100644
index 21d4f410448f092466196e433705f6a96463e0a1..0000000000000000000000000000000000000000
--- a/demos/input_M3500b_toro.graph
+++ /dev/null
@@ -1,8953 +0,0 @@
-VERTEX2 0 0.000000 0.000000 0.000000
-VERTEX2 1 1.030390 0.011350 0.387567
-VERTEX2 2 1.991252 0.340250 0.471361
-VERTEX2 3 2.910224 0.800262 0.167817
-VERTEX2 4 2.730618 1.788506 1.475181
-VERTEX2 5 2.813120 2.801285 1.677202
-VERTEX2 6 2.697609 3.818661 1.740977
-VERTEX2 7 2.532293 4.762025 1.885102
-VERTEX2 8 2.861812 3.792586 -1.443491
-VERTEX2 9 3.005620 2.779204 -1.588453
-VERTEX2 10 3.010151 1.775618 -1.498040
-VERTEX2 11 3.100182 0.802342 -1.878003
-VERTEX2 12 3.377318 1.760265 1.280066
-VERTEX2 13 3.643665 2.714611 1.353133
-VERTEX2 14 3.839241 3.640087 1.153732
-VERTEX2 15 4.238446 4.556982 1.114735
-VERTEX2 16 5.176343 4.139078 -0.188893
-VERTEX2 17 6.146369 3.973886 0.126050
-VERTEX2 18 7.092254 4.123574 -0.094755
-VERTEX2 19 8.081760 4.048820 -0.025535
-VERTEX2 20 8.009905 3.034004 -1.535535
-VERTEX2 21 8.039875 2.035990 -1.789560
-VERTEX2 22 7.834117 1.068866 -1.615560
-VERTEX2 23 7.796816 0.080123 -1.744510
-VERTEX2 24 7.963836 1.068135 1.518927
-VERTEX2 25 8.011572 2.082374 1.270385
-VERTEX2 26 8.318896 3.068591 1.288482
-VERTEX2 27 8.602681 4.016183 1.405397
-VERTEX2 28 7.626395 4.130080 3.066433
-VERTEX2 29 6.600082 4.228693 2.906166
-VERTEX2 30 5.612239 4.448738 2.978911
-VERTEX2 31 4.632725 4.601591 2.860578
-VERTEX2 32 4.894579 5.539797 1.067367
-VERTEX2 33 5.388198 6.458157 1.152328
-VERTEX2 34 5.775578 7.370271 1.257071
-VERTEX2 35 6.054859 8.326758 1.326843
-VERTEX2 36 5.085698 8.556889 2.727556
-VERTEX2 37 4.161165 8.951055 2.843635
-VERTEX2 38 3.164181 9.241874 2.841768
-VERTEX2 39 2.200830 9.532630 2.819498
-VERTEX2 40 1.835753 8.616074 -1.942024
-VERTEX2 41 1.448809 7.710071 -1.982670
-VERTEX2 42 1.075490 6.778997 -1.751631
-VERTEX2 43 0.885198 5.811020 -1.883499
-VERTEX2 44 0.595605 4.876047 -2.046603
-VERTEX2 45 0.103124 4.009236 -2.027326
-VERTEX2 46 -0.340778 3.110313 -1.936592
-VERTEX2 47 -0.729765 2.188845 -1.953972
-VERTEX2 48 -0.363779 3.065323 1.478864
-VERTEX2 49 -0.282708 4.079553 1.652104
-VERTEX2 50 -0.392495 5.089453 1.868665
-VERTEX2 51 -0.715409 6.091060 2.097129
-VERTEX2 52 0.141156 6.576218 0.485381
-VERTEX2 53 1.048636 7.065015 -0.076897
-VERTEX2 54 2.029903 6.999232 0.143914
-VERTEX2 55 3.033679 7.129204 0.047546
-VERTEX2 56 3.076840 6.145157 -1.134105
-VERTEX2 57 3.498102 5.204006 -1.093796
-VERTEX2 58 3.939516 4.325175 -1.332336
-VERTEX2 59 4.189390 3.346256 -1.590844
-VERTEX2 60 3.180599 3.346370 3.035322
-VERTEX2 61 2.194086 3.455690 3.015793
-VERTEX2 62 1.222517 3.591341 -3.140766
-VERTEX2 63 0.214837 3.627602 2.949331
-VERTEX2 64 0.419924 4.605033 1.683679
-VERTEX2 65 0.319731 5.638263 1.852402
-VERTEX2 66 0.033163 6.587204 1.749121
-VERTEX2 67 -0.120004 7.524339 1.322594
-VERTEX2 68 -0.351359 6.573681 -1.829715
-VERTEX2 69 -0.642455 5.612504 -1.742908
-VERTEX2 70 -0.816307 4.597471 -1.565410
-VERTEX2 71 -0.798917 3.561030 -1.672027
-VERTEX2 72 -1.788209 3.657518 2.869088
-VERTEX2 73 -2.713574 3.947891 2.974705
-VERTEX2 74 -3.687422 4.100639 -3.130183
-VERTEX2 75 -4.668631 4.095408 3.084621
-VERTEX2 76 -4.716292 3.114690 -1.541281
-VERTEX2 77 -4.653173 2.102122 -1.632315
-VERTEX2 78 -4.706549 1.075511 -1.579063
-VERTEX2 79 -4.722580 0.072959 -1.559024
-VERTEX2 80 -5.701305 0.071647 3.003596
-VERTEX2 81 -6.706970 0.191929 2.811293
-VERTEX2 82 -7.637118 0.544571 2.819470
-VERTEX2 83 -8.596329 0.858474 2.876326
-VERTEX2 84 -8.849329 -0.143307 -2.099755
-VERTEX2 85 -9.329299 -1.003331 -1.967419
-VERTEX2 86 -9.702951 -1.923441 -2.127639
-VERTEX2 87 -10.209576 -2.777636 -1.882692
-VERTEX2 88 -9.219831 -3.098029 -0.391226
-VERTEX2 89 -8.294056 -3.496234 -0.490758
-VERTEX2 90 -7.403807 -3.978061 -0.273185
-VERTEX2 91 -6.406801 -4.254324 -0.323297
-VERTEX2 92 -6.112993 -3.327211 1.236186
-VERTEX2 93 -5.846213 -2.371038 0.847917
-VERTEX2 94 -5.214257 -1.633138 0.552362
-VERTEX2 95 -4.328999 -1.120253 0.641163
-VERTEX2 96 -4.972125 -0.290869 2.057058
-VERTEX2 97 -5.446247 0.634087 2.115764
-VERTEX2 98 -5.994674 1.462842 2.191564
-VERTEX2 99 -6.579563 2.321873 1.807342
-VERTEX2 100 -6.339007 1.365769 -1.200140
-VERTEX2 101 -5.938277 0.422196 -1.058120
-VERTEX2 102 -5.433076 -0.434039 -1.156309
-VERTEX2 103 -5.007648 -1.341010 -0.993775
-VERTEX2 104 -5.560871 -0.497581 1.935166
-VERTEX2 105 -5.923112 0.438655 2.214185
-VERTEX2 106 -6.502892 1.208307 2.155523
-VERTEX2 107 -7.080157 2.033462 1.779814
-VERTEX2 108 -6.058702 2.225354 -0.059028
-VERTEX2 109 -5.050614 2.190479 -0.177455
-VERTEX2 110 -4.073476 2.002032 -0.087723
-VERTEX2 111 -3.095253 1.911633 -0.317432
-VERTEX2 112 -4.027204 2.257290 2.728999
-VERTEX2 113 -4.962018 2.670202 2.876649
-VERTEX2 114 -5.946864 2.944772 2.742665
-VERTEX2 115 -6.873360 3.351060 2.562982
-VERTEX2 116 -7.402122 2.525257 -1.962823
-VERTEX2 117 -7.815686 1.604254 -2.085653
-VERTEX2 118 -8.304703 0.750686 -2.080498
-VERTEX2 119 -8.780566 -0.111932 -1.987825
-VERTEX2 120 -8.420028 0.833184 1.214542
-VERTEX2 121 -8.041270 1.770372 1.170425
-VERTEX2 122 -7.641473 2.675993 1.082664
-VERTEX2 123 -7.193666 3.594629 0.897481
-VERTEX2 124 -7.942394 4.180934 2.778443
-VERTEX2 125 -8.887600 4.522151 2.738811
-VERTEX2 126 -9.824047 4.942028 2.502684
-VERTEX2 127 -10.616986 5.489873 2.725328
-VERTEX2 128 -10.990276 4.544086 -1.853840
-VERTEX2 129 -11.284302 3.582562 -2.073823
-VERTEX2 130 -11.792429 2.713508 -2.403848
-VERTEX2 131 -12.542099 2.045875 -2.363901
-VERTEX2 132 -13.281247 2.739254 2.397721
-VERTEX2 133 -14.013813 3.425918 2.441374
-VERTEX2 134 -14.763916 4.062356 2.015649
-VERTEX2 135 -15.225598 4.937977 2.139731
-VERTEX2 136 -14.697118 4.051257 -0.907333
-VERTEX2 137 -14.098834 3.256341 -0.671228
-VERTEX2 138 -13.294698 2.626506 -0.629233
-VERTEX2 139 -12.461824 2.009320 -0.827974
-VERTEX2 140 -13.121565 2.762116 2.143666
-VERTEX2 141 -13.674950 3.611977 1.849362
-VERTEX2 142 -13.979540 4.603058 2.202883
-VERTEX2 143 -14.600228 5.413943 2.116288
-VERTEX2 144 -14.111104 4.539621 -1.180744
-VERTEX2 145 -13.721942 3.644747 -1.510919
-VERTEX2 146 -13.688654 2.653977 -1.251406
-VERTEX2 147 -13.382712 1.688366 -1.098128
-VERTEX2 148 -14.294957 1.208476 -2.772051
-VERTEX2 149 -15.213077 0.788447 -2.555129
-VERTEX2 150 -16.047363 0.232573 -2.688222
-VERTEX2 151 -16.906955 -0.231068 -2.779504
-VERTEX2 152 -16.547347 -1.197691 -1.270383
-VERTEX2 153 -16.251503 -2.136603 -1.577188
-VERTEX2 154 -16.283348 -3.120441 -1.747322
-VERTEX2 155 -16.444332 -4.065069 -1.736334
-VERTEX2 156 -16.262663 -3.102450 1.373086
-VERTEX2 157 -16.021599 -2.116269 1.321543
-VERTEX2 158 -15.755051 -1.148060 1.392938
-VERTEX2 159 -15.536178 -0.169548 1.571690
-VERTEX2 160 -15.530560 -1.184883 -1.448269
-VERTEX2 161 -15.410950 -2.154537 -1.641735
-VERTEX2 162 -15.487776 -3.179820 -1.464186
-VERTEX2 163 -15.370490 -4.137304 -1.328058
-VERTEX2 164 -15.591823 -3.206650 1.864169
-VERTEX2 165 -15.902213 -2.252785 2.285598
-VERTEX2 166 -16.500169 -1.510358 2.776517
-VERTEX2 167 -17.469859 -1.162237 3.061965
-VERTEX2 168 -17.408170 -0.156297 1.246958
-VERTEX2 169 -17.090561 0.787721 1.370210
-VERTEX2 170 -16.891144 1.735555 1.544328
-VERTEX2 171 -16.854698 2.736301 1.704668
-VERTEX2 172 -15.859446 2.877030 0.094566
-VERTEX2 173 -14.876897 2.972701 0.605420
-VERTEX2 174 -14.084730 3.536064 0.405513
-VERTEX2 175 -13.174992 3.966244 0.345965
-VERTEX2 176 -14.141331 3.615681 -3.008196
-VERTEX2 177 -15.137523 3.466594 3.139682
-VERTEX2 178 -16.114053 3.447404 3.065513
-VERTEX2 179 -17.078060 3.522618 3.009200
-VERTEX2 180 -18.074505 3.675075 -3.096362
-VERTEX2 181 -19.061605 3.612348 -3.107980
-VERTEX2 182 -20.066835 3.572224 -3.083524
-VERTEX2 183 -21.077349 3.533483 -3.108840
-VERTEX2 184 -21.112928 4.501245 1.887947
-VERTEX2 185 -21.441857 5.477734 2.092288
-VERTEX2 186 -21.956295 6.315438 1.890966
-VERTEX2 187 -22.284276 7.262290 2.035652
-VERTEX2 188 -21.405221 7.714542 0.622735
-VERTEX2 189 -20.610158 8.302866 0.501720
-VERTEX2 190 -19.714834 8.806924 0.276048
-VERTEX2 191 -18.769678 9.089214 0.244680
-VERTEX2 192 -18.962480 10.055933 1.816333
-VERTEX2 193 -19.252906 10.997580 1.811266
-VERTEX2 194 -19.505844 11.958446 1.769679
-VERTEX2 195 -19.716010 12.940807 1.702803
-VERTEX2 196 -20.698055 12.814996 2.942497
-VERTEX2 197 -21.690619 13.027066 2.767646
-VERTEX2 198 -22.616936 13.386885 2.685189
-VERTEX2 199 -23.518042 13.865162 2.538877
-VERTEX2 200 -22.923842 14.689257 0.687085
-VERTEX2 201 -22.203335 15.330032 0.648462
-VERTEX2 202 -21.414958 15.926061 1.172961
-VERTEX2 203 -21.025613 16.836778 1.259706
-VERTEX2 204 -21.991730 17.176757 2.654537
-VERTEX2 205 -22.847160 17.621059 2.580238
-VERTEX2 206 -23.687085 18.170591 2.772781
-VERTEX2 207 -24.622203 18.552782 2.864277
-VERTEX2 208 -23.666659 18.244269 -0.133495
-VERTEX2 209 -22.657272 18.110726 0.269138
-VERTEX2 210 -21.711048 18.360825 -0.010429
-VERTEX2 211 -20.696357 18.356595 0.011279
-VERTEX2 212 -20.680712 17.366163 -1.677229
-VERTEX2 213 -20.787832 16.406057 -1.756527
-VERTEX2 214 -20.958785 15.400329 -1.831467
-VERTEX2 215 -21.254654 14.446212 -1.970521
-VERTEX2 216 -20.365879 14.030614 -0.536016
-VERTEX2 217 -19.507730 13.491396 -1.027613
-VERTEX2 218 -18.967043 12.667553 -1.143318
-VERTEX2 219 -18.560706 11.763722 -1.342575
-VERTEX2 220 -18.786310 12.742941 1.851152
-VERTEX2 221 -19.034334 13.723966 1.918405
-VERTEX2 222 -19.392490 14.672523 2.018542
-VERTEX2 223 -19.830065 15.595460 2.042763
-VERTEX2 224 -19.401868 14.707980 -1.184226
-VERTEX2 225 -19.046969 13.758713 -1.208897
-VERTEX2 226 -18.658498 12.801296 -1.331535
-VERTEX2 227 -18.459371 11.861329 -1.205329
-VERTEX2 228 -19.413448 11.498882 -2.568525
-VERTEX2 229 -20.193685 10.979904 -2.711006
-VERTEX2 230 -21.100123 10.589947 -2.719673
-VERTEX2 231 -22.000392 10.195531 -2.845758
-VERTEX2 232 -22.310517 11.133700 1.782479
-VERTEX2 233 -22.553688 12.104856 1.744347
-VERTEX2 234 -22.704124 13.069514 2.051227
-VERTEX2 235 -23.176160 13.973344 2.254210
-VERTEX2 236 -23.952508 13.329324 -2.627132
-VERTEX2 237 -24.852344 12.833089 -2.392215
-VERTEX2 238 -25.575038 12.106579 -2.347890
-VERTEX2 239 -26.281339 11.412235 -2.561596
-VERTEX2 240 -25.442995 11.938767 0.752091
-VERTEX2 241 -24.739062 12.596478 0.387757
-VERTEX2 242 -23.811073 12.954074 0.449404
-VERTEX2 243 -22.868704 13.389870 0.776992
-VERTEX2 244 -23.548236 14.070097 2.094752
-VERTEX2 245 -24.002528 14.916516 1.961874
-VERTEX2 246 -24.375017 15.848894 1.721955
-VERTEX2 247 -24.506369 16.829262 1.760460
-VERTEX2 248 -24.692665 17.816534 1.402422
-VERTEX2 249 -24.563208 18.800880 1.403497
-VERTEX2 250 -24.433631 19.785756 1.114799
-VERTEX2 251 -24.003407 20.688694 1.104253
-VERTEX2 252 -24.488497 19.779949 -2.030166
-VERTEX2 253 -24.960776 18.864954 -1.969063
-VERTEX2 254 -25.367872 17.952808 -1.942093
-VERTEX2 255 -25.750968 17.010686 -2.200473
-VERTEX2 256 -25.156510 17.820732 1.082176
-VERTEX2 257 -24.686561 18.681038 1.226316
-VERTEX2 258 -24.327770 19.620228 1.261266
-VERTEX2 259 -24.025213 20.530189 1.596206
-VERTEX2 260 -23.974566 19.532951 -1.781082
-VERTEX2 261 -24.136114 18.540898 -1.442969
-VERTEX2 262 -23.984541 17.575518 -1.387614
-VERTEX2 263 -23.789615 16.624195 -1.655107
-VERTEX2 264 -24.781058 16.697874 3.018084
-VERTEX2 265 -25.807058 16.810342 2.890422
-VERTEX2 266 -26.770763 17.059833 2.931079
-VERTEX2 267 -27.737371 17.234324 -3.065280
-VERTEX2 268 -27.850293 18.232510 1.606425
-VERTEX2 269 -27.866759 19.205348 1.142981
-VERTEX2 270 -27.437813 20.138078 1.322773
-VERTEX2 271 -27.188645 21.088232 1.224607
-VERTEX2 272 -28.161855 21.414944 2.876494
-VERTEX2 273 -29.183946 21.680755 2.908594
-VERTEX2 274 -30.159972 21.912710 3.035149
-VERTEX2 275 -31.126538 22.042214 -3.061708
-VERTEX2 276 -31.026637 20.983018 -1.538298
-VERTEX2 277 -31.001493 19.995639 -1.534626
-VERTEX2 278 -30.954866 19.016533 -1.520426
-VERTEX2 279 -30.885709 18.005223 -1.520357
-VERTEX2 280 -31.852723 17.974671 -2.922086
-VERTEX2 281 -32.822635 17.784610 -2.792845
-VERTEX2 282 -33.758902 17.471625 -2.900741
-VERTEX2 283 -34.718786 17.204189 -2.965590
-VERTEX2 284 -33.715723 17.368398 0.204561
-VERTEX2 285 -32.747753 17.541222 0.088567
-VERTEX2 286 -31.792048 17.657416 0.268083
-VERTEX2 287 -30.833169 17.965442 0.042939
-VERTEX2 288 -30.845559 18.948231 1.584225
-VERTEX2 289 -30.859606 19.943312 1.439986
-VERTEX2 290 -30.711181 20.962963 1.425094
-VERTEX2 291 -30.553537 21.916528 1.424958
-VERTEX2 292 -29.570376 21.774152 -0.400351
-VERTEX2 293 -28.618566 21.378466 -0.342169
-VERTEX2 294 -27.713591 21.046243 -0.447305
-VERTEX2 295 -26.844596 20.581832 -0.410470
-VERTEX2 296 -26.414892 21.513689 1.368622
-VERTEX2 297 -26.212176 22.508128 1.591116
-VERTEX2 298 -26.199926 23.472073 1.582350
-VERTEX2 299 -26.207550 24.474491 1.601613
-VERTEX2 300 -25.212264 24.484334 0.139338
-VERTEX2 301 -24.249228 24.586008 0.184542
-VERTEX2 302 -23.287743 24.751484 0.489173
-VERTEX2 303 -22.415858 25.184826 0.506780
-VERTEX2 304 -21.928814 24.327255 -0.752122
-VERTEX2 305 -21.203013 23.644699 -0.736970
-VERTEX2 306 -20.458592 22.978533 -0.802819
-VERTEX2 307 -19.783053 22.245855 -1.092165
-VERTEX2 308 -18.894420 22.713338 0.593442
-VERTEX2 309 -18.062331 23.284351 0.623392
-VERTEX2 310 -17.249969 23.888061 0.773216
-VERTEX2 311 -16.514323 24.614295 0.601936
-VERTEX2 312 -17.091751 25.450492 2.522046
-VERTEX2 313 -17.910996 26.002916 2.713211
-VERTEX2 314 -18.793979 26.410550 2.792362
-VERTEX2 315 -19.755323 26.791135 2.752671
-VERTEX2 316 -19.361236 27.705800 1.293336
-VERTEX2 317 -19.070602 28.699975 1.291274
-VERTEX2 318 -18.787070 29.634945 1.517927
-VERTEX2 319 -18.773418 30.681906 1.267665
-VERTEX2 320 -17.811680 30.385698 -0.315341
-VERTEX2 321 -16.871677 30.117251 -0.032998
-VERTEX2 322 -15.857235 30.083278 0.374868
-VERTEX2 323 -14.941606 30.434193 0.383545
-VERTEX2 324 -15.310797 31.378432 2.025716
-VERTEX2 325 -15.761280 32.268916 2.090417
-VERTEX2 326 -16.268362 33.187650 1.846686
-VERTEX2 327 -16.521659 34.130311 1.695316
-VERTEX2 328 -16.436656 33.120198 -1.419293
-VERTEX2 329 -16.293856 32.122259 -1.530475
-VERTEX2 330 -16.261238 31.141950 -1.523017
-VERTEX2 331 -16.203834 30.155441 -1.397115
-VERTEX2 332 -17.150917 29.957680 -2.794353
-VERTEX2 333 -18.095953 29.593277 -3.077981
-VERTEX2 334 -19.110759 29.550074 -3.133438
-VERTEX2 335 -20.127039 29.500611 3.111898
-VERTEX2 336 -20.122429 28.463567 -1.314113
-VERTEX2 337 -19.851326 27.508602 -1.247625
-VERTEX2 338 -19.563782 26.589634 -1.473903
-VERTEX2 339 -19.505023 25.561758 -1.590506
-VERTEX2 340 -18.504968 25.555867 -0.152862
-VERTEX2 341 -17.513645 25.395319 -0.225236
-VERTEX2 342 -16.541087 25.176417 -0.046385
-VERTEX2 343 -15.531705 25.129767 -0.036579
-VERTEX2 344 -15.579685 24.092948 -1.673546
-VERTEX2 345 -15.669699 23.074740 -1.561938
-VERTEX2 346 -15.658146 22.049163 -1.449302
-VERTEX2 347 -15.536726 21.046175 -1.919938
-VERTEX2 348 -14.623066 20.748852 -0.411377
-VERTEX2 349 -13.721153 20.356289 0.000868
-VERTEX2 350 -12.685953 20.356979 0.048307
-VERTEX2 351 -11.683690 20.368288 -0.126139
-VERTEX2 352 -12.682184 20.489656 2.979624
-VERTEX2 353 -13.677894 20.650529 -3.077333
-VERTEX2 354 -14.659937 20.573661 -3.140529
-VERTEX2 355 -15.679742 20.549561 -3.037723
-VERTEX2 356 -14.683938 20.652983 0.109406
-VERTEX2 357 -13.674552 20.787418 0.105872
-VERTEX2 358 -12.676715 20.907500 0.026727
-VERTEX2 359 -11.717615 20.930892 -0.041171
-VERTEX2 360 -12.780513 21.006285 3.060229
-VERTEX2 361 -13.776172 21.074126 -3.088332
-VERTEX2 362 -14.833454 20.976684 -2.869917
-VERTEX2 363 -15.789459 20.702276 -3.010874
-VERTEX2 364 -14.772731 20.843021 0.108311
-VERTEX2 365 -13.738000 20.966177 0.062663
-VERTEX2 366 -12.765060 21.025360 -0.140458
-VERTEX2 367 -11.756763 20.902841 0.145634
-VERTEX2 368 -11.874435 21.901425 1.718155
-VERTEX2 369 -11.992194 22.886772 1.691512
-VERTEX2 370 -12.112003 23.899696 2.069420
-VERTEX2 371 -12.549003 24.737287 2.136300
-VERTEX2 372 -13.412978 24.215939 -2.560239
-VERTEX2 373 -14.272506 23.633982 -2.598453
-VERTEX2 374 -15.138594 23.101829 -2.624294
-VERTEX2 375 -15.987415 22.596680 -2.847573
-VERTEX2 376 -16.266130 23.542865 1.943140
-VERTEX2 377 -16.615002 24.430564 2.310877
-VERTEX2 378 -17.302527 25.111326 1.807318
-VERTEX2 379 -17.552019 26.091822 2.025797
-VERTEX2 380 -18.435439 25.645761 -2.549154
-VERTEX2 381 -19.284700 25.104166 -2.265513
-VERTEX2 382 -19.935291 24.346297 -2.174276
-VERTEX2 383 -20.453284 23.507435 -2.100620
-VERTEX2 384 -19.550593 23.024985 -0.347440
-VERTEX2 385 -18.627391 22.690096 -0.507490
-VERTEX2 386 -17.742141 22.230916 -1.117704
-VERTEX2 387 -17.299415 21.338528 -1.118464
-VERTEX2 388 -16.415834 21.736633 0.368077
-VERTEX2 389 -15.478665 22.091425 0.615551
-VERTEX2 390 -14.673064 22.683891 0.293672
-VERTEX2 391 -13.718242 22.991441 0.069516
-VERTEX2 392 -13.664056 21.990257 -1.488448
-VERTEX2 393 -13.569653 21.012617 -1.442022
-VERTEX2 394 -13.468346 20.005619 -1.263206
-VERTEX2 395 -13.136862 19.048341 -1.138463
-VERTEX2 396 -14.063296 18.664546 -2.561466
-VERTEX2 397 -14.900500 18.148289 -2.517709
-VERTEX2 398 -15.713285 17.579067 -2.521064
-VERTEX2 399 -16.505084 16.975317 -2.344483
-VERTEX2 400 -17.250673 17.670446 2.349200
-VERTEX2 401 -17.973164 18.416234 1.952487
-VERTEX2 402 -18.378596 19.335994 2.292196
-VERTEX2 403 -19.031151 20.102624 2.459811
-VERTEX2 404 -18.244488 19.447501 -0.609020
-VERTEX2 405 -17.414754 18.875738 -0.697919
-VERTEX2 406 -16.649696 18.218236 -0.659947
-VERTEX2 407 -15.874116 17.632625 -0.399338
-VERTEX2 408 -16.272393 16.728955 -1.753094
-VERTEX2 409 -16.452577 15.754890 -1.547512
-VERTEX2 410 -16.449708 14.749444 -1.727811
-VERTEX2 411 -16.613910 13.786356 -1.626108
-VERTEX2 412 -15.618538 13.705920 -0.167434
-VERTEX2 413 -14.652308 13.531853 -0.337572
-VERTEX2 414 -13.738986 13.207403 -0.707338
-VERTEX2 415 -12.976879 12.534368 -0.568182
-VERTEX2 416 -13.820417 13.080415 2.820491
-VERTEX2 417 -14.741840 13.423998 2.414205
-VERTEX2 418 -15.490289 14.116174 2.161891
-VERTEX2 419 -16.061508 14.941094 2.562212
-VERTEX2 420 -16.609923 14.047943 -2.246661
-VERTEX2 421 -17.245633 13.307580 -1.987376
-VERTEX2 422 -17.610010 12.428051 -2.085491
-VERTEX2 423 -18.149950 11.577135 -1.851112
-VERTEX2 424 -18.412469 10.633516 -1.974825
-VERTEX2 425 -18.755599 9.711754 -1.894198
-VERTEX2 426 -19.099267 8.805256 -2.166532
-VERTEX2 427 -19.678991 7.992049 -2.424218
-VERTEX2 428 -19.000363 7.220042 -0.711701
-VERTEX2 429 -18.264029 6.592860 -0.741861
-VERTEX2 430 -17.501109 5.902061 -0.720468
-VERTEX2 431 -16.775464 5.258472 -0.636406
-VERTEX2 432 -17.328117 4.468537 -1.858724
-VERTEX2 433 -17.573521 3.499695 -1.996701
-VERTEX2 434 -17.967868 2.582570 -2.318262
-VERTEX2 435 -18.649520 1.838849 -2.508998
-VERTEX2 436 -19.202891 2.664836 2.440652
-VERTEX2 437 -19.929882 3.261654 2.256923
-VERTEX2 438 -20.552150 4.032619 2.276646
-VERTEX2 439 -21.189253 4.751438 2.465761
-VERTEX2 440 -21.806618 3.969003 -1.757916
-VERTEX2 441 -21.993608 2.985420 -1.689646
-VERTEX2 442 -22.087518 1.987112 -1.827748
-VERTEX2 443 -22.332824 0.998320 -1.757323
-VERTEX2 444 -22.179704 1.938307 1.420639
-VERTEX2 445 -22.030878 2.933593 1.785414
-VERTEX2 446 -22.267045 3.922656 1.499150
-VERTEX2 447 -22.187525 4.897650 1.798230
-VERTEX2 448 -21.226307 5.117001 0.532021
-VERTEX2 449 -20.333283 5.649052 0.795018
-VERTEX2 450 -19.650671 6.385350 0.828914
-VERTEX2 451 -18.957795 7.106604 1.042599
-VERTEX2 452 -19.502770 6.237899 -2.462941
-VERTEX2 453 -20.292209 5.602520 -2.643122
-VERTEX2 454 -21.169352 5.096578 -2.412941
-VERTEX2 455 -21.918773 4.434609 -2.169889
-VERTEX2 456 -21.299985 5.237530 0.758735
-VERTEX2 457 -20.575050 5.932278 0.931564
-VERTEX2 458 -19.973310 6.732790 0.816739
-VERTEX2 459 -19.270351 7.467130 0.478386
-VERTEX2 460 -20.168559 7.017369 -2.366010
-VERTEX2 461 -20.899894 6.342435 -2.406647
-VERTEX2 462 -21.667761 5.627295 -2.270570
-VERTEX2 463 -22.317192 4.869584 -2.648335
-VERTEX2 464 -21.866726 3.997978 -1.025315
-VERTEX2 465 -21.391003 3.156323 -0.585419
-VERTEX2 466 -20.521195 2.621435 -0.554247
-VERTEX2 467 -19.670310 2.085784 -0.318248
-VERTEX2 468 -19.967642 1.140859 -2.014677
-VERTEX2 469 -20.397118 0.219134 -1.838017
-VERTEX2 470 -20.625483 -0.707260 -1.947845
-VERTEX2 471 -21.009987 -1.622358 -1.791265
-VERTEX2 472 -20.798678 -0.661041 1.336920
-VERTEX2 473 -20.576881 0.332753 1.410626
-VERTEX2 474 -20.382149 1.337477 1.511450
-VERTEX2 475 -20.324333 2.360619 1.321074
-VERTEX2 476 -20.599318 1.380825 -1.829049
-VERTEX2 477 -20.831614 0.410921 -1.985379
-VERTEX2 478 -21.216826 -0.536395 -1.811032
-VERTEX2 479 -21.441722 -1.505483 -1.794099
-VERTEX2 480 -21.206896 -0.515404 1.415340
-VERTEX2 481 -21.036479 0.447981 1.589547
-VERTEX2 482 -21.060502 1.436715 1.740469
-VERTEX2 483 -21.210533 2.466692 1.828819
-VERTEX2 484 -22.213051 2.236768 -2.756868
-VERTEX2 485 -23.196515 1.844991 -2.894675
-VERTEX2 486 -24.141737 1.586684 -2.487177
-VERTEX2 487 -24.954276 0.961646 -2.130557
-VERTEX2 488 -25.763062 1.440981 3.067430
-VERTEX2 489 -26.742011 1.517853 2.739579
-VERTEX2 490 -27.679575 1.910248 2.790705
-VERTEX2 491 -28.621987 2.249353 2.730696
-VERTEX2 492 -28.182167 3.138977 1.018134
-VERTEX2 493 -27.668341 3.990238 1.265917
-VERTEX2 494 -27.377018 4.910071 1.307074
-VERTEX2 495 -27.146998 5.861875 0.976342
-VERTEX2 496 -27.712445 5.077445 -1.990385
-VERTEX2 497 -28.155912 4.200223 -2.213285
-VERTEX2 498 -28.760480 3.434850 -1.907334
-VERTEX2 499 -29.129893 2.495928 -2.205485
-VERTEX2 500 -28.550441 3.302866 0.984068
-VERTEX2 501 -27.989686 4.113937 0.975372
-VERTEX2 502 -27.430807 4.923794 0.938562
-VERTEX2 503 -26.830772 5.739156 1.084566
-VERTEX2 504 -27.290611 4.878982 -2.256394
-VERTEX2 505 -27.877087 4.142893 -2.232520
-VERTEX2 506 -28.482222 3.347096 -1.839713
-VERTEX2 507 -28.790829 2.408644 -1.704933
-VERTEX2 508 -29.759230 2.528900 2.944743
-VERTEX2 509 -30.740862 2.729154 2.829828
-VERTEX2 510 -31.717662 3.054570 3.076870
-VERTEX2 511 -32.743682 3.122421 3.063340
-VERTEX2 512 -32.655120 4.130182 1.767108
-VERTEX2 513 -32.866388 5.099467 1.925937
-VERTEX2 514 -33.224313 6.071271 1.848601
-VERTEX2 515 -33.515655 7.026753 2.029640
-VERTEX2 516 -33.099491 6.132085 -1.420624
-VERTEX2 517 -32.978005 5.178771 -1.135977
-VERTEX2 518 -32.521746 4.257600 -1.193254
-VERTEX2 519 -32.104637 3.295645 -1.120200
-VERTEX2 520 -31.213537 3.703656 0.342000
-VERTEX2 521 -30.298490 4.076482 0.392517
-VERTEX2 522 -29.356351 4.432368 0.700886
-VERTEX2 523 -28.567328 5.083144 0.621117
-VERTEX2 524 -29.388438 4.560119 -2.462113
-VERTEX2 525 -30.132687 3.962121 -2.627543
-VERTEX2 526 -31.004439 3.445974 -2.644302
-VERTEX2 527 -31.897091 2.984054 -2.594952
-VERTEX2 528 -31.019157 3.498102 0.600091
-VERTEX2 529 -30.185871 4.052878 0.839089
-VERTEX2 530 -29.494616 4.794596 1.064005
-VERTEX2 531 -29.006844 5.710429 1.281981
-VERTEX2 532 -29.985726 5.942231 2.830659
-VERTEX2 533 -30.980130 6.246921 2.800161
-VERTEX2 534 -31.924534 6.567107 3.103057
-VERTEX2 535 -32.926487 6.606062 2.987611
-VERTEX2 536 -31.942455 6.465900 -0.077062
-VERTEX2 537 -30.943035 6.382171 -0.163208
-VERTEX2 538 -30.003813 6.234700 0.035680
-VERTEX2 539 -29.001666 6.243390 0.112820
-VERTEX2 540 -30.011579 6.175108 -3.045915
-VERTEX2 541 -31.012787 6.045400 -2.797799
-VERTEX2 542 -31.954065 5.665579 -2.596319
-VERTEX2 543 -32.849992 5.127365 -2.529990
-VERTEX2 544 -32.272636 4.338427 -1.387811
-VERTEX2 545 -32.101116 3.353991 -1.525915
-VERTEX2 546 -32.127174 2.385859 -1.732185
-VERTEX2 547 -32.321285 1.401448 -1.781032
-VERTEX2 548 -31.309715 1.204011 -0.342710
-VERTEX2 549 -30.398498 0.871988 -0.810198
-VERTEX2 550 -29.668351 0.155626 -0.914601
-VERTEX2 551 -29.057350 -0.656514 -1.248519
-VERTEX2 552 -28.121262 -0.339577 0.209222
-VERTEX2 553 -27.161539 -0.100233 0.223293
-VERTEX2 554 -26.192698 0.098922 0.557667
-VERTEX2 555 -25.311928 0.651075 0.303836
-VERTEX2 556 -26.229332 0.317298 -2.873733
-VERTEX2 557 -27.188202 0.042173 -2.605969
-VERTEX2 558 -28.076680 -0.487932 -2.643847
-VERTEX2 559 -28.963979 -0.961586 -2.374395
-VERTEX2 560 -28.284544 -1.686894 -0.839689
-VERTEX2 561 -27.630394 -2.370843 -1.032600
-VERTEX2 562 -27.091456 -3.271781 -1.172508
-VERTEX2 563 -26.738456 -4.184625 -0.937441
-VERTEX2 564 -26.167809 -4.980723 -1.042063
-VERTEX2 565 -25.623331 -5.834190 -0.741390
-VERTEX2 566 -24.864774 -6.483634 -0.882074
-VERTEX2 567 -24.219876 -7.238341 -0.960344
-VERTEX2 568 -24.827017 -6.400029 2.198880
-VERTEX2 569 -25.405838 -5.595431 2.373350
-VERTEX2 570 -26.145986 -4.888214 2.635718
-VERTEX2 571 -27.003747 -4.349879 2.562253
-VERTEX2 572 -27.577085 -5.209467 -1.924249
-VERTEX2 573 -27.953675 -6.146254 -2.004812
-VERTEX2 574 -28.345867 -7.048983 -1.792389
-VERTEX2 575 -28.582509 -8.029457 -1.846765
-VERTEX2 576 -27.632465 -8.293294 -0.286949
-VERTEX2 577 -26.664444 -8.562880 -0.729533
-VERTEX2 578 -25.945928 -9.218977 -0.737879
-VERTEX2 579 -25.186603 -9.887928 -0.812969
-VERTEX2 580 -24.447694 -9.205421 0.864706
-VERTEX2 581 -23.822267 -8.476625 1.125249
-VERTEX2 582 -23.432926 -7.571935 1.175611
-VERTEX2 583 -23.023444 -6.649013 1.105437
-VERTEX2 584 -23.883367 -6.198703 2.984093
-VERTEX2 585 -24.875447 -6.102336 2.674854
-VERTEX2 586 -25.758153 -5.683277 2.556369
-VERTEX2 587 -26.632551 -5.124768 2.721536
-VERTEX2 588 -26.213215 -4.215196 0.989351
-VERTEX2 589 -25.655540 -3.372845 1.162059
-VERTEX2 590 -25.268241 -2.424964 1.010860
-VERTEX2 591 -24.742106 -1.578246 0.975637
-VERTEX2 592 -23.930569 -2.155272 -0.783569
-VERTEX2 593 -23.211705 -2.826366 -0.944483
-VERTEX2 594 -22.641351 -3.649132 -1.036480
-VERTEX2 595 -22.133512 -4.520567 -0.916615
-VERTEX2 596 -22.903336 -5.135797 -2.850886
-VERTEX2 597 -23.888371 -5.450858 -2.829541
-VERTEX2 598 -24.845836 -5.777313 -3.105003
-VERTEX2 599 -25.863381 -5.837057 3.116710
-VERTEX2 600 -25.855389 -4.863095 1.564702
-VERTEX2 601 -25.818161 -3.858023 1.820753
-VERTEX2 602 -26.055364 -2.872811 1.598703
-VERTEX2 603 -26.079902 -1.879244 1.529565
-VERTEX2 604 -27.093084 -1.875622 -2.965774
-VERTEX2 605 -28.099280 -2.016066 3.110026
-VERTEX2 606 -29.081260 -1.986410 2.794126
-VERTEX2 607 -30.027272 -1.640132 2.896114
-VERTEX2 608 -30.255976 -2.633832 -1.923474
-VERTEX2 609 -30.575602 -3.585003 -1.595835
-VERTEX2 610 -30.578103 -4.587704 -1.826846
-VERTEX2 611 -30.828311 -5.546189 -1.828801
-VERTEX2 612 -30.583819 -4.590733 1.346393
-VERTEX2 613 -30.326582 -3.622047 1.347768
-VERTEX2 614 -30.080890 -2.590008 1.293773
-VERTEX2 615 -29.818139 -1.582975 1.069354
-VERTEX2 616 -30.307617 -2.468459 -2.190022
-VERTEX2 617 -30.888708 -3.272526 -2.083694
-VERTEX2 618 -31.378799 -4.120603 -1.836953
-VERTEX2 619 -31.614431 -5.057194 -1.811433
-VERTEX2 620 -32.570970 -4.868461 3.016512
-VERTEX2 621 -33.581161 -4.775631 3.103766
-VERTEX2 622 -34.620596 -4.769597 -2.796990
-VERTEX2 623 -35.571654 -5.082379 -2.925754
-VERTEX2 624 -34.581546 -4.898059 0.208318
-VERTEX2 625 -33.562555 -4.704266 -0.035390
-VERTEX2 626 -32.545198 -4.745829 -0.180809
-VERTEX2 627 -31.575845 -4.954178 -0.164317
-VERTEX2 628 -31.386576 -3.992265 1.509407
-VERTEX2 629 -31.343067 -2.985869 1.645277
-VERTEX2 630 -31.407184 -2.010899 1.754959
-VERTEX2 631 -31.600010 -1.037248 1.591672
-VERTEX2 632 -32.591377 -1.100597 -3.000732
-VERTEX2 633 -33.561779 -1.207953 -3.081635
-VERTEX2 634 -34.552629 -1.265358 -3.135169
-VERTEX2 635 -35.498103 -1.276753 2.651016
-VERTEX2 636 -34.634820 -1.730863 -0.720605
-VERTEX2 637 -33.881639 -2.381980 -1.103494
-VERTEX2 638 -33.446373 -3.286486 -1.158273
-VERTEX2 639 -33.050469 -4.218123 -1.325986
-VERTEX2 640 -34.002630 -4.459821 -2.981668
-VERTEX2 641 -34.995765 -4.604743 -2.950809
-VERTEX2 642 -36.022844 -4.784972 3.061651
-VERTEX2 643 -36.992556 -4.657848 -3.064559
-VERTEX2 644 -35.975121 -4.550356 0.245151
-VERTEX2 645 -35.001416 -4.339439 0.212172
-VERTEX2 646 -34.081769 -4.134862 0.171781
-VERTEX2 647 -33.095717 -3.976794 0.129961
-VERTEX2 648 -33.202143 -2.966294 1.821601
-VERTEX2 649 -33.425204 -1.990030 1.846116
-VERTEX2 650 -33.710101 -1.007372 1.690211
-VERTEX2 651 -33.871130 -0.016499 1.479284
-VERTEX2 652 -34.930151 0.076839 2.971502
-VERTEX2 653 -35.871207 0.235759 2.870323
-VERTEX2 654 -36.867936 0.462066 2.738568
-VERTEX2 655 -37.749890 0.851688 2.788381
-VERTEX2 656 -36.821935 0.483976 -0.462244
-VERTEX2 657 -35.969003 0.007355 -0.330401
-VERTEX2 658 -35.052174 -0.326195 -0.428885
-VERTEX2 659 -34.139275 -0.729630 -0.314324
-VERTEX2 660 -33.803980 0.238993 1.181810
-VERTEX2 661 -33.430113 1.155331 1.292367
-VERTEX2 662 -33.165663 2.110432 1.063079
-VERTEX2 663 -32.695910 2.931043 0.755896
-VERTEX2 664 -32.000572 2.206308 -1.001497
-VERTEX2 665 -31.444866 1.340560 -0.537064
-VERTEX2 666 -30.601848 0.797346 -0.189246
-VERTEX2 667 -29.634707 0.603379 -0.062455
-VERTEX2 668 -30.600792 0.664445 3.065230
-VERTEX2 669 -31.609103 0.718675 2.984330
-VERTEX2 670 -32.596995 0.876699 2.948463
-VERTEX2 671 -33.602512 1.099995 2.984258
-VERTEX2 672 -32.628725 0.939771 0.156403
-VERTEX2 673 -31.623938 1.102758 0.337992
-VERTEX2 674 -30.692332 1.440964 0.631556
-VERTEX2 675 -29.915305 2.020480 0.415562
-VERTEX2 676 -30.841775 1.613519 3.087394
-VERTEX2 677 -31.832269 1.688048 3.020292
-VERTEX2 678 -32.809206 1.801274 2.750877
-VERTEX2 679 -33.693732 2.196957 2.821912
-VERTEX2 680 -34.001817 1.247929 -2.179898
-VERTEX2 681 -34.511004 0.463367 -2.066739
-VERTEX2 682 -34.995796 -0.443646 -2.189136
-VERTEX2 683 -35.559515 -1.259439 -2.126458
-VERTEX2 684 -36.437483 -0.722799 2.545502
-VERTEX2 685 -37.278668 -0.163194 2.806796
-VERTEX2 686 -38.228950 0.132148 2.593146
-VERTEX2 687 -39.076748 0.634615 2.570634
-VERTEX2 688 -39.634301 -0.186637 -2.467472
-VERTEX2 689 -40.387846 -0.798654 -2.757188
-VERTEX2 690 -41.331054 -1.165222 -2.468788
-VERTEX2 691 -42.091215 -1.783659 -2.537924
-VERTEX2 692 -42.666185 -0.988363 2.114567
-VERTEX2 693 -43.211913 -0.147584 1.988701
-VERTEX2 694 -43.631503 0.752285 2.059776
-VERTEX2 695 -44.103290 1.644421 2.043286
-VERTEX2 696 -43.232784 2.096206 0.483337
-VERTEX2 697 -42.320559 2.553135 0.314032
-VERTEX2 698 -41.367649 2.815936 0.259968
-VERTEX2 699 -40.406527 3.026476 0.572880
-VERTEX2 700 -40.948296 3.905060 2.325591
-VERTEX2 701 -41.653282 4.627283 2.343951
-VERTEX2 702 -42.347874 5.327746 2.137829
-VERTEX2 703 -42.889333 6.176254 2.189128
-VERTEX2 704 -43.721611 5.592962 -2.950414
-VERTEX2 705 -44.676140 5.439317 -2.769627
-VERTEX2 706 -45.649961 5.076037 -2.800748
-VERTEX2 707 -46.639276 4.745753 -2.798814
-VERTEX2 708 -46.320082 3.800693 -0.791322
-VERTEX2 709 -45.613296 3.113216 -0.645733
-VERTEX2 710 -44.793965 2.530226 -0.429661
-VERTEX2 711 -43.859253 2.138913 -0.275658
-VERTEX2 712 -44.082812 1.187371 -1.928221
-VERTEX2 713 -44.421607 0.256124 -1.940586
-VERTEX2 714 -44.797597 -0.668041 -1.943872
-VERTEX2 715 -45.161553 -1.577137 -2.015638
-VERTEX2 716 -44.735359 -0.704126 1.101788
-VERTEX2 717 -44.295228 0.183918 0.729237
-VERTEX2 718 -43.536145 0.854283 0.734081
-VERTEX2 719 -42.782131 1.530222 0.682725
-VERTEX2 720 -43.407132 2.307568 1.913181
-VERTEX2 721 -43.728812 3.252007 1.808914
-VERTEX2 722 -43.947561 4.223672 1.548151
-VERTEX2 723 -43.896174 5.240969 1.587423
-VERTEX2 724 -43.886770 4.240747 -1.798565
-VERTEX2 725 -44.100691 3.286095 -1.734794
-VERTEX2 726 -44.265400 2.319604 -1.790133
-VERTEX2 727 -44.493515 1.355622 -1.707217
-VERTEX2 728 -43.466889 1.205677 0.015346
-VERTEX2 729 -42.465290 1.244762 -0.205379
-VERTEX2 730 -41.490399 1.035221 -0.243654
-VERTEX2 731 -40.506968 0.833663 -0.593152
-VERTEX2 732 -39.964436 1.636958 0.767306
-VERTEX2 733 -39.232278 2.312201 0.780288
-VERTEX2 734 -38.528350 3.022089 0.626615
-VERTEX2 735 -37.729237 3.633841 0.813762
-VERTEX2 736 -38.494054 4.354539 2.156327
-VERTEX2 737 -39.111591 5.173603 2.034511
-VERTEX2 738 -39.547606 6.053304 2.352964
-VERTEX2 739 -40.268957 6.778321 2.523266
-VERTEX2 740 -39.431096 6.214780 -0.972123
-VERTEX2 741 -38.837514 5.410774 -0.956395
-VERTEX2 742 -38.226068 4.621180 -0.921206
-VERTEX2 743 -37.623312 3.827416 -1.147304
-VERTEX2 744 -38.532356 3.392952 -2.355318
-VERTEX2 745 -39.217662 2.683886 -2.493331
-VERTEX2 746 -40.018587 2.044561 -2.338311
-VERTEX2 747 -40.706455 1.340014 -2.139286
-VERTEX2 748 -40.180816 2.194950 1.093920
-VERTEX2 749 -39.726760 3.122331 1.106965
-VERTEX2 750 -39.341339 4.049055 1.217008
-VERTEX2 751 -39.005339 4.982375 1.019984
-VERTEX2 752 -39.544291 4.182994 -2.317176
-VERTEX2 753 -40.238879 3.491461 -2.380479
-VERTEX2 754 -40.969938 2.823528 -2.882837
-VERTEX2 755 -41.896890 2.526895 -3.009341
-VERTEX2 756 -41.768145 1.579209 -0.968297
-VERTEX2 757 -41.201470 0.737099 -0.909801
-VERTEX2 758 -40.598632 -0.039637 -0.843153
-VERTEX2 759 -39.919806 -0.719287 -0.849886
-VERTEX2 760 -39.160646 -0.094943 0.717066
-VERTEX2 761 -38.407854 0.518247 0.572987
-VERTEX2 762 -37.551441 1.043861 0.852981
-VERTEX2 763 -36.858696 1.788856 0.776050
-VERTEX2 764 -37.581562 1.101872 -2.307267
-VERTEX2 765 -38.261513 0.334339 -2.483635
-VERTEX2 766 -39.061738 -0.294420 -2.647881
-VERTEX2 767 -39.954920 -0.747307 -2.764538
-VERTEX2 768 -40.871538 -1.130106 -2.924744
-VERTEX2 769 -41.860983 -1.318517 3.032802
-VERTEX2 770 -42.843269 -1.231207 3.035018
-VERTEX2 771 -43.857142 -1.151778 2.748712
-VERTEX2 772 -43.455964 -0.215646 1.177988
-VERTEX2 773 -43.120288 0.675487 1.293765
-VERTEX2 774 -42.836623 1.614496 1.430994
-VERTEX2 775 -42.689626 2.609308 1.637997
-VERTEX2 776 -41.689585 2.672998 0.461425
-VERTEX2 777 -40.786764 3.106053 0.324667
-VERTEX2 778 -39.871140 3.431402 0.142001
-VERTEX2 779 -38.872199 3.547733 0.345840
-VERTEX2 780 -38.510762 2.644349 -0.941203
-VERTEX2 781 -37.860409 1.860844 -1.061258
-VERTEX2 782 -37.376636 0.977082 -1.182773
-VERTEX2 783 -37.019673 0.019061 -1.307553
-VERTEX2 784 -37.999171 -0.232634 -3.138235
-VERTEX2 785 -38.994512 -0.210004 -3.094430
-VERTEX2 786 -40.001270 -0.319916 -2.972070
-VERTEX2 787 -40.988269 -0.413397 -2.504720
-VERTEX2 788 -40.407775 -1.244471 -1.017372
-VERTEX2 789 -39.882895 -2.066062 -1.068881
-VERTEX2 790 -39.431762 -2.956345 -1.008582
-VERTEX2 791 -38.895163 -3.793303 -0.930602
-VERTEX2 792 -39.636532 -4.350474 -2.058682
-VERTEX2 793 -40.083964 -5.255556 -2.002446
-VERTEX2 794 -40.497035 -6.188315 -1.606266
-VERTEX2 795 -40.479163 -7.186994 -1.388166
-VERTEX2 796 -39.501656 -6.985580 0.504788
-VERTEX2 797 -38.630937 -6.491662 0.333772
-VERTEX2 798 -37.655185 -6.169858 0.610131
-VERTEX2 799 -36.840096 -5.610023 0.401610
-VERTEX2 800 -36.422937 -6.572062 -0.903253
-VERTEX2 801 -35.781507 -7.354963 -1.151987
-VERTEX2 802 -35.369718 -8.286674 -0.556657
-VERTEX2 803 -34.525082 -8.780736 -0.400845
-VERTEX2 804 -34.127039 -7.859191 1.198738
-VERTEX2 805 -33.736316 -6.958176 1.191372
-VERTEX2 806 -33.360827 -6.009102 1.208662
-VERTEX2 807 -32.998357 -5.044311 1.230869
-VERTEX2 808 -32.035138 -5.353464 -0.293602
-VERTEX2 809 -31.080692 -5.675703 -0.225116
-VERTEX2 810 -30.121334 -5.875236 -0.436359
-VERTEX2 811 -29.194429 -6.316174 -0.524828
-VERTEX2 812 -28.719843 -5.476363 1.144693
-VERTEX2 813 -28.278837 -4.535744 1.160747
-VERTEX2 814 -27.877266 -3.635539 1.058824
-VERTEX2 815 -27.388776 -2.726881 1.211611
-VERTEX2 816 -28.283128 -2.415033 2.923030
-VERTEX2 817 -29.264177 -2.228396 2.898766
-VERTEX2 818 -30.211801 -1.989273 2.705043
-VERTEX2 819 -31.145634 -1.539843 2.917443
-VERTEX2 820 -30.197714 -1.803863 -0.280420
-VERTEX2 821 -29.244228 -2.071154 -0.410092
-VERTEX2 822 -28.347655 -2.481366 -0.040411
-VERTEX2 823 -27.354690 -2.533343 0.205250
-VERTEX2 824 -27.138464 -3.527164 -1.568099
-VERTEX2 825 -27.182646 -4.499645 -1.229092
-VERTEX2 826 -26.813121 -5.469354 -1.583141
-VERTEX2 827 -26.812729 -6.492416 -1.481826
-VERTEX2 828 -26.730084 -7.443891 -1.259417
-VERTEX2 829 -26.414206 -8.388932 -1.307109
-VERTEX2 830 -26.170832 -9.322658 -1.659756
-VERTEX2 831 -26.239724 -10.297627 -1.761652
-VERTEX2 832 -26.002034 -9.311073 1.275345
-VERTEX2 833 -25.689339 -8.334654 1.404496
-VERTEX2 834 -25.536387 -7.363369 1.327377
-VERTEX2 835 -25.309488 -6.434387 1.293650
-VERTEX2 836 -26.257289 -6.166902 3.033525
-VERTEX2 837 -27.260481 -6.049428 2.893632
-VERTEX2 838 -28.230577 -5.774624 2.670676
-VERTEX2 839 -29.143454 -5.327070 2.584649
-VERTEX2 840 -28.585223 -4.449294 0.732644
-VERTEX2 841 -27.830549 -3.777892 0.527488
-VERTEX2 842 -26.977521 -3.295653 0.481033
-VERTEX2 843 -26.070201 -2.852045 0.332780
-VERTEX2 844 -26.430950 -1.901577 1.704115
-VERTEX2 845 -26.551045 -0.948067 1.621878
-VERTEX2 846 -26.628552 0.083801 1.878000
-VERTEX2 847 -26.940502 1.043636 2.155678
-VERTEX2 848 -26.397279 0.176191 -0.891401
-VERTEX2 849 -25.809713 -0.627873 -0.897378
-VERTEX2 850 -25.193070 -1.426157 -1.029472
-VERTEX2 851 -24.665113 -2.263048 -1.200617
-VERTEX2 852 -24.311881 -3.184306 -1.316627
-VERTEX2 853 -24.043134 -4.169576 -1.054992
-VERTEX2 854 -23.509847 -5.050732 -0.884602
-VERTEX2 855 -22.879667 -5.823087 -0.683501
-VERTEX2 856 -23.442873 -6.588276 -2.307531
-VERTEX2 857 -24.112413 -7.332720 -2.099553
-VERTEX2 858 -24.583413 -8.183390 -2.118271
-VERTEX2 859 -25.107790 -9.035849 -2.300675
-VERTEX2 860 -24.471919 -8.302172 1.092163
-VERTEX2 861 -24.002996 -7.479829 0.992555
-VERTEX2 862 -23.448850 -6.679562 0.858511
-VERTEX2 863 -22.788455 -5.914228 0.747531
-VERTEX2 864 -23.509430 -6.623109 -2.182431
-VERTEX2 865 -24.071343 -7.432238 -2.453931
-VERTEX2 866 -24.837399 -8.080549 -2.403090
-VERTEX2 867 -25.555724 -8.757566 -2.344002
-VERTEX2 868 -24.835755 -8.040621 0.954615
-VERTEX2 869 -24.269581 -7.230552 1.130985
-VERTEX2 870 -23.837793 -6.294447 1.360524
-VERTEX2 871 -23.605433 -5.339831 1.562134
-VERTEX2 872 -23.597944 -4.324618 1.469465
-VERTEX2 873 -23.499734 -3.366132 1.650689
-VERTEX2 874 -23.541996 -2.345931 1.683247
-VERTEX2 875 -23.668336 -1.347552 1.941889
-VERTEX2 876 -24.601236 -1.704191 -2.533524
-VERTEX2 877 -25.425049 -2.265005 -2.365966
-VERTEX2 878 -26.154128 -2.964686 -2.431124
-VERTEX2 879 -26.912782 -3.624030 -2.394176
-VERTEX2 880 -26.182811 -2.923380 0.757115
-VERTEX2 881 -25.464092 -2.207945 0.783962
-VERTEX2 882 -24.748322 -1.520493 0.636065
-VERTEX2 883 -23.920655 -0.939749 0.625681
-VERTEX2 884 -24.709769 -1.521829 -2.512207
-VERTEX2 885 -25.479824 -2.114011 -2.259716
-VERTEX2 886 -26.130118 -2.893374 -2.255862
-VERTEX2 887 -26.747492 -3.689372 -2.573171
-VERTEX2 888 -27.269096 -2.847443 1.985776
-VERTEX2 889 -27.659711 -1.918011 2.052872
-VERTEX2 890 -28.133221 -0.989784 1.851653
-VERTEX2 891 -28.393062 -0.026462 1.784083
-VERTEX2 892 -28.181112 -1.018466 -1.128097
-VERTEX2 893 -27.739277 -1.903050 -0.932443
-VERTEX2 894 -27.138313 -2.691600 -0.969271
-VERTEX2 895 -26.588720 -3.541433 -0.827136
-VERTEX2 896 -27.349574 -4.211558 -2.505779
-VERTEX2 897 -28.152276 -4.821940 -2.737379
-VERTEX2 898 -29.076008 -5.237429 -2.585552
-VERTEX2 899 -29.936724 -5.749962 -2.394020
-VERTEX2 900 -29.170809 -5.085616 0.361250
-VERTEX2 901 -28.253468 -4.726133 0.455423
-VERTEX2 902 -27.319976 -4.338620 0.089180
-VERTEX2 903 -26.341694 -4.240681 0.169548
-VERTEX2 904 -26.522216 -3.249033 2.046987
-VERTEX2 905 -27.012088 -2.382304 2.091039
-VERTEX2 906 -27.518947 -1.509265 2.243373
-VERTEX2 907 -28.147249 -0.752474 2.111773
-VERTEX2 908 -28.986343 -1.269821 -2.808174
-VERTEX2 909 -29.902226 -1.612372 -3.080975
-VERTEX2 910 -30.883454 -1.646950 -2.912628
-VERTEX2 911 -31.902462 -1.888240 -3.022517
-VERTEX2 912 -30.933741 -1.790611 0.306102
-VERTEX2 913 -29.973811 -1.488590 0.338150
-VERTEX2 914 -29.053133 -1.127611 0.122245
-VERTEX2 915 -28.045395 -0.986620 0.312607
-VERTEX2 916 -28.335013 -0.045397 1.962745
-VERTEX2 917 -28.709078 0.864200 1.938954
-VERTEX2 918 -29.059630 1.785229 1.749802
-VERTEX2 919 -29.241357 2.775719 1.471085
-VERTEX2 920 -29.356111 1.807480 -1.457088
-VERTEX2 921 -29.231808 0.821808 -1.318841
-VERTEX2 922 -28.981173 -0.145132 -0.895309
-VERTEX2 923 -28.362314 -0.944233 -0.694853
-VERTEX2 924 -29.134059 -0.298406 2.370327
-VERTEX2 925 -29.824054 0.378968 2.091995
-VERTEX2 926 -30.299787 1.241989 1.958309
-VERTEX2 927 -30.680408 2.159939 2.374506
-VERTEX2 928 -30.005692 2.916839 0.908712
-VERTEX2 929 -29.401462 3.702544 1.007136
-VERTEX2 930 -28.918139 4.561229 1.350512
-VERTEX2 931 -28.737532 5.575485 1.176444
-VERTEX2 932 -29.697057 5.987401 2.655137
-VERTEX2 933 -30.581753 6.433335 2.465344
-VERTEX2 934 -31.400347 7.013321 2.706381
-VERTEX2 935 -32.318794 7.418395 3.097792
-VERTEX2 936 -31.302063 7.416460 0.161076
-VERTEX2 937 -30.343685 7.589747 -0.091121
-VERTEX2 938 -29.318739 7.481101 0.082662
-VERTEX2 939 -28.327849 7.544246 -0.040260
-VERTEX2 940 -27.302004 7.485207 -0.070815
-VERTEX2 941 -26.289629 7.419174 -0.187927
-VERTEX2 942 -25.346188 7.255418 -0.036402
-VERTEX2 943 -24.333107 7.191412 -0.251248
-VERTEX2 944 -24.565951 6.211081 -1.735539
-VERTEX2 945 -24.767195 5.231713 -1.423749
-VERTEX2 946 -24.624473 4.235715 -1.642517
-VERTEX2 947 -24.676580 3.279024 -1.387019
-VERTEX2 948 -25.653170 3.044621 3.078398
-VERTEX2 949 -26.654393 3.156807 2.805618
-VERTEX2 950 -27.573940 3.519974 2.977915
-VERTEX2 951 -28.538096 3.669817 2.900765
-VERTEX2 952 -27.559584 3.422958 0.106806
-VERTEX2 953 -26.568942 3.515005 0.225797
-VERTEX2 954 -25.552032 3.715216 0.405602
-VERTEX2 955 -24.639666 4.120680 0.221584
-VERTEX2 956 -24.844487 5.103075 1.660221
-VERTEX2 957 -24.900834 6.139313 2.085265
-VERTEX2 958 -25.409013 7.034225 2.208948
-VERTEX2 959 -26.010878 7.851062 1.922190
-VERTEX2 960 -25.634016 6.916705 -1.544836
-VERTEX2 961 -25.570101 5.904123 -1.896006
-VERTEX2 962 -25.882147 4.955952 -1.969388
-VERTEX2 963 -26.276626 3.998176 -1.426426
-VERTEX2 964 -26.422654 4.966378 1.308472
-VERTEX2 965 -26.167915 5.903094 1.349939
-VERTEX2 966 -25.945409 6.906298 1.729560
-VERTEX2 967 -26.106756 7.901972 1.903451
-VERTEX2 968 -25.778403 6.954029 -0.771971
-VERTEX2 969 -25.033316 6.221926 -1.122569
-VERTEX2 970 -24.598386 5.331036 -1.177181
-VERTEX2 971 -24.230926 4.384516 -1.328424
-VERTEX2 972 -23.269586 4.604232 0.127692
-VERTEX2 973 -22.274778 4.710327 0.393767
-VERTEX2 974 -21.386040 5.077501 0.260288
-VERTEX2 975 -20.436328 5.333264 0.345691
-VERTEX2 976 -19.537831 5.649638 0.595160
-VERTEX2 977 -18.672730 6.182234 0.690422
-VERTEX2 978 -17.957980 6.833383 0.980927
-VERTEX2 979 -17.349939 7.629589 0.867225
-VERTEX2 980 -18.004598 6.841100 -2.065547
-VERTEX2 981 -18.444992 5.932627 -2.375157
-VERTEX2 982 -19.159775 5.263683 -2.294022
-VERTEX2 983 -19.808153 4.519214 -2.183324
-VERTEX2 984 -19.212503 5.329531 0.891052
-VERTEX2 985 -18.597744 6.083291 0.985603
-VERTEX2 986 -18.058576 6.896775 0.549970
-VERTEX2 987 -17.220801 7.423357 0.924237
-VERTEX2 988 -16.443686 6.825325 -0.842885
-VERTEX2 989 -15.788976 6.055944 -0.759740
-VERTEX2 990 -15.053693 5.342549 -0.600263
-VERTEX2 991 -14.224925 4.732766 -0.946895
-VERTEX2 992 -15.041652 4.132840 -2.570709
-VERTEX2 993 -15.920732 3.561828 -2.896676
-VERTEX2 994 -16.856551 3.302155 -2.730347
-VERTEX2 995 -17.760733 2.918331 -2.664362
-VERTEX2 996 -17.334266 2.014026 -0.879360
-VERTEX2 997 -16.671740 1.258852 -1.071050
-VERTEX2 998 -16.201617 0.389345 -0.916474
-VERTEX2 999 -15.597082 -0.465010 -0.835131
-VERTEX2 1000 -16.260433 0.295147 2.272408
-VERTEX2 1001 -16.913605 1.047265 2.516850
-VERTEX2 1002 -17.742176 1.634052 2.155684
-VERTEX2 1003 -18.322776 2.516806 2.034340
-VERTEX2 1004 -17.481283 2.954547 0.426021
-VERTEX2 1005 -16.541357 3.362016 0.286480
-VERTEX2 1006 -15.592007 3.655561 0.194274
-VERTEX2 1007 -14.581075 3.833156 0.645521
-VERTEX2 1008 -14.003815 3.058209 -0.835647
-VERTEX2 1009 -13.338964 2.310059 -0.789302
-VERTEX2 1010 -12.631958 1.585500 -0.742787
-VERTEX2 1011 -11.873862 0.867557 -0.793649
-VERTEX2 1012 -11.164609 1.587090 0.771645
-VERTEX2 1013 -10.447586 2.277699 0.946594
-VERTEX2 1014 -9.790235 3.061601 0.929401
-VERTEX2 1015 -9.194196 3.862835 0.633558
-VERTEX2 1016 -8.638580 3.082050 -0.823603
-VERTEX2 1017 -7.980642 2.352686 -1.112452
-VERTEX2 1018 -7.518499 1.460747 -1.075394
-VERTEX2 1019 -7.042439 0.611487 -0.901191
-VERTEX2 1020 -7.680393 1.370017 2.125701
-VERTEX2 1021 -8.196669 2.222069 2.206567
-VERTEX2 1022 -8.771281 3.020341 2.554672
-VERTEX2 1023 -9.566178 3.558417 2.611848
-VERTEX2 1024 -8.752928 3.048541 -0.551109
-VERTEX2 1025 -7.926473 2.501570 -0.311298
-VERTEX2 1026 -6.938856 2.215143 -0.437408
-VERTEX2 1027 -6.045666 1.777477 -0.156531
-VERTEX2 1028 -6.201197 0.829836 -1.317094
-VERTEX2 1029 -5.989151 -0.130232 -1.263132
-VERTEX2 1030 -5.686844 -1.044503 -1.102504
-VERTEX2 1031 -5.229495 -1.942545 -1.299872
-VERTEX2 1032 -4.265105 -1.669971 0.260534
-VERTEX2 1033 -3.301076 -1.404327 0.612573
-VERTEX2 1034 -2.484667 -0.818323 0.678895
-VERTEX2 1035 -1.724495 -0.217020 0.675849
-VERTEX2 1036 -2.380578 0.558408 2.663906
-VERTEX2 1037 -3.272920 0.955773 2.580590
-VERTEX2 1038 -4.124346 1.479631 2.541465
-VERTEX2 1039 -4.938068 2.039886 2.535036
-VERTEX2 1040 -5.492512 1.202045 -1.889800
-VERTEX2 1041 -5.792463 0.240557 -1.719355
-VERTEX2 1042 -5.942341 -0.734167 -1.825182
-VERTEX2 1043 -6.211076 -1.697584 -1.703048
-VERTEX2 1044 -6.361915 -2.663840 -1.790078
-VERTEX2 1045 -6.598934 -3.615518 -1.412990
-VERTEX2 1046 -6.405114 -4.607211 -1.837776
-VERTEX2 1047 -6.669165 -5.564033 -1.731708
-VERTEX2 1048 -5.714153 -5.729160 0.087595
-VERTEX2 1049 -4.674530 -5.643513 0.051612
-VERTEX2 1050 -3.660611 -5.598903 -0.212364
-VERTEX2 1051 -2.639099 -5.843080 0.021392
-VERTEX2 1052 -2.613163 -6.832289 -1.698188
-VERTEX2 1053 -2.734398 -7.795184 -1.561599
-VERTEX2 1054 -2.714194 -8.791164 -1.607191
-VERTEX2 1055 -2.751672 -9.741037 -1.433562
-VERTEX2 1056 -2.887452 -8.703609 1.657349
-VERTEX2 1057 -2.941846 -7.745131 1.857410
-VERTEX2 1058 -3.242989 -6.790086 1.758519
-VERTEX2 1059 -3.403759 -5.805752 2.200700
-VERTEX2 1060 -2.798148 -6.588076 -0.680996
-VERTEX2 1061 -2.031522 -7.228933 -0.520312
-VERTEX2 1062 -1.173445 -7.744788 -0.710237
-VERTEX2 1063 -0.433499 -8.371326 -0.501990
-VERTEX2 1064 0.040233 -7.475102 0.971424
-VERTEX2 1065 0.599464 -6.643272 1.305402
-VERTEX2 1066 0.904407 -5.709220 1.460719
-VERTEX2 1067 1.028763 -4.709762 1.713835
-VERTEX2 1068 1.157724 -5.728412 -1.450707
-VERTEX2 1069 1.257854 -6.717626 -1.523579
-VERTEX2 1070 1.305508 -7.685640 -1.503361
-VERTEX2 1071 1.359129 -8.704432 -1.597488
-VERTEX2 1072 1.342612 -7.654568 1.423849
-VERTEX2 1073 1.451608 -6.680900 1.667078
-VERTEX2 1074 1.396811 -5.671383 1.483607
-VERTEX2 1075 1.504671 -4.676486 1.532740
-VERTEX2 1076 1.478721 -5.697287 -1.316893
-VERTEX2 1077 1.739235 -6.706672 -1.566336
-VERTEX2 1078 1.772350 -7.706854 -1.337791
-VERTEX2 1079 2.003172 -8.696850 -1.370835
-VERTEX2 1080 1.815666 -7.705113 1.546987
-VERTEX2 1081 1.855425 -6.707328 1.298408
-VERTEX2 1082 2.075648 -5.743869 1.396779
-VERTEX2 1083 2.248471 -4.753820 1.989340
-VERTEX2 1084 3.142755 -4.337268 0.507250
-VERTEX2 1085 4.037130 -3.899046 0.431837
-VERTEX2 1086 4.973821 -3.488191 0.885991
-VERTEX2 1087 5.634701 -2.679431 0.756121
-VERTEX2 1088 4.917681 -3.346296 -2.469140
-VERTEX2 1089 4.135167 -3.994198 -2.293747
-VERTEX2 1090 3.530570 -4.773885 -2.703052
-VERTEX2 1091 2.653680 -5.211048 -2.900989
-VERTEX2 1092 3.618940 -4.988271 0.267806
-VERTEX2 1093 4.555607 -4.720606 0.388607
-VERTEX2 1094 5.528474 -4.326246 0.227432
-VERTEX2 1095 6.499417 -4.107486 0.282547
-VERTEX2 1096 5.546022 -4.398276 -2.821102
-VERTEX2 1097 4.662040 -4.682656 -2.831488
-VERTEX2 1098 3.723684 -4.968737 -2.654089
-VERTEX2 1099 2.836677 -5.437726 -2.727121
-VERTEX2 1100 2.424413 -4.501638 2.138818
-VERTEX2 1101 1.827603 -3.682559 2.077315
-VERTEX2 1102 1.335886 -2.776000 1.837733
-VERTEX2 1103 1.071017 -1.833423 1.922213
-VERTEX2 1104 2.056032 -1.475249 -0.480885
-VERTEX2 1105 2.984196 -1.927755 -0.569712
-VERTEX2 1106 3.815457 -2.457122 -0.703276
-VERTEX2 1107 4.591544 -3.101879 -0.610996
-VERTEX2 1108 4.062655 -3.895120 -2.186657
-VERTEX2 1109 3.474913 -4.720421 -1.953747
-VERTEX2 1110 3.106406 -5.610783 -2.083340
-VERTEX2 1111 2.649859 -6.479240 -2.127849
-VERTEX2 1112 3.522595 -6.967495 -0.620420
-VERTEX2 1113 4.364317 -7.507236 -0.271572
-VERTEX2 1114 5.304822 -7.752244 -0.281208
-VERTEX2 1115 6.248602 -8.039497 -0.476784
-VERTEX2 1116 6.712998 -7.133600 1.398551
-VERTEX2 1117 6.884827 -6.142952 1.009180
-VERTEX2 1118 7.396883 -5.286236 0.882807
-VERTEX2 1119 8.032537 -4.501758 0.705474
-VERTEX2 1120 7.217593 -5.140766 -2.608113
-VERTEX2 1121 6.390256 -5.615094 -2.689945
-VERTEX2 1122 5.531267 -6.038592 -2.382947
-VERTEX2 1123 4.808792 -6.760750 -2.293017
-VERTEX2 1124 4.055274 -6.065563 2.928197
-VERTEX2 1125 3.081620 -5.865785 2.831591
-VERTEX2 1126 2.155974 -5.556948 2.825562
-VERTEX2 1127 1.143627 -5.240027 2.843887
-VERTEX2 1128 0.196019 -4.901412 3.073638
-VERTEX2 1129 -0.804245 -4.849492 3.087989
-VERTEX2 1130 -1.838148 -4.794024 3.086949
-VERTEX2 1131 -2.832102 -4.735545 3.088201
-VERTEX2 1132 -1.829400 -4.784567 0.181308
-VERTEX2 1133 -0.863050 -4.580306 0.247273
-VERTEX2 1134 0.118200 -4.343887 0.160336
-VERTEX2 1135 1.123179 -4.153090 0.181272
-VERTEX2 1136 0.206227 -4.348115 -2.494291
-VERTEX2 1137 -0.593937 -4.916471 -2.187782
-VERTEX2 1138 -1.165805 -5.726335 -2.250277
-VERTEX2 1139 -1.781333 -6.496593 -2.289444
-VERTEX2 1140 -1.125198 -5.699284 0.980842
-VERTEX2 1141 -0.575068 -4.888259 0.756814
-VERTEX2 1142 0.148946 -4.170482 0.697409
-VERTEX2 1143 0.932001 -3.522008 0.458414
-VERTEX2 1144 0.026472 -3.963025 -3.084466
-VERTEX2 1145 -0.949180 -4.037034 3.102512
-VERTEX2 1146 -1.990173 -3.986809 3.049549
-VERTEX2 1147 -2.977213 -3.859660 -3.115446
-VERTEX2 1148 -2.949144 -4.858453 -1.581949
-VERTEX2 1149 -3.004810 -5.838843 -1.942197
-VERTEX2 1150 -3.372584 -6.790279 -1.832975
-VERTEX2 1151 -3.595012 -7.772784 -1.979025
-VERTEX2 1152 -2.675831 -8.196181 -0.706611
-VERTEX2 1153 -1.891121 -8.822935 -0.399712
-VERTEX2 1154 -0.983979 -9.187921 -0.284430
-VERTEX2 1155 -0.048174 -9.475193 -0.286764
-VERTEX2 1156 0.210040 -8.518974 1.343792
-VERTEX2 1157 0.453230 -7.535854 1.654493
-VERTEX2 1158 0.372794 -6.590915 1.595044
-VERTEX2 1159 0.323381 -5.609790 1.512335
-VERTEX2 1160 0.246033 -6.605834 -1.973579
-VERTEX2 1161 -0.120751 -7.519477 -1.751250
-VERTEX2 1162 -0.285661 -8.469582 -1.638994
-VERTEX2 1163 -0.340033 -9.483253 -1.550564
-VERTEX2 1164 -0.323048 -10.452345 -1.730410
-VERTEX2 1165 -0.479721 -11.470111 -1.778802
-VERTEX2 1166 -0.664299 -12.420544 -1.686095
-VERTEX2 1167 -0.828899 -13.445392 -1.596742
-VERTEX2 1168 -0.772437 -12.446291 1.470649
-VERTEX2 1169 -0.710382 -11.457771 1.048658
-VERTEX2 1170 -0.241540 -10.577563 0.894562
-VERTEX2 1171 0.388192 -9.861302 0.939378
-VERTEX2 1172 -0.189030 -10.657416 -2.230173
-VERTEX2 1173 -0.784038 -11.427099 -1.875173
-VERTEX2 1174 -1.084527 -12.366454 -2.243951
-VERTEX2 1175 -1.745330 -13.138101 -2.565368
-VERTEX2 1176 -1.178741 -13.921980 -0.912676
-VERTEX2 1177 -0.566477 -14.724642 -1.202253
-VERTEX2 1178 -0.167347 -15.671298 -0.961390
-VERTEX2 1179 0.456528 -16.495159 -0.813208
-VERTEX2 1180 -0.283767 -17.174099 -2.432395
-VERTEX2 1181 -1.022098 -17.823840 -2.682803
-VERTEX2 1182 -1.927151 -18.284250 -2.688667
-VERTEX2 1183 -2.797705 -18.711421 -2.569699
-VERTEX2 1184 -1.962713 -18.212830 0.661479
-VERTEX2 1185 -1.192805 -17.618898 0.563017
-VERTEX2 1186 -0.274613 -17.052924 0.505803
-VERTEX2 1187 0.607479 -16.545034 0.776385
-VERTEX2 1188 -0.094039 -15.789842 2.553835
-VERTEX2 1189 -0.934922 -15.251632 2.657165
-VERTEX2 1190 -1.851125 -14.767774 2.360835
-VERTEX2 1191 -2.538295 -14.081984 1.815476
-VERTEX2 1192 -3.500712 -14.305415 -3.074909
-VERTEX2 1193 -4.534522 -14.366604 3.020217
-VERTEX2 1194 -5.554700 -14.243321 2.932347
-VERTEX2 1195 -6.546467 -14.048129 2.814422
-VERTEX2 1196 -5.626612 -14.318741 -0.223992
-VERTEX2 1197 -4.683956 -14.541893 -0.292491
-VERTEX2 1198 -3.739860 -14.834017 0.013501
-VERTEX2 1199 -2.774398 -14.869456 0.002488
-VERTEX2 1200 -2.744949 -15.863154 -1.557010
-VERTEX2 1201 -2.700253 -16.848572 -1.961915
-VERTEX2 1202 -3.125905 -17.773091 -2.130309
-VERTEX2 1203 -3.666769 -18.627748 -2.601990
-VERTEX2 1204 -4.196346 -17.735972 2.007949
-VERTEX2 1205 -4.611256 -16.844453 2.165182
-VERTEX2 1206 -5.181717 -16.043231 2.445350
-VERTEX2 1207 -5.943557 -15.412541 2.079950
-VERTEX2 1208 -5.461533 -16.287261 -1.472715
-VERTEX2 1209 -5.345795 -17.307018 -1.435905
-VERTEX2 1210 -5.236705 -18.281754 -1.366936
-VERTEX2 1211 -5.064069 -19.298774 -1.593144
-VERTEX2 1212 -5.077353 -18.299225 1.673821
-VERTEX2 1213 -5.193110 -17.311561 1.618777
-VERTEX2 1214 -5.240834 -16.270472 1.681500
-VERTEX2 1215 -5.361784 -15.299355 1.615701
-VERTEX2 1216 -4.343023 -15.276722 -0.120364
-VERTEX2 1217 -3.337581 -15.423986 0.103227
-VERTEX2 1218 -2.322695 -15.343088 0.363962
-VERTEX2 1219 -1.379193 -14.984518 0.628333
-VERTEX2 1220 -0.604790 -14.370288 0.544417
-VERTEX2 1221 0.236760 -13.864720 0.455578
-VERTEX2 1222 1.145776 -13.419163 0.515881
-VERTEX2 1223 2.018065 -12.967532 0.393143
-VERTEX2 1224 1.645062 -12.016415 2.055442
-VERTEX2 1225 1.199304 -11.137609 1.910511
-VERTEX2 1226 0.897073 -10.175249 1.859064
-VERTEX2 1227 0.617646 -9.236642 1.547759
-VERTEX2 1228 1.585151 -9.253582 -0.401500
-VERTEX2 1229 2.538716 -9.618670 -0.362229
-VERTEX2 1230 3.463593 -9.973158 -0.433510
-VERTEX2 1231 4.420411 -10.405313 -0.600552
-VERTEX2 1232 4.984987 -9.592775 0.912922
-VERTEX2 1233 5.561136 -8.787987 0.876447
-VERTEX2 1234 6.202383 -8.036953 1.157920
-VERTEX2 1235 6.594982 -7.108854 1.048447
-VERTEX2 1236 7.089134 -6.270129 1.260323
-VERTEX2 1237 7.386030 -5.336427 1.113153
-VERTEX2 1238 7.841015 -4.479455 0.925550
-VERTEX2 1239 8.367212 -3.665182 0.557467
-VERTEX2 1240 7.520987 -4.206381 -2.491922
-VERTEX2 1241 6.750966 -4.807259 -2.361559
-VERTEX2 1242 6.030377 -5.530930 -2.042894
-VERTEX2 1243 5.559959 -6.449120 -1.565338
-VERTEX2 1244 4.574356 -6.427190 -2.855094
-VERTEX2 1245 3.627218 -6.696559 -2.781737
-VERTEX2 1246 2.708693 -7.038581 -2.597292
-VERTEX2 1247 1.880981 -7.584916 -2.587565
-VERTEX2 1248 1.340398 -6.695489 2.043878
-VERTEX2 1249 0.901062 -5.803645 1.943239
-VERTEX2 1250 0.518050 -4.836236 2.068184
-VERTEX2 1251 0.041873 -3.974482 2.155559
-VERTEX2 1252 0.614281 -4.825541 -1.199648
-VERTEX2 1253 0.984056 -5.759694 -1.105314
-VERTEX2 1254 1.441518 -6.621321 -0.902037
-VERTEX2 1255 2.049628 -7.394139 -0.866868
-VERTEX2 1256 1.251181 -8.011180 -2.406788
-VERTEX2 1257 0.535533 -8.734189 -2.358740
-VERTEX2 1258 -0.201363 -9.452278 -2.547175
-VERTEX2 1259 -1.005112 -10.030829 -2.673982
-VERTEX2 1260 -0.547892 -10.890320 -0.772240
-VERTEX2 1261 0.146116 -11.600203 -0.615101
-VERTEX2 1262 0.967818 -12.206059 -0.378114
-VERTEX2 1263 1.887602 -12.578490 -0.477182
-VERTEX2 1264 1.003486 -12.089688 2.836835
-VERTEX2 1265 0.049217 -11.776005 3.027999
-VERTEX2 1266 -0.955991 -11.676138 -3.072318
-VERTEX2 1267 -1.956359 -11.760958 -3.098691
-VERTEX2 1268 -0.963062 -11.696108 0.041892
-VERTEX2 1269 0.056793 -11.655749 0.412882
-VERTEX2 1270 0.965940 -11.253369 0.508512
-VERTEX2 1271 1.805449 -10.782818 0.200698
-VERTEX2 1272 2.009107 -11.775922 -1.432182
-VERTEX2 1273 2.138732 -12.810010 -1.829980
-VERTEX2 1274 1.849409 -13.770428 -1.669228
-VERTEX2 1275 1.776115 -14.760750 -1.836033
-VERTEX2 1276 2.724239 -15.005906 -0.308807
-VERTEX2 1277 3.682218 -15.357265 -0.223151
-VERTEX2 1278 4.648743 -15.560741 -0.159760
-VERTEX2 1279 5.645567 -15.731661 -0.422024
-VERTEX2 1280 6.020906 -14.798291 1.032927
-VERTEX2 1281 6.541934 -13.945620 0.846623
-VERTEX2 1282 7.262252 -13.195951 0.754119
-VERTEX2 1283 8.004173 -12.514808 1.130044
-VERTEX2 1284 7.116452 -12.045193 2.723632
-VERTEX2 1285 6.194623 -11.615274 2.420737
-VERTEX2 1286 5.416052 -10.963565 2.555000
-VERTEX2 1287 4.545672 -10.437014 2.477992
-VERTEX2 1288 3.952948 -11.216998 -2.070655
-VERTEX2 1289 3.471478 -12.099035 -2.130594
-VERTEX2 1290 2.929181 -12.923769 -2.115729
-VERTEX2 1291 2.447746 -13.805593 -1.831936
-VERTEX2 1292 2.692642 -12.842171 1.372516
-VERTEX2 1293 2.910032 -11.849065 1.642502
-VERTEX2 1294 2.829731 -10.861997 1.581406
-VERTEX2 1295 2.825476 -9.870130 1.555441
-VERTEX2 1296 3.824150 -9.894865 0.076829
-VERTEX2 1297 4.826345 -9.817195 0.014357
-VERTEX2 1298 5.849250 -9.815757 0.088181
-VERTEX2 1299 6.848518 -9.712833 0.217514
-VERTEX2 1300 7.073373 -10.671031 -1.438539
-VERTEX2 1301 7.231287 -11.605947 -1.679458
-VERTEX2 1302 7.117383 -12.604746 -1.783353
-VERTEX2 1303 6.907530 -13.584812 -1.866080
-VERTEX2 1304 7.190141 -12.607789 1.428260
-VERTEX2 1305 7.327189 -11.629297 1.450191
-VERTEX2 1306 7.404519 -10.646787 1.685456
-VERTEX2 1307 7.272326 -9.665081 1.516952
-VERTEX2 1308 8.236964 -9.730194 0.001275
-VERTEX2 1309 9.233793 -9.659249 0.177901
-VERTEX2 1310 10.200711 -9.456031 0.337040
-VERTEX2 1311 11.151326 -9.055997 0.169199
-VERTEX2 1312 11.355893 -10.044982 -1.348644
-VERTEX2 1313 11.546814 -11.004526 -1.485992
-VERTEX2 1314 11.612159 -11.986753 -1.459021
-VERTEX2 1315 11.706815 -12.972195 -1.490927
-VERTEX2 1316 11.671490 -11.992086 1.840180
-VERTEX2 1317 11.397072 -11.036866 1.470349
-VERTEX2 1318 11.476555 -10.013498 1.484467
-VERTEX2 1319 11.525796 -9.032483 1.495812
-VERTEX2 1320 10.486887 -9.004306 2.858350
-VERTEX2 1321 9.535675 -8.717459 2.877597
-VERTEX2 1322 8.528193 -8.454859 2.782322
-VERTEX2 1323 7.546732 -8.104933 3.025345
-VERTEX2 1324 8.548972 -8.225847 -0.466562
-VERTEX2 1325 9.397565 -8.655677 -0.438706
-VERTEX2 1326 10.318823 -9.097737 -0.311696
-VERTEX2 1327 11.290214 -9.406717 -0.751676
-VERTEX2 1328 10.651684 -10.134772 -2.162299
-VERTEX2 1329 10.069858 -10.940490 -1.906871
-VERTEX2 1330 9.733216 -11.884248 -1.764426
-VERTEX2 1331 9.518180 -12.902194 -1.425171
-VERTEX2 1332 10.503927 -12.762106 0.260407
-VERTEX2 1333 11.463669 -12.511678 -0.127535
-VERTEX2 1334 12.460751 -12.625652 -0.327132
-VERTEX2 1335 13.381428 -12.984331 -0.065477
-VERTEX2 1336 13.449461 -11.963184 1.477258
-VERTEX2 1337 13.573140 -10.939993 1.263292
-VERTEX2 1338 13.904939 -10.008893 0.808731
-VERTEX2 1339 14.572717 -9.233962 0.590643
-VERTEX2 1340 13.740318 -9.819160 -2.692416
-VERTEX2 1341 12.833636 -10.255538 -2.573684
-VERTEX2 1342 11.974396 -10.830170 -2.486136
-VERTEX2 1343 11.191964 -11.412914 -2.138076
-VERTEX2 1344 12.047621 -11.933569 -0.352375
-VERTEX2 1345 12.987609 -12.262614 -0.490709
-VERTEX2 1346 13.851970 -12.739174 -0.465587
-VERTEX2 1347 14.744418 -13.211691 -0.415387
-VERTEX2 1348 15.107495 -12.280907 1.260706
-VERTEX2 1349 15.430359 -11.317037 1.009944
-VERTEX2 1350 15.996144 -10.434240 0.844515
-VERTEX2 1351 16.645754 -9.679901 0.808667
-VERTEX2 1352 17.372085 -10.372416 -0.780791
-VERTEX2 1353 18.067912 -11.066875 -0.433425
-VERTEX2 1354 18.968019 -11.480067 -0.348390
-VERTEX2 1355 19.905104 -11.838611 -0.497641
-VERTEX2 1356 20.362241 -11.007231 1.076828
-VERTEX2 1357 20.834808 -10.116791 1.064043
-VERTEX2 1358 21.285812 -9.259930 0.968205
-VERTEX2 1359 21.846441 -8.443259 1.263435
-VERTEX2 1360 20.861644 -8.144807 -3.058920
-VERTEX2 1361 19.844635 -8.245861 -3.003068
-VERTEX2 1362 18.886741 -8.351527 -2.934018
-VERTEX2 1363 17.906147 -8.550597 3.125350
-VERTEX2 1364 17.931339 -7.539792 1.773069
-VERTEX2 1365 17.753284 -6.607942 1.570670
-VERTEX2 1366 17.743824 -5.617743 1.620257
-VERTEX2 1367 17.699833 -4.594036 1.670831
-VERTEX2 1368 18.715466 -4.518889 0.105892
-VERTEX2 1369 19.701444 -4.383400 0.185286
-VERTEX2 1370 20.698625 -4.194679 0.199741
-VERTEX2 1371 21.640869 -3.995654 0.621946
-VERTEX2 1372 22.283063 -4.788111 -0.662345
-VERTEX2 1373 23.082109 -5.383709 -0.248594
-VERTEX2 1374 24.041572 -5.634269 -0.070739
-VERTEX2 1375 25.057890 -5.697285 -0.056047
-VERTEX2 1376 25.037034 -6.737380 -1.599043
-VERTEX2 1377 24.986066 -7.766681 -1.577920
-VERTEX2 1378 24.944477 -8.722169 -1.381653
-VERTEX2 1379 25.167995 -9.713789 -1.394509
-VERTEX2 1380 24.991542 -8.746670 1.799601
-VERTEX2 1381 24.799780 -7.800709 1.608040
-VERTEX2 1382 24.774270 -6.815774 1.795388
-VERTEX2 1383 24.570881 -5.803661 1.708184
-VERTEX2 1384 25.576801 -5.703640 -0.135263
-VERTEX2 1385 26.565090 -5.874627 -0.180145
-VERTEX2 1386 27.525807 -6.024148 -0.243342
-VERTEX2 1387 28.501680 -6.261906 -0.418834
-VERTEX2 1388 28.906017 -5.345793 1.040037
-VERTEX2 1389 29.399944 -4.488330 1.160822
-VERTEX2 1390 29.787325 -3.555197 1.114377
-VERTEX2 1391 30.217730 -2.700169 0.799862
-VERTEX2 1392 29.529422 -3.439599 -2.012931
-VERTEX2 1393 29.077953 -4.365138 -1.956541
-VERTEX2 1394 28.720729 -5.253916 -1.801673
-VERTEX2 1395 28.515884 -6.206143 -2.039286
-VERTEX2 1396 28.905698 -5.333054 1.074997
-VERTEX2 1397 29.404686 -4.457754 1.344051
-VERTEX2 1398 29.620562 -3.501351 1.086397
-VERTEX2 1399 30.086284 -2.591593 1.168488
-VERTEX2 1400 29.677510 -3.508627 -1.655822
-VERTEX2 1401 29.599950 -4.453682 -1.762191
-VERTEX2 1402 29.426850 -5.413221 -2.179664
-VERTEX2 1403 28.844518 -6.227246 -2.551682
-VERTEX2 1404 29.664408 -5.737756 0.563117
-VERTEX2 1405 30.494273 -5.200002 0.878502
-VERTEX2 1406 31.106968 -4.433210 0.826738
-VERTEX2 1407 31.781550 -3.670847 0.778756
-VERTEX2 1408 31.095703 -2.982264 2.018144
-VERTEX2 1409 30.646311 -2.071377 2.057936
-VERTEX2 1410 30.193989 -1.147092 2.071922
-VERTEX2 1411 29.723812 -0.203567 2.186119
-VERTEX2 1412 30.326286 -0.994230 -1.117348
-VERTEX2 1413 30.758503 -1.927927 -0.819323
-VERTEX2 1414 31.459674 -2.643912 -0.848085
-VERTEX2 1415 32.099279 -3.384779 -1.200690
-VERTEX2 1416 31.177022 -3.727758 -2.842223
-VERTEX2 1417 30.231725 -3.996572 -2.579460
-VERTEX2 1418 29.360808 -4.509581 -2.489728
-VERTEX2 1419 28.563642 -5.084717 -2.848022
-VERTEX2 1420 29.506824 -4.802675 0.689488
-VERTEX2 1421 30.268318 -4.163024 0.820591
-VERTEX2 1422 30.930942 -3.444302 0.858149
-VERTEX2 1423 31.585841 -2.684355 0.833063
-VERTEX2 1424 30.842679 -2.014081 2.324597
-VERTEX2 1425 30.139796 -1.300898 2.295100
-VERTEX2 1426 29.491145 -0.564173 2.072969
-VERTEX2 1427 28.999175 0.286601 2.298105
-VERTEX2 1428 29.738870 0.956300 0.706388
-VERTEX2 1429 30.466256 1.580830 0.924530
-VERTEX2 1430 31.073762 2.357477 1.069494
-VERTEX2 1431 31.572855 3.259245 1.079211
-VERTEX2 1432 32.480785 2.778673 -0.434293
-VERTEX2 1433 33.382043 2.389146 -0.630652
-VERTEX2 1434 34.214457 1.798857 -0.527948
-VERTEX2 1435 35.078484 1.265695 -0.219407
-VERTEX2 1436 34.877386 0.343061 -1.553545
-VERTEX2 1437 34.894911 -0.675148 -1.452098
-VERTEX2 1438 34.990313 -1.648231 -1.379148
-VERTEX2 1439 35.167943 -2.596641 -1.404974
-VERTEX2 1440 36.161813 -2.402394 -0.022446
-VERTEX2 1441 37.178755 -2.420392 -0.159519
-VERTEX2 1442 38.160365 -2.592558 -0.318822
-VERTEX2 1443 39.074566 -2.889378 -0.451172
-VERTEX2 1444 39.561317 -1.993186 1.333143
-VERTEX2 1445 39.783585 -1.038864 1.539987
-VERTEX2 1446 39.827222 -0.029239 1.829645
-VERTEX2 1447 39.573528 0.931881 1.983630
-VERTEX2 1448 39.952104 0.045629 -0.920130
-VERTEX2 1449 40.548349 -0.759951 -1.329111
-VERTEX2 1450 40.820473 -1.743280 -1.292388
-VERTEX2 1451 41.090622 -2.731478 -1.810884
-VERTEX2 1452 40.867108 -3.713857 -1.595316
-VERTEX2 1453 40.843936 -4.731044 -1.761786
-VERTEX2 1454 40.654214 -5.693629 -1.560983
-VERTEX2 1455 40.673460 -6.700369 -1.358948
-VERTEX2 1456 39.654156 -6.876686 -3.027524
-VERTEX2 1457 38.657430 -6.983360 3.077497
-VERTEX2 1458 37.694354 -6.933770 2.999467
-VERTEX2 1459 36.722252 -6.783005 2.971172
-VERTEX2 1460 36.568686 -7.755797 -1.660358
-VERTEX2 1461 36.464828 -8.786699 -1.767747
-VERTEX2 1462 36.251452 -9.787871 -1.679682
-VERTEX2 1463 36.122247 -10.801875 -1.924735
-VERTEX2 1464 35.182023 -10.440053 2.755370
-VERTEX2 1465 34.254549 -10.091067 2.683284
-VERTEX2 1466 33.380857 -9.674497 2.651055
-VERTEX2 1467 32.496066 -9.227576 2.533198
-VERTEX2 1468 31.991027 -10.011694 -2.295351
-VERTEX2 1469 31.294218 -10.729048 -2.273432
-VERTEX2 1470 30.674587 -11.519589 -2.210786
-VERTEX2 1471 30.074330 -12.300371 -2.110559
-VERTEX2 1472 29.243780 -11.813691 2.547936
-VERTEX2 1473 28.407808 -11.250902 2.713358
-VERTEX2 1474 27.530298 -10.819534 2.813258
-VERTEX2 1475 26.577876 -10.512105 2.737723
-VERTEX2 1476 27.471338 -10.891368 -0.494126
-VERTEX2 1477 28.365108 -11.351366 -0.394109
-VERTEX2 1478 29.306864 -11.735181 -0.836213
-VERTEX2 1479 29.985235 -12.468836 -0.618866
-VERTEX2 1480 30.563152 -11.634444 1.122301
-VERTEX2 1481 30.985447 -10.736510 0.858793
-VERTEX2 1482 31.646756 -10.009437 1.216695
-VERTEX2 1483 32.004146 -9.066500 1.220317
-VERTEX2 1484 31.073790 -8.721778 3.019526
-VERTEX2 1485 30.103026 -8.566514 -2.660359
-VERTEX2 1486 29.220598 -9.021390 -2.810284
-VERTEX2 1487 28.229737 -9.324682 -2.771860
-VERTEX2 1488 27.867978 -8.367336 1.376611
-VERTEX2 1489 28.066391 -7.363088 1.159028
-VERTEX2 1490 28.455657 -6.463994 1.058008
-VERTEX2 1491 28.936107 -5.616462 1.225375
-VERTEX2 1492 27.972170 -5.316811 2.656666
-VERTEX2 1493 27.063927 -4.831748 2.690986
-VERTEX2 1494 26.178656 -4.367728 2.635303
-VERTEX2 1495 25.290933 -3.898156 2.755231
-VERTEX2 1496 25.632062 -2.967565 1.714935
-VERTEX2 1497 25.511437 -1.994200 1.691544
-VERTEX2 1498 25.369119 -1.018102 1.507046
-VERTEX2 1499 25.435558 0.004162 1.390255
-VERTEX2 1500 24.493422 0.136808 2.852359
-VERTEX2 1501 23.515023 0.415021 2.687094
-VERTEX2 1502 22.635187 0.821472 3.112214
-VERTEX2 1503 21.655422 0.882923 -3.131021
-VERTEX2 1504 22.645196 0.901998 0.016985
-VERTEX2 1505 23.636684 0.929132 -0.123067
-VERTEX2 1506 24.669512 0.821572 -0.282082
-VERTEX2 1507 25.664524 0.501289 -0.100534
-VERTEX2 1508 24.691319 0.601499 3.071509
-VERTEX2 1509 23.709758 0.667713 -3.002165
-VERTEX2 1510 22.701142 0.533648 -2.986552
-VERTEX2 1511 21.682767 0.383085 -3.099249
-VERTEX2 1512 22.687557 0.389952 0.368262
-VERTEX2 1513 23.619592 0.761745 0.393366
-VERTEX2 1514 24.555104 1.130591 0.951195
-VERTEX2 1515 25.151148 1.938873 1.175868
-VERTEX2 1516 24.278787 2.325528 2.468318
-VERTEX2 1517 23.494215 2.958235 2.229618
-VERTEX2 1518 22.926537 3.736610 2.040506
-VERTEX2 1519 22.473583 4.617399 1.968609
-VERTEX2 1520 22.878996 3.693587 -1.161055
-VERTEX2 1521 23.273702 2.747155 -0.997855
-VERTEX2 1522 23.877275 1.857449 -0.735680
-VERTEX2 1523 24.584096 1.183715 -0.727397
-VERTEX2 1524 23.916718 0.431907 -2.222053
-VERTEX2 1525 23.336555 -0.363669 -2.382590
-VERTEX2 1526 22.615482 -1.066497 -2.352796
-VERTEX2 1527 21.911090 -1.768421 -2.300635
-VERTEX2 1528 22.590965 -0.986216 0.948922
-VERTEX2 1529 23.183508 -0.161575 1.198623
-VERTEX2 1530 23.565054 0.775911 0.964708
-VERTEX2 1531 24.117125 1.623147 1.075768
-VERTEX2 1532 23.618944 0.784423 -2.312793
-VERTEX2 1533 22.943707 0.004888 -2.050379
-VERTEX2 1534 22.457505 -0.882435 -1.484204
-VERTEX2 1535 22.533312 -1.877340 -1.293103
-VERTEX2 1536 21.566571 -2.094199 3.122119
-VERTEX2 1537 20.562147 -2.084123 3.111553
-VERTEX2 1538 19.554231 -2.096825 2.997859
-VERTEX2 1539 18.553807 -1.940009 2.718632
-VERTEX2 1540 19.458375 -2.330862 -0.613820
-VERTEX2 1541 20.282736 -2.920664 -1.002188
-VERTEX2 1542 20.813971 -3.762744 -1.173289
-VERTEX2 1543 21.215613 -4.677800 -0.979750
-VERTEX2 1544 20.636641 -3.841210 2.219241
-VERTEX2 1545 20.024607 -3.072293 1.694980
-VERTEX2 1546 19.904454 -2.080104 1.911786
-VERTEX2 1547 19.527785 -1.153985 2.068112
-VERTEX2 1548 19.999834 -2.037180 -1.285097
-VERTEX2 1549 20.303144 -2.982001 -1.421356
-VERTEX2 1550 20.434113 -3.981481 -1.359835
-VERTEX2 1551 20.620153 -4.941579 -1.585111
-VERTEX2 1552 21.630613 -4.948811 -0.235426
-VERTEX2 1553 22.578595 -5.179365 -0.472896
-VERTEX2 1554 23.513534 -5.668023 -0.356941
-VERTEX2 1555 24.450856 -6.021126 -0.390458
-VERTEX2 1556 24.105638 -6.983733 -2.073753
-VERTEX2 1557 23.619000 -7.846550 -2.335964
-VERTEX2 1558 22.946145 -8.523227 -2.373318
-VERTEX2 1559 22.227766 -9.191230 -2.562866
-VERTEX2 1560 23.047158 -8.652283 0.859893
-VERTEX2 1561 23.709252 -7.944674 0.469625
-VERTEX2 1562 24.618245 -7.458926 0.325143
-VERTEX2 1563 25.553583 -7.113671 0.056528
-VERTEX2 1564 25.596397 -8.080427 -1.239821
-VERTEX2 1565 25.900495 -9.029166 -1.118637
-VERTEX2 1566 26.330858 -9.943002 -1.276023
-VERTEX2 1567 26.617847 -10.909230 -1.367396
-VERTEX2 1568 27.659350 -10.703702 0.314773
-VERTEX2 1569 28.618449 -10.383048 0.353997
-VERTEX2 1570 29.581984 -10.085689 -0.165624
-VERTEX2 1571 30.608534 -10.236769 -0.574821
-VERTEX2 1572 29.824365 -9.736065 2.481884
-VERTEX2 1573 29.060450 -9.111215 2.728763
-VERTEX2 1574 28.154675 -8.720648 2.828456
-VERTEX2 1575 27.204085 -8.412547 2.639414
-VERTEX2 1576 27.704742 -7.569707 1.171137
-VERTEX2 1577 28.104791 -6.667555 1.030334
-VERTEX2 1578 28.629320 -5.795254 0.778084
-VERTEX2 1579 29.320143 -5.100201 0.784730
-VERTEX2 1580 28.647092 -4.366028 2.278706
-VERTEX2 1581 28.003457 -3.649614 2.214250
-VERTEX2 1582 27.368016 -2.863580 2.022028
-VERTEX2 1583 26.881352 -1.978783 2.025165
-VERTEX2 1584 26.000861 -2.401560 -2.847800
-VERTEX2 1585 25.057755 -2.661539 -3.046801
-VERTEX2 1586 24.095379 -2.771999 -2.892033
-VERTEX2 1587 23.104745 -3.002077 -2.900502
-VERTEX2 1588 22.889819 -2.108015 1.571377
-VERTEX2 1589 22.893880 -1.086072 1.834543
-VERTEX2 1590 22.623806 -0.109293 1.645791
-VERTEX2 1591 22.537654 0.883797 1.670800
-VERTEX2 1592 21.524027 0.806092 2.981439
-VERTEX2 1593 20.553455 0.969772 -2.891054
-VERTEX2 1594 19.571202 0.733272 -2.492599
-VERTEX2 1595 18.795725 0.116649 -2.490751
-VERTEX2 1596 19.424486 -0.639131 -1.047504
-VERTEX2 1597 19.959713 -1.527734 -0.854751
-VERTEX2 1598 20.606338 -2.299030 -0.784561
-VERTEX2 1599 21.346970 -3.052243 -0.783159
-VERTEX2 1600 22.038459 -2.315164 1.109266
-VERTEX2 1601 22.502580 -1.410011 1.340883
-VERTEX2 1602 22.719188 -0.422161 1.412609
-VERTEX2 1603 22.863600 0.566482 1.528521
-VERTEX2 1604 21.868711 0.610137 3.085214
-VERTEX2 1605 20.811980 0.637957 -3.117893
-VERTEX2 1606 19.838025 0.616263 -2.946557
-VERTEX2 1607 18.867543 0.368034 -2.694934
-VERTEX2 1608 19.298699 -0.543923 -1.305607
-VERTEX2 1609 19.541644 -1.523336 -1.299076
-VERTEX2 1610 19.767334 -2.492492 -1.165199
-VERTEX2 1611 20.160427 -3.391443 -1.405566
-VERTEX2 1612 19.993092 -2.392917 1.730132
-VERTEX2 1613 19.839303 -1.358849 1.617260
-VERTEX2 1614 19.777821 -0.359887 1.618089
-VERTEX2 1615 19.723543 0.614089 1.649841
-VERTEX2 1616 20.777093 0.699317 -0.095606
-VERTEX2 1617 21.784872 0.629415 0.073726
-VERTEX2 1618 22.763122 0.743592 0.110596
-VERTEX2 1619 23.791462 0.826877 0.420524
-VERTEX2 1620 22.872399 0.425443 -2.759975
-VERTEX2 1621 21.933346 0.074690 -2.825424
-VERTEX2 1622 20.999302 -0.236993 -2.788606
-VERTEX2 1623 20.078184 -0.579131 -2.699524
-VERTEX2 1624 19.641670 0.339316 2.233776
-VERTEX2 1625 19.051819 1.106481 2.033564
-VERTEX2 1626 18.583305 2.003335 2.161769
-VERTEX2 1627 18.026820 2.846946 1.894020
-VERTEX2 1628 18.985116 3.170120 0.471866
-VERTEX2 1629 19.885462 3.646113 0.225816
-VERTEX2 1630 20.860044 3.849011 0.211859
-VERTEX2 1631 21.898092 4.074096 0.555585
-VERTEX2 1632 22.424993 3.224277 -0.970542
-VERTEX2 1633 22.989877 2.380832 -1.110103
-VERTEX2 1634 23.435490 1.466493 -1.223545
-VERTEX2 1635 23.787319 0.576341 -1.196596
-VERTEX2 1636 23.398257 1.478671 2.024977
-VERTEX2 1637 22.982096 2.360472 1.908841
-VERTEX2 1638 22.683886 3.259536 2.005918
-VERTEX2 1639 22.295946 4.177453 1.861021
-VERTEX2 1640 21.349581 3.898847 -2.882604
-VERTEX2 1641 20.373497 3.675426 -2.788939
-VERTEX2 1642 19.447511 3.311551 -2.842158
-VERTEX2 1643 18.460907 3.019102 -2.891497
-VERTEX2 1644 18.706820 1.997532 -1.182028
-VERTEX2 1645 19.112360 1.071109 -1.243869
-VERTEX2 1646 19.398205 0.143477 -1.642371
-VERTEX2 1647 19.352814 -0.834931 -1.518414
-VERTEX2 1648 20.369995 -0.773929 -0.171579
-VERTEX2 1649 21.336771 -0.952205 -0.096653
-VERTEX2 1650 22.273676 -1.053848 -0.200331
-VERTEX2 1651 23.255078 -1.219306 0.023636
-VERTEX2 1652 23.296217 -2.202053 -1.818134
-VERTEX2 1653 23.048103 -3.141385 -1.967447
-VERTEX2 1654 22.672687 -4.071668 -1.881524
-VERTEX2 1655 22.378360 -5.018052 -1.758741
-VERTEX2 1656 23.353686 -5.219994 -0.198653
-VERTEX2 1657 24.332512 -5.432024 -0.306314
-VERTEX2 1658 25.291417 -5.747500 -0.533657
-VERTEX2 1659 26.121174 -6.273021 -0.505782
-VERTEX2 1660 27.000067 -6.766441 -0.761893
-VERTEX2 1661 27.742511 -7.433369 -0.308066
-VERTEX2 1662 28.655357 -7.735368 -0.375586
-VERTEX2 1663 29.604531 -8.082818 -0.212111
-VERTEX2 1664 29.411096 -9.049695 -1.694799
-VERTEX2 1665 29.276888 -10.063056 -1.520970
-VERTEX2 1666 29.302160 -11.073402 -1.577126
-VERTEX2 1667 29.294664 -12.069345 -1.244714
-VERTEX2 1668 28.994075 -11.091443 1.963076
-VERTEX2 1669 28.632513 -10.154940 2.011345
-VERTEX2 1670 28.218578 -9.252324 2.036735
-VERTEX2 1671 27.741923 -8.378788 2.275773
-VERTEX2 1672 26.998815 -9.062211 -2.223359
-VERTEX2 1673 26.413019 -9.843235 -2.229210
-VERTEX2 1674 25.789415 -10.620030 -2.570085
-VERTEX2 1675 24.954914 -11.171071 -2.748044
-VERTEX2 1676 24.533617 -10.263568 1.632850
-VERTEX2 1677 24.475923 -9.327099 1.333217
-VERTEX2 1678 24.722765 -8.304767 1.408474
-VERTEX2 1679 24.908226 -7.349920 1.240935
-VERTEX2 1680 23.971471 -7.063180 3.138082
-VERTEX2 1681 22.978545 -7.072082 -2.967405
-VERTEX2 1682 21.975741 -7.238611 -3.085868
-VERTEX2 1683 20.988947 -7.303505 -3.096022
-VERTEX2 1684 21.054322 -8.318060 -1.359457
-VERTEX2 1685 21.264554 -9.278306 -1.313610
-VERTEX2 1686 21.532350 -10.248003 -1.449351
-VERTEX2 1687 21.609108 -11.220520 -1.128866
-VERTEX2 1688 20.688858 -11.652615 -2.622316
-VERTEX2 1689 19.857860 -12.181121 -2.211936
-VERTEX2 1690 19.278677 -12.998869 -1.986729
-VERTEX2 1691 18.866498 -13.914257 -1.996237
-VERTEX2 1692 19.771543 -14.327902 -0.485705
-VERTEX2 1693 20.631191 -14.803702 -0.705626
-VERTEX2 1694 21.400269 -15.425300 -0.448369
-VERTEX2 1695 22.300596 -15.843785 -0.155611
-VERTEX2 1696 22.095160 -16.834254 -1.727844
-VERTEX2 1697 21.930802 -17.837555 -1.835701
-VERTEX2 1698 21.666017 -18.772820 -1.776128
-VERTEX2 1699 21.463657 -19.739075 -1.912474
-VERTEX2 1700 21.131070 -20.702827 -1.977949
-VERTEX2 1701 20.694439 -21.598602 -2.021410
-VERTEX2 1702 20.268751 -22.526956 -1.991930
-VERTEX2 1703 19.867182 -23.413593 -2.004110
-VERTEX2 1704 20.815826 -23.835643 -0.236533
-VERTEX2 1705 21.790312 -24.127907 -0.394341
-VERTEX2 1706 22.708425 -24.505410 -0.184954
-VERTEX2 1707 23.683643 -24.692834 0.048180
-VERTEX2 1708 22.665736 -24.732801 -3.016187
-VERTEX2 1709 21.691464 -24.825442 -2.979637
-VERTEX2 1710 20.709044 -25.029515 3.077285
-VERTEX2 1711 19.704287 -24.979264 -3.016030
-VERTEX2 1712 18.737540 -25.106379 -3.023447
-VERTEX2 1713 17.730969 -25.202979 -3.136061
-VERTEX2 1714 16.748994 -25.233491 -3.017320
-VERTEX2 1715 15.759060 -25.376229 -2.862700
-VERTEX2 1716 15.521452 -24.458771 2.278260
-VERTEX2 1717 14.867102 -23.673518 2.306589
-VERTEX2 1718 14.225302 -22.902075 1.989436
-VERTEX2 1719 13.831206 -21.988265 1.679766
-VERTEX2 1720 13.942264 -22.968586 -1.719611
-VERTEX2 1721 13.786911 -23.956171 -1.954348
-VERTEX2 1722 13.427473 -24.903214 -2.062596
-VERTEX2 1723 12.955224 -25.792280 -1.886991
-VERTEX2 1724 12.027889 -25.463822 2.715396
-VERTEX2 1725 11.100828 -25.025636 2.516791
-VERTEX2 1726 10.240717 -24.441827 2.176068
-VERTEX2 1727 9.639539 -23.601916 2.251796
-VERTEX2 1728 10.445976 -23.002008 0.733049
-VERTEX2 1729 11.177493 -22.353631 0.665551
-VERTEX2 1730 11.961226 -21.755415 0.632314
-VERTEX2 1731 12.799108 -21.187099 0.506368
-VERTEX2 1732 11.944406 -21.657242 -2.514971
-VERTEX2 1733 11.183784 -22.279182 -2.113928
-VERTEX2 1734 10.692603 -23.166757 -2.036192
-VERTEX2 1735 10.243728 -24.037473 -1.820559
-VERTEX2 1736 11.223407 -24.262168 -0.166479
-VERTEX2 1737 12.250466 -24.405798 -0.136377
-VERTEX2 1738 13.268872 -24.542316 0.284769
-VERTEX2 1739 14.245527 -24.266316 -0.015757
-VERTEX2 1740 15.247582 -24.284651 -0.135228
-VERTEX2 1741 16.202664 -24.417400 0.090335
-VERTEX2 1742 17.218722 -24.344522 0.054494
-VERTEX2 1743 18.187123 -24.336116 0.134581
-VERTEX2 1744 18.330431 -25.326413 -1.534036
-VERTEX2 1745 18.338536 -26.326421 -1.270098
-VERTEX2 1746 18.633658 -27.220970 -1.091941
-VERTEX2 1747 19.103889 -28.073178 -1.209072
-VERTEX2 1748 19.495613 -28.971661 -1.264148
-VERTEX2 1749 19.829051 -29.918780 -1.399196
-VERTEX2 1750 20.019587 -30.896631 -1.244813
-VERTEX2 1751 20.359409 -31.849752 -1.717046
-VERTEX2 1752 19.378888 -31.719379 -3.083623
-VERTEX2 1753 18.395952 -31.812323 2.875448
-VERTEX2 1754 17.460289 -31.535108 2.447458
-VERTEX2 1755 16.694526 -30.902383 2.399079
-VERTEX2 1756 17.381623 -30.185881 1.102483
-VERTEX2 1757 17.824056 -29.294047 1.483526
-VERTEX2 1758 17.911189 -28.273849 1.791769
-VERTEX2 1759 17.697059 -27.314201 1.521233
-VERTEX2 1760 18.696518 -27.336527 0.138675
-VERTEX2 1761 19.651263 -27.194477 0.021422
-VERTEX2 1762 20.669028 -27.136202 -0.450249
-VERTEX2 1763 21.564318 -27.568355 -0.226821
-VERTEX2 1764 21.784475 -26.621382 1.105273
-VERTEX2 1765 22.200794 -25.707225 0.852696
-VERTEX2 1766 22.845389 -24.952401 0.513170
-VERTEX2 1767 23.700425 -24.516646 0.462383
-VERTEX2 1768 24.153881 -25.383615 -0.834477
-VERTEX2 1769 24.801955 -26.108794 -1.111628
-VERTEX2 1770 25.240496 -27.031716 -0.720958
-VERTEX2 1771 25.984150 -27.729115 -0.824628
-VERTEX2 1772 25.300868 -27.038282 2.296014
-VERTEX2 1773 24.650533 -26.288835 2.397707
-VERTEX2 1774 23.908965 -25.617373 2.699787
-VERTEX2 1775 22.979939 -25.171487 2.548189
-VERTEX2 1776 23.491827 -24.310742 1.328625
-VERTEX2 1777 23.782602 -23.401166 1.443955
-VERTEX2 1778 23.906208 -22.386340 1.184545
-VERTEX2 1779 24.279882 -21.436222 0.831450
-VERTEX2 1780 23.570689 -20.783925 2.243820
-VERTEX2 1781 22.978084 -20.031776 2.093581
-VERTEX2 1782 22.481168 -19.208119 2.518206
-VERTEX2 1783 21.679350 -18.658324 2.254628
-VERTEX2 1784 22.444829 -18.000309 1.260003
-VERTEX2 1785 22.713482 -17.076704 0.731993
-VERTEX2 1786 23.449572 -16.445919 0.705359
-VERTEX2 1787 24.253467 -15.763108 0.608231
-VERTEX2 1788 23.684007 -14.940172 2.228144
-VERTEX2 1789 23.054970 -14.175160 2.372162
-VERTEX2 1790 22.363498 -13.482183 2.212860
-VERTEX2 1791 21.785040 -12.667059 2.273087
-VERTEX2 1792 22.415409 -13.432423 -1.191559
-VERTEX2 1793 22.781753 -14.377638 -1.369792
-VERTEX2 1794 22.978561 -15.379200 -1.048264
-VERTEX2 1795 23.435536 -16.256677 -0.845638
-VERTEX2 1796 22.774897 -15.516292 2.071290
-VERTEX2 1797 22.310251 -14.613289 2.348802
-VERTEX2 1798 21.608518 -13.909366 2.305463
-VERTEX2 1799 20.952169 -13.176668 2.364102
-VERTEX2 1800 20.285632 -13.889227 -2.255332
-VERTEX2 1801 19.622117 -14.660727 -2.189945
-VERTEX2 1802 19.049831 -15.468281 -1.745925
-VERTEX2 1803 18.886528 -16.433734 -1.695583
-VERTEX2 1804 19.868526 -16.596708 -0.097835
-VERTEX2 1805 20.884843 -16.689188 -0.153790
-VERTEX2 1806 21.888454 -16.827931 -0.426018
-VERTEX2 1807 22.785698 -17.225876 -0.382707
-VERTEX2 1808 22.420932 -18.141957 -1.863235
-VERTEX2 1809 22.144136 -19.126124 -1.753760
-VERTEX2 1810 21.959110 -20.109615 -1.606713
-VERTEX2 1811 21.909948 -21.104834 -1.401944
-VERTEX2 1812 22.099506 -22.106382 -1.518397
-VERTEX2 1813 22.176006 -23.122930 -1.413480
-VERTEX2 1814 22.328449 -24.080797 -1.249107
-VERTEX2 1815 22.652580 -25.071549 -1.341468
-VERTEX2 1816 21.693068 -25.294566 -2.912701
-VERTEX2 1817 20.766636 -25.509906 -3.102302
-VERTEX2 1818 19.751726 -25.540782 -3.053132
-VERTEX2 1819 18.761694 -25.631521 -3.099217
-VERTEX2 1820 19.754909 -25.594682 0.444874
-VERTEX2 1821 20.685727 -25.179285 0.591190
-VERTEX2 1822 21.473541 -24.640240 0.509611
-VERTEX2 1823 22.372349 -24.187530 0.349181
-VERTEX2 1824 22.719676 -25.154396 -1.757726
-VERTEX2 1825 22.558013 -26.129785 -1.720861
-VERTEX2 1826 22.410518 -27.121409 -1.904430
-VERTEX2 1827 22.059379 -28.076821 -1.853115
-VERTEX2 1828 22.331279 -27.124919 0.995949
-VERTEX2 1829 22.872094 -26.273988 1.157535
-VERTEX2 1830 23.254266 -25.334367 1.015751
-VERTEX2 1831 23.797699 -24.494103 1.103657
-VERTEX2 1832 22.883850 -24.039306 2.515020
-VERTEX2 1833 22.047174 -23.471138 2.537821
-VERTEX2 1834 21.232329 -22.930407 2.759291
-VERTEX2 1835 20.294202 -22.531465 2.782793
-VERTEX2 1836 19.955237 -23.435603 -1.887316
-VERTEX2 1837 19.621781 -24.431861 -1.594106
-VERTEX2 1838 19.611846 -25.421878 -1.667989
-VERTEX2 1839 19.516586 -26.384398 -1.542278
-VERTEX2 1840 20.464635 -26.325356 0.229888
-VERTEX2 1841 21.456784 -26.086087 0.480958
-VERTEX2 1842 22.314715 -25.613862 0.559276
-VERTEX2 1843 23.114365 -25.077185 1.054913
-VERTEX2 1844 23.613982 -24.164084 0.794345
-VERTEX2 1845 24.325235 -23.452977 0.644367
-VERTEX2 1846 25.113252 -22.850752 0.741004
-VERTEX2 1847 25.827315 -22.192289 0.649681
-VERTEX2 1848 26.397981 -22.963796 -0.809892
-VERTEX2 1849 27.070640 -23.679535 -1.080842
-VERTEX2 1850 27.554681 -24.560913 -1.052435
-VERTEX2 1851 28.048761 -25.411355 -0.883032
-VERTEX2 1852 28.818562 -24.758955 0.495234
-VERTEX2 1853 29.696275 -24.298665 0.437038
-VERTEX2 1854 30.617009 -23.884552 0.044959
-VERTEX2 1855 31.620492 -23.831353 -0.100926
-VERTEX2 1856 31.511599 -24.797240 -1.614870
-VERTEX2 1857 31.477335 -25.809353 -1.348883
-VERTEX2 1858 31.701090 -26.747557 -1.049160
-VERTEX2 1859 32.174877 -27.599921 -1.008624
-VERTEX2 1860 31.676519 -26.758141 1.993118
-VERTEX2 1861 31.220093 -25.821892 1.689256
-VERTEX2 1862 31.106326 -24.833306 1.867989
-VERTEX2 1863 30.776798 -23.863479 1.955768
-VERTEX2 1864 29.869201 -24.264966 -2.838809
-VERTEX2 1865 28.897963 -24.544575 -2.903561
-VERTEX2 1866 27.957606 -24.748225 -2.869693
-VERTEX2 1867 26.989675 -24.982600 -3.012004
-VERTEX2 1868 27.107167 -26.028517 -1.567575
-VERTEX2 1869 27.111748 -27.000345 -1.605734
-VERTEX2 1870 27.063293 -28.005596 -1.719526
-VERTEX2 1871 26.907760 -29.005872 -1.458050
-VERTEX2 1872 25.934742 -29.126938 -2.996553
-VERTEX2 1873 24.970099 -29.271608 -2.877286
-VERTEX2 1874 24.033072 -29.489081 -2.421203
-VERTEX2 1875 23.268442 -30.167479 -2.469452
-VERTEX2 1876 23.872317 -30.948370 -0.973505
-VERTEX2 1877 24.432057 -31.789573 -1.224135
-VERTEX2 1878 24.768116 -32.734889 -1.160977
-VERTEX2 1879 25.108421 -33.612194 -1.249975
-VERTEX2 1880 26.039689 -33.280194 0.184021
-VERTEX2 1881 27.041482 -33.098151 0.307038
-VERTEX2 1882 27.984091 -32.845927 0.060102
-VERTEX2 1883 29.015324 -32.772350 -0.460353
-VERTEX2 1884 28.060693 -32.337450 2.992631
-VERTEX2 1885 27.055601 -32.203437 -3.032559
-VERTEX2 1886 26.054025 -32.334680 -2.902141
-VERTEX2 1887 25.079566 -32.589745 3.030436
-VERTEX2 1888 24.960098 -33.573074 -1.783570
-VERTEX2 1889 24.730844 -34.596164 -1.329993
-VERTEX2 1890 24.901220 -35.555212 -1.166243
-VERTEX2 1891 25.295593 -36.517958 -1.307889
-VERTEX2 1892 24.333081 -36.796082 -2.992481
-VERTEX2 1893 23.336424 -36.968893 -3.040522
-VERTEX2 1894 22.335324 -37.058077 -3.070255
-VERTEX2 1895 21.335672 -37.080269 3.003615
-VERTEX2 1896 21.486032 -36.094012 1.851953
-VERTEX2 1897 21.251291 -35.176668 2.071307
-VERTEX2 1898 20.747798 -34.318798 2.152040
-VERTEX2 1899 20.203217 -33.442144 2.411840
-VERTEX2 1900 20.999364 -34.128825 -0.899894
-VERTEX2 1901 21.621150 -34.871606 -0.914335
-VERTEX2 1902 22.236365 -35.678951 -0.874999
-VERTEX2 1903 22.875100 -36.483755 -1.105618
-VERTEX2 1904 21.986585 -36.945136 -2.521045
-VERTEX2 1905 21.181085 -37.524096 -2.576313
-VERTEX2 1906 20.354205 -38.031674 -2.316695
-VERTEX2 1907 19.663793 -38.770381 -2.175071
-VERTEX2 1908 20.526706 -39.395070 -0.727832
-VERTEX2 1909 21.250132 -40.065081 -0.460933
-VERTEX2 1910 22.199517 -40.492310 -0.246757
-VERTEX2 1911 23.137988 -40.735821 -0.066740
-VERTEX2 1912 23.173873 -39.734916 1.690051
-VERTEX2 1913 23.057947 -38.734925 1.603428
-VERTEX2 1914 23.033154 -37.722875 1.279334
-VERTEX2 1915 23.311423 -36.781925 1.401527
-VERTEX2 1916 22.314522 -36.627550 -3.114302
-VERTEX2 1917 21.318973 -36.648775 -2.889658
-VERTEX2 1918 20.333238 -36.867300 -3.098692
-VERTEX2 1919 19.335113 -36.869854 3.046968
-VERTEX2 1920 19.412078 -35.829100 1.629980
-VERTEX2 1921 19.361068 -34.813870 1.789559
-VERTEX2 1922 19.115530 -33.846183 1.966744
-VERTEX2 1923 18.710338 -32.966487 1.757512
-VERTEX2 1924 17.737448 -33.155240 -2.834580
-VERTEX2 1925 16.770775 -33.472893 -2.764309
-VERTEX2 1926 15.833352 -33.780144 -2.581710
-VERTEX2 1927 15.024712 -34.306447 -3.059823
-VERTEX2 1928 14.965991 -33.304445 1.458796
-VERTEX2 1929 15.114358 -32.288311 1.782184
-VERTEX2 1930 14.883481 -31.263685 1.978915
-VERTEX2 1931 14.467533 -30.396057 1.637206
-VERTEX2 1932 15.461384 -30.310066 -0.111649
-VERTEX2 1933 16.479075 -30.383981 -0.560154
-VERTEX2 1934 17.347429 -30.934544 -0.552268
-VERTEX2 1935 18.241932 -31.405946 -0.536163
-VERTEX2 1936 17.375514 -30.914342 2.483964
-VERTEX2 1937 16.574615 -30.336329 2.323580
-VERTEX2 1938 15.854583 -29.588216 2.139198
-VERTEX2 1939 15.350217 -28.737285 1.781232
-VERTEX2 1940 15.166376 -27.737001 1.511665
-VERTEX2 1941 15.217281 -26.756218 1.620417
-VERTEX2 1942 15.152203 -25.756776 1.638181
-VERTEX2 1943 15.067420 -24.776699 1.850400
-VERTEX2 1944 14.768588 -23.836877 1.983482
-VERTEX2 1945 14.353805 -22.906867 1.992002
-VERTEX2 1946 13.961027 -22.005851 1.921221
-VERTEX2 1947 13.613177 -21.067554 1.608218
-VERTEX2 1948 12.563894 -21.110619 3.072896
-VERTEX2 1949 11.577613 -21.039123 3.047811
-VERTEX2 1950 10.576657 -20.928677 2.982235
-VERTEX2 1951 9.583285 -20.758314 2.637231
-VERTEX2 1952 10.037809 -19.881573 0.894581
-VERTEX2 1953 10.636042 -19.143649 0.706496
-VERTEX2 1954 11.414467 -18.516914 0.797824
-VERTEX2 1955 12.188280 -17.804025 0.804905
-VERTEX2 1956 11.520338 -17.108207 2.291134
-VERTEX2 1957 10.881434 -16.420438 2.078332
-VERTEX2 1958 10.354691 -15.571693 1.873298
-VERTEX2 1959 10.074507 -14.616579 1.462665
-VERTEX2 1960 9.079434 -14.492426 -2.946963
-VERTEX2 1961 8.054350 -14.726550 3.114575
-VERTEX2 1962 7.052164 -14.679189 2.623212
-VERTEX2 1963 6.218016 -14.167258 2.543904
-VERTEX2 1964 7.036643 -14.747587 -0.428529
-VERTEX2 1965 7.935013 -15.136008 -0.623426
-VERTEX2 1966 8.782942 -15.704420 -0.667008
-VERTEX2 1967 9.557828 -16.321589 -0.653508
-VERTEX2 1968 8.959927 -17.078912 -2.233094
-VERTEX2 1969 8.328686 -17.862066 -2.004690
-VERTEX2 1970 7.901910 -18.779545 -2.296508
-VERTEX2 1971 7.211393 -19.499037 -2.051770
-VERTEX2 1972 7.678326 -18.653918 1.344800
-VERTEX2 1973 7.931913 -17.664235 1.423264
-VERTEX2 1974 8.102010 -16.689091 1.241050
-VERTEX2 1975 8.417165 -15.742879 1.000052
-VERTEX2 1976 7.586007 -15.190506 2.441261
-VERTEX2 1977 6.806028 -14.569638 2.310679
-VERTEX2 1978 6.127658 -13.803984 2.366353
-VERTEX2 1979 5.388541 -13.115525 2.558059
-VERTEX2 1980 5.958448 -12.263893 1.210710
-VERTEX2 1981 6.301673 -11.344309 1.387750
-VERTEX2 1982 6.500612 -10.356305 1.350387
-VERTEX2 1983 6.735116 -9.381112 1.339109
-VERTEX2 1984 7.721308 -9.604304 -0.200548
-VERTEX2 1985 8.705419 -9.825395 -0.078371
-VERTEX2 1986 9.720132 -9.919550 -0.248594
-VERTEX2 1987 10.694996 -10.189480 -0.132358
-VERTEX2 1988 11.711386 -10.322992 -0.167463
-VERTEX2 1989 12.681814 -10.469991 -0.542868
-VERTEX2 1990 13.545249 -11.007453 -0.690550
-VERTEX2 1991 14.278614 -11.645094 -0.657445
-VERTEX2 1992 13.491671 -10.994362 2.543870
-VERTEX2 1993 12.693214 -10.419759 2.653261
-VERTEX2 1994 11.826313 -9.947264 2.736116
-VERTEX2 1995 10.892160 -9.577052 2.879871
-VERTEX2 1996 10.665167 -10.562611 -1.756981
-VERTEX2 1997 10.481875 -11.556437 -1.863485
-VERTEX2 1998 10.213356 -12.493519 -1.679083
-VERTEX2 1999 10.112877 -13.519107 -1.679270
-VERTEX2 2000 9.996702 -14.504869 -1.722846
-VERTEX2 2001 9.847718 -15.433365 -1.901594
-VERTEX2 2002 9.507388 -16.446146 -1.737197
-VERTEX2 2003 9.327656 -17.473040 -1.898854
-VERTEX2 2004 9.619802 -16.506781 1.414759
-VERTEX2 2005 9.792877 -15.519462 1.138887
-VERTEX2 2006 10.225127 -14.610767 1.144517
-VERTEX2 2007 10.623966 -13.677593 1.021757
-VERTEX2 2008 10.108131 -14.551941 -2.155859
-VERTEX2 2009 9.544655 -15.406096 -2.416177
-VERTEX2 2010 8.837374 -16.086921 -2.533259
-VERTEX2 2011 7.991043 -16.647725 -2.339765
-VERTEX2 2012 8.658521 -15.927790 1.278537
-VERTEX2 2013 8.916649 -14.982411 1.455077
-VERTEX2 2014 9.033363 -13.980517 1.100724
-VERTEX2 2015 9.427718 -13.109229 0.982329
-VERTEX2 2016 10.283308 -13.693349 -0.454460
-VERTEX2 2017 11.205465 -14.128961 -0.575490
-VERTEX2 2018 12.062740 -14.668257 -0.151523
-VERTEX2 2019 13.072306 -14.783207 -0.180705
-VERTEX2 2020 12.931904 -15.717144 -1.503000
-VERTEX2 2021 13.006119 -16.707598 -1.448046
-VERTEX2 2022 13.101222 -17.687637 -1.432216
-VERTEX2 2023 13.267419 -18.716099 -1.081541
-VERTEX2 2024 14.158379 -18.249037 0.266006
-VERTEX2 2025 15.097948 -17.982353 0.180686
-VERTEX2 2026 16.106358 -17.826006 -0.105966
-VERTEX2 2027 17.081835 -17.887936 -0.077834
-VERTEX2 2028 16.100468 -17.825254 3.128935
-VERTEX2 2029 15.109006 -17.801997 -3.053619
-VERTEX2 2030 14.085811 -17.890912 -2.861588
-VERTEX2 2031 13.087614 -18.117099 -3.120434
-VERTEX2 2032 13.054251 -17.121943 1.853901
-VERTEX2 2033 12.768936 -16.139640 1.456008
-VERTEX2 2034 12.873661 -15.155726 1.528943
-VERTEX2 2035 12.867329 -14.159101 1.136549
-VERTEX2 2036 11.960992 -13.716931 2.856711
-VERTEX2 2037 10.979223 -13.407474 2.902746
-VERTEX2 2038 10.013484 -13.150923 3.087892
-VERTEX2 2039 9.033505 -13.091471 -3.034051
-VERTEX2 2040 10.045984 -13.009023 0.401651
-VERTEX2 2041 10.939103 -12.604501 0.278322
-VERTEX2 2042 11.870567 -12.328232 -0.002820
-VERTEX2 2043 12.877887 -12.358009 -0.444935
-VERTEX2 2044 12.444887 -13.273744 -1.593388
-VERTEX2 2045 12.460959 -14.310221 -1.353664
-VERTEX2 2046 12.628606 -15.228939 -1.217664
-VERTEX2 2047 12.975044 -16.165401 -1.125098
-VERTEX2 2048 12.549190 -15.250050 1.864586
-VERTEX2 2049 12.239352 -14.283992 1.865447
-VERTEX2 2050 11.936923 -13.324519 1.807929
-VERTEX2 2051 11.717472 -12.392707 1.891992
-VERTEX2 2052 12.655201 -12.088025 0.177082
-VERTEX2 2053 13.655009 -11.901905 0.663465
-VERTEX2 2054 14.456159 -11.360342 0.440637
-VERTEX2 2055 15.368826 -10.986541 0.594833
-VERTEX2 2056 14.547893 -11.550122 -2.360796
-VERTEX2 2057 13.858556 -12.232584 -2.428360
-VERTEX2 2058 13.099044 -12.874210 -1.842033
-VERTEX2 2059 12.834491 -13.864131 -1.931744
-VERTEX2 2060 13.193384 -12.930550 1.160108
-VERTEX2 2061 13.573808 -11.987896 1.642037
-VERTEX2 2062 13.520214 -11.032762 1.959113
-VERTEX2 2063 13.108643 -10.121807 1.695589
-VERTEX2 2064 14.066193 -10.015114 0.334693
-VERTEX2 2065 15.046398 -9.688257 0.372260
-VERTEX2 2066 16.018696 -9.331592 0.497512
-VERTEX2 2067 16.883015 -8.811267 0.423120
-VERTEX2 2068 15.961370 -9.214725 -2.978181
-VERTEX2 2069 15.003473 -9.373583 3.049600
-VERTEX2 2070 14.056253 -9.289979 2.817990
-VERTEX2 2071 13.076386 -8.952787 2.742744
-VERTEX2 2072 13.471184 -8.024193 0.939217
-VERTEX2 2073 14.025678 -7.172590 1.149938
-VERTEX2 2074 14.414961 -6.268030 1.180413
-VERTEX2 2075 14.817619 -5.325095 1.237178
-VERTEX2 2076 13.903635 -5.021524 2.587001
-VERTEX2 2077 13.062522 -4.522314 2.750186
-VERTEX2 2078 12.147325 -4.162166 2.247283
-VERTEX2 2079 11.550362 -3.362584 2.136594
-VERTEX2 2080 10.681289 -3.920163 -2.459450
-VERTEX2 2081 9.855435 -4.545237 -2.027666
-VERTEX2 2082 9.400045 -5.454591 -2.104997
-VERTEX2 2083 8.889349 -6.302947 -1.974091
-VERTEX2 2084 9.292772 -5.445500 1.655412
-VERTEX2 2085 9.210300 -4.485009 1.482808
-VERTEX2 2086 9.260277 -3.459935 1.292952
-VERTEX2 2087 9.490137 -2.491057 1.812231
-VERTEX2 2088 9.695514 -3.451143 -1.316438
-VERTEX2 2089 9.954018 -4.460493 -1.070273
-VERTEX2 2090 10.464769 -5.324464 -1.014123
-VERTEX2 2091 11.006284 -6.187617 -0.894438
-VERTEX2 2092 10.352194 -5.378613 2.359398
-VERTEX2 2093 9.658803 -4.701575 2.426344
-VERTEX2 2094 8.918414 -4.048244 1.868554
-VERTEX2 2095 8.627922 -3.027512 1.747830
-VERTEX2 2096 9.640976 -2.849448 0.095590
-VERTEX2 2097 10.610224 -2.757624 -0.061594
-VERTEX2 2098 11.632203 -2.822594 -0.211931
-VERTEX2 2099 12.662142 -3.055987 -0.323974
-VERTEX2 2100 13.621674 -3.361874 -0.549102
-VERTEX2 2101 14.479857 -3.883266 -0.555412
-VERTEX2 2102 15.333497 -4.437618 -0.717069
-VERTEX2 2103 16.059734 -5.099755 -0.880957
-VERTEX2 2104 15.434423 -4.383800 1.918270
-VERTEX2 2105 15.083484 -3.423033 2.110434
-VERTEX2 2106 14.543505 -2.543735 1.736616
-VERTEX2 2107 14.394164 -1.574150 1.879086
-VERTEX2 2108 14.693076 -2.551615 -1.299141
-VERTEX2 2109 14.943459 -3.533914 -1.157964
-VERTEX2 2110 15.359268 -4.451410 -1.288969
-VERTEX2 2111 15.627979 -5.420552 -1.171888
-VERTEX2 2112 16.572903 -5.085866 0.562751
-VERTEX2 2113 17.420722 -4.523215 0.612230
-VERTEX2 2114 18.273200 -3.975621 0.597165
-VERTEX2 2115 19.132434 -3.429078 0.626891
-VERTEX2 2116 18.334100 -4.017656 -2.533130
-VERTEX2 2117 17.514904 -4.599905 -2.522672
-VERTEX2 2118 16.701050 -5.208140 -2.585576
-VERTEX2 2119 15.868221 -5.753119 -2.932871
-VERTEX2 2120 16.836319 -5.576416 0.429938
-VERTEX2 2121 17.759035 -5.149657 0.296579
-VERTEX2 2122 18.683820 -4.814962 0.314129
-VERTEX2 2123 19.649088 -4.452207 0.262564
-VERTEX2 2124 19.913732 -5.404194 -1.169001
-VERTEX2 2125 20.305241 -6.342796 -0.860738
-VERTEX2 2126 20.994856 -7.079586 -1.286526
-VERTEX2 2127 21.245758 -8.060819 -1.262426
-VERTEX2 2128 20.302018 -8.369603 -2.907513
-VERTEX2 2129 19.347098 -8.610141 3.103778
-VERTEX2 2130 18.341701 -8.570424 3.078095
-VERTEX2 2131 17.317795 -8.497183 3.044070
-VERTEX2 2132 18.368523 -8.618826 0.353536
-VERTEX2 2133 19.299040 -8.245819 0.053762
-VERTEX2 2134 20.318545 -8.222034 0.467813
-VERTEX2 2135 21.230415 -7.749672 0.515757
-VERTEX2 2136 22.082729 -7.240496 0.656665
-VERTEX2 2137 22.799009 -6.644345 0.578672
-VERTEX2 2138 23.656461 -6.041744 0.387205
-VERTEX2 2139 24.601580 -5.649319 0.519414
-VERTEX2 2140 24.120458 -4.812408 2.258113
-VERTEX2 2141 23.490265 -3.993553 2.429138
-VERTEX2 2142 22.701113 -3.296848 2.376443
-VERTEX2 2143 21.966616 -2.588400 2.549854
-VERTEX2 2144 21.390450 -3.422442 -2.232206
-VERTEX2 2145 20.723620 -4.168981 -1.986272
-VERTEX2 2146 20.323964 -5.093745 -1.961396
-VERTEX2 2147 19.963712 -5.999328 -1.995820
-VERTEX2 2148 20.366949 -5.108150 0.774365
-VERTEX2 2149 21.070051 -4.361910 1.208041
-VERTEX2 2150 21.442480 -3.438647 1.345018
-VERTEX2 2151 21.671431 -2.417392 1.283924
-VERTEX2 2152 22.631746 -2.646627 -0.270361
-VERTEX2 2153 23.602923 -2.918948 -0.315064
-VERTEX2 2154 24.584590 -3.212012 -0.412804
-VERTEX2 2155 25.507339 -3.644218 -0.557519
-VERTEX2 2156 26.013705 -2.789109 0.696420
-VERTEX2 2157 26.770953 -2.172772 0.978632
-VERTEX2 2158 27.311775 -1.345006 1.162804
-VERTEX2 2159 27.707051 -0.435951 1.112706
-VERTEX2 2160 27.236426 -1.381724 -2.113208
-VERTEX2 2161 26.741147 -2.260445 -1.891885
-VERTEX2 2162 26.413749 -3.198366 -2.249791
-VERTEX2 2163 25.804693 -3.963478 -2.826022
-VERTEX2 2164 25.480794 -3.018109 1.731837
-VERTEX2 2165 25.357665 -2.008934 1.573301
-VERTEX2 2166 25.347712 -0.972416 1.846029
-VERTEX2 2167 25.096825 -0.009831 1.702721
-VERTEX2 2168 25.177467 -1.038552 -1.801535
-VERTEX2 2169 24.925298 -2.053746 -2.147616
-VERTEX2 2170 24.420110 -2.920770 -2.204938
-VERTEX2 2171 23.792226 -3.725069 -2.089592
-VERTEX2 2172 24.657570 -4.252545 -1.066255
-VERTEX2 2173 25.141831 -5.147060 -1.172120
-VERTEX2 2174 25.495881 -6.057589 -1.363067
-VERTEX2 2175 25.699012 -7.009365 -1.107727
-VERTEX2 2176 26.633390 -6.573845 0.864769
-VERTEX2 2177 27.313465 -5.810772 0.959424
-VERTEX2 2178 27.889192 -4.998769 1.080606
-VERTEX2 2179 28.314966 -4.144288 0.949008
-VERTEX2 2180 27.516461 -3.557687 2.236037
-VERTEX2 2181 26.896402 -2.739557 2.207062
-VERTEX2 2182 26.279579 -1.947140 2.382217
-VERTEX2 2183 25.544029 -1.286394 2.822213
-VERTEX2 2184 26.558473 -1.613856 -0.289028
-VERTEX2 2185 27.509913 -1.874682 -0.450095
-VERTEX2 2186 28.414950 -2.314353 -0.666039
-VERTEX2 2187 29.173561 -2.916246 -0.884270
-VERTEX2 2188 29.912928 -2.293654 0.831300
-VERTEX2 2189 30.632511 -1.534280 0.879933
-VERTEX2 2190 31.254364 -0.774519 0.916572
-VERTEX2 2191 31.870142 0.007271 1.038863
-VERTEX2 2192 31.400688 -0.832030 -2.113055
-VERTEX2 2193 30.865929 -1.697934 -2.267900
-VERTEX2 2194 30.245033 -2.454934 -2.194610
-VERTEX2 2195 29.657300 -3.278293 -2.061503
-VERTEX2 2196 30.117975 -2.412780 0.879003
-VERTEX2 2197 30.744404 -1.648804 0.403144
-VERTEX2 2198 31.698645 -1.290267 0.241638
-VERTEX2 2199 32.626187 -1.067237 0.030100
-VERTEX2 2200 32.574166 -0.065019 1.454875
-VERTEX2 2201 32.665154 0.932975 1.652727
-VERTEX2 2202 32.596640 1.947240 1.484963
-VERTEX2 2203 32.679674 2.938831 1.489335
-VERTEX2 2204 32.576754 1.875500 -1.090494
-VERTEX2 2205 33.041426 0.966942 -1.066368
-VERTEX2 2206 33.515245 0.055446 -1.268111
-VERTEX2 2207 33.829057 -0.928209 -1.228127
-VERTEX2 2208 33.513788 0.005820 1.607649
-VERTEX2 2209 33.558254 0.980402 1.464757
-VERTEX2 2210 33.653804 1.969111 1.359340
-VERTEX2 2211 33.864155 2.924077 1.357852
-VERTEX2 2212 34.861609 2.701991 0.059646
-VERTEX2 2213 35.870686 2.762183 0.095182
-VERTEX2 2214 36.850355 2.888637 0.054098
-VERTEX2 2215 37.879127 2.940107 0.111157
-VERTEX2 2216 37.963220 1.904726 -1.686281
-VERTEX2 2217 37.851450 0.917953 -1.578091
-VERTEX2 2218 37.837855 -0.081863 -1.698341
-VERTEX2 2219 37.685644 -1.055563 -1.573592
-VERTEX2 2220 37.692101 -0.057424 1.488268
-VERTEX2 2221 37.771732 0.920460 1.285527
-VERTEX2 2222 38.066775 1.872225 0.906773
-VERTEX2 2223 38.676321 2.686380 0.944182
-VERTEX2 2224 38.096904 1.890703 -2.152043
-VERTEX2 2225 37.583950 1.081733 -2.209271
-VERTEX2 2226 37.018286 0.318232 -2.472663
-VERTEX2 2227 36.260736 -0.245762 -2.681770
-VERTEX2 2228 35.394227 -0.667285 -2.461651
-VERTEX2 2229 34.610187 -1.261665 -2.603732
-VERTEX2 2230 33.773297 -1.744780 -2.591876
-VERTEX2 2231 32.950502 -2.316007 -2.612287
-VERTEX2 2232 33.504588 -3.157190 -0.833910
-VERTEX2 2233 34.190302 -3.901798 -0.924039
-VERTEX2 2234 34.774729 -4.669068 -0.902865
-VERTEX2 2235 35.396401 -5.464425 -0.545732
-VERTEX2 2236 35.902532 -4.618260 1.162191
-VERTEX2 2237 36.276452 -3.701990 1.619906
-VERTEX2 2238 36.220110 -2.681265 1.676473
-VERTEX2 2239 36.119197 -1.679953 1.851962
-VERTEX2 2240 36.408120 -2.654460 -1.058618
-VERTEX2 2241 36.879868 -3.539634 -0.846150
-VERTEX2 2242 37.549150 -4.278885 -1.025683
-VERTEX2 2243 38.062517 -5.156910 -1.390271
-VERTEX2 2244 39.088549 -4.958956 0.269306
-VERTEX2 2245 40.082055 -4.677502 0.079750
-VERTEX2 2246 41.091253 -4.587756 0.500904
-VERTEX2 2247 41.963734 -4.114583 0.622401
-VERTEX2 2248 41.366742 -3.325543 2.340757
-VERTEX2 2249 40.673415 -2.622035 2.428554
-VERTEX2 2250 39.926209 -1.970222 2.683851
-VERTEX2 2251 39.054537 -1.531873 2.647610
-VERTEX2 2252 39.901442 -2.009481 -0.474075
-VERTEX2 2253 40.754729 -2.455085 -0.414427
-VERTEX2 2254 41.689648 -2.891588 -0.207670
-VERTEX2 2255 42.653816 -3.106400 -0.120524
-VERTEX2 2256 42.767627 -2.108999 1.393508
-VERTEX2 2257 42.958884 -1.150609 1.583481
-VERTEX2 2258 42.970930 -0.139045 1.606548
-VERTEX2 2259 42.929213 0.864956 1.419503
-VERTEX2 2260 42.760748 -0.082270 -1.705689
-VERTEX2 2261 42.645120 -1.070298 -1.960929
-VERTEX2 2262 42.242698 -2.005743 -2.130275
-VERTEX2 2263 41.714831 -2.845206 -1.983876
-VERTEX2 2264 42.119807 -1.917943 0.788329
-VERTEX2 2265 42.806012 -1.246654 0.483224
-VERTEX2 2266 43.693023 -0.813370 0.753465
-VERTEX2 2267 44.406807 -0.137612 0.865046
-VERTEX2 2268 43.613731 0.520754 2.532544
-VERTEX2 2269 42.792117 1.083540 2.729063
-VERTEX2 2270 41.895766 1.479543 2.964192
-VERTEX2 2271 40.908114 1.660699 3.018553
-VERTEX2 2272 41.897665 1.520799 0.041604
-VERTEX2 2273 42.861566 1.545826 -0.121037
-VERTEX2 2274 43.845252 1.416568 -0.179046
-VERTEX2 2275 44.834045 1.246802 -0.239802
-VERTEX2 2276 43.820140 1.471201 2.761743
-VERTEX2 2277 42.893974 1.865291 2.677737
-VERTEX2 2278 42.010242 2.281737 2.808531
-VERTEX2 2279 41.101862 2.577619 2.767165
-VERTEX2 2280 40.752747 1.660332 -1.924102
-VERTEX2 2281 40.413982 0.737706 -2.037565
-VERTEX2 2282 40.006162 -0.136641 -2.032770
-VERTEX2 2283 39.564741 -1.033207 -1.844338
-VERTEX2 2284 38.626354 -0.764126 2.536685
-VERTEX2 2285 37.768548 -0.191792 2.802559
-VERTEX2 2286 36.798702 0.156010 2.963481
-VERTEX2 2287 35.803214 0.335561 -3.022900
-VERTEX2 2288 36.814171 0.445481 0.254260
-VERTEX2 2289 37.770926 0.690908 0.017207
-VERTEX2 2290 38.799004 0.690541 0.003197
-VERTEX2 2291 39.764355 0.696567 -0.038246
-VERTEX2 2292 39.709790 -0.291457 -1.900649
-VERTEX2 2293 39.368403 -1.226009 -1.573870
-VERTEX2 2294 39.367644 -2.247241 -1.639759
-VERTEX2 2295 39.308719 -3.247940 -1.682669
-VERTEX2 2296 39.433539 -2.263747 1.654119
-VERTEX2 2297 39.381511 -1.288556 1.625988
-VERTEX2 2298 39.308634 -0.280040 1.617653
-VERTEX2 2299 39.232957 0.698230 1.693535
-VERTEX2 2300 38.272174 0.548868 -2.875445
-VERTEX2 2301 37.290296 0.286443 -2.733099
-VERTEX2 2302 36.398986 -0.134058 -2.646862
-VERTEX2 2303 35.487791 -0.616625 -2.498724
-VERTEX2 2304 36.299884 -0.032753 0.579321
-VERTEX2 2305 37.160146 0.509817 0.218073
-VERTEX2 2306 38.198503 0.754677 0.598487
-VERTEX2 2307 39.027122 1.305447 0.497319
-VERTEX2 2308 39.501419 0.424985 -0.747835
-VERTEX2 2309 40.277413 -0.267181 -0.462374
-VERTEX2 2310 41.146363 -0.692650 -0.285322
-VERTEX2 2311 42.106847 -0.966037 -0.145091
-VERTEX2 2312 41.130375 -0.812989 2.983028
-VERTEX2 2313 40.104986 -0.625371 2.965402
-VERTEX2 2314 39.104244 -0.423346 2.865333
-VERTEX2 2315 38.116493 -0.153396 2.750434
-VERTEX2 2316 38.486626 0.757346 1.068728
-VERTEX2 2317 38.986714 1.604417 1.389058
-VERTEX2 2318 39.175936 2.589378 1.002834
-VERTEX2 2319 39.713461 3.442757 0.959759
-VERTEX2 2320 39.120312 2.672212 -2.221525
-VERTEX2 2321 38.515374 1.922734 -2.059599
-VERTEX2 2322 38.048317 1.014366 -1.997977
-VERTEX2 2323 37.651347 0.108912 -2.190296
-VERTEX2 2324 38.411478 -0.469661 -0.378791
-VERTEX2 2325 39.342645 -0.825896 -0.072644
-VERTEX2 2326 40.317334 -0.896873 -0.017253
-VERTEX2 2327 41.313627 -0.904350 -0.003349
-VERTEX2 2328 40.331139 -0.893967 -3.044939
-VERTEX2 2329 39.337845 -0.978108 3.134128
-VERTEX2 2330 38.306635 -0.928747 -2.871556
-VERTEX2 2331 37.331393 -1.221925 3.050444
-VERTEX2 2332 37.402106 -0.251029 1.784601
-VERTEX2 2333 37.176174 0.726705 1.649843
-VERTEX2 2334 37.090889 1.722438 1.496615
-VERTEX2 2335 37.180402 2.738017 1.516137
-VERTEX2 2336 37.132646 1.748256 -1.556697
-VERTEX2 2337 37.150003 0.701037 -1.458242
-VERTEX2 2338 37.283998 -0.283190 -1.320520
-VERTEX2 2339 37.574463 -1.225611 -1.567834
-VERTEX2 2340 37.593524 -0.220760 1.677826
-VERTEX2 2341 37.447139 0.737012 1.829884
-VERTEX2 2342 37.209753 1.687140 1.938633
-VERTEX2 2343 36.817005 2.618393 1.933467
-VERTEX2 2344 35.934477 2.282430 -2.722391
-VERTEX2 2345 35.011859 1.867234 -2.850560
-VERTEX2 2346 34.053886 1.587262 -2.836302
-VERTEX2 2347 33.143580 1.265047 -2.663036
-VERTEX2 2348 33.612133 0.382434 -1.390015
-VERTEX2 2349 33.819154 -0.626445 -1.448829
-VERTEX2 2350 33.932438 -1.646611 -1.737504
-VERTEX2 2351 33.762990 -2.642865 -1.472884
-VERTEX2 2352 32.767375 -2.750318 -2.931677
-VERTEX2 2353 31.778944 -2.926807 -2.685156
-VERTEX2 2354 30.887336 -3.375170 -2.754334
-VERTEX2 2355 29.971188 -3.722892 -2.586876
-VERTEX2 2356 30.812350 -3.154201 0.631278
-VERTEX2 2357 31.621458 -2.557705 0.842430
-VERTEX2 2358 32.275406 -1.821036 0.918686
-VERTEX2 2359 32.878328 -1.040285 1.011854
-VERTEX2 2360 32.019394 -0.513362 2.230409
-VERTEX2 2361 31.446856 0.306205 2.496857
-VERTEX2 2362 30.616503 0.913295 2.419328
-VERTEX2 2363 29.847131 1.569189 2.278456
-VERTEX2 2364 30.634158 2.230501 0.396478
-VERTEX2 2365 31.550700 2.628293 0.416440
-VERTEX2 2366 32.465875 3.031080 0.450912
-VERTEX2 2367 33.344588 3.467452 0.463950
-VERTEX2 2368 32.488328 3.034921 -2.546626
-VERTEX2 2369 31.639329 2.460063 -2.627445
-VERTEX2 2370 30.768853 2.000207 -2.402613
-VERTEX2 2371 30.039193 1.296475 -2.471817
-VERTEX2 2372 30.674321 0.497193 -1.149636
-VERTEX2 2373 31.114490 -0.387510 -1.227918
-VERTEX2 2374 31.445270 -1.348458 -1.393112
-VERTEX2 2375 31.617538 -2.286627 -1.342861
-VERTEX2 2376 32.579147 -2.020469 0.355277
-VERTEX2 2377 33.511910 -1.655004 0.107777
-VERTEX2 2378 34.525954 -1.552836 0.161809
-VERTEX2 2379 35.524153 -1.398338 0.052993
-VERTEX2 2380 35.451953 -0.405712 1.793650
-VERTEX2 2381 35.243546 0.592718 1.647497
-VERTEX2 2382 35.176637 1.581846 1.651700
-VERTEX2 2383 35.127496 2.571356 1.555239
-VERTEX2 2384 34.092409 2.593710 3.033557
-VERTEX2 2385 33.078449 2.688232 -3.126274
-VERTEX2 2386 32.083014 2.665627 2.992336
-VERTEX2 2387 31.084304 2.818705 2.714377
-VERTEX2 2388 30.640608 1.945481 -2.186395
-VERTEX2 2389 30.048786 1.105977 -2.452927
-VERTEX2 2390 29.234774 0.474772 -2.356765
-VERTEX2 2391 28.521576 -0.216528 -2.308705
-VERTEX2 2392 27.785333 0.459733 2.547192
-VERTEX2 2393 26.911921 0.997902 2.424802
-VERTEX2 2394 26.139182 1.686715 2.499756
-VERTEX2 2395 25.334982 2.287412 2.380380
-VERTEX2 2396 26.049451 1.599622 -0.689975
-VERTEX2 2397 26.811546 0.977276 -0.604518
-VERTEX2 2398 27.631703 0.425759 -0.564551
-VERTEX2 2399 28.521063 -0.120864 -0.161306
-VERTEX2 2400 28.681390 0.816861 1.208706
-VERTEX2 2401 29.056050 1.752311 1.193296
-VERTEX2 2402 29.453933 2.660060 1.229602
-VERTEX2 2403 29.787771 3.584510 1.277367
-VERTEX2 2404 30.766995 3.293605 -0.172620
-VERTEX2 2405 31.774154 3.150858 0.124624
-VERTEX2 2406 32.780849 3.273488 0.002415
-VERTEX2 2407 33.767021 3.266322 0.073760
-VERTEX2 2408 34.740762 3.321038 -0.347829
-VERTEX2 2409 35.669834 2.980805 -0.675080
-VERTEX2 2410 36.443432 2.366278 -0.667167
-VERTEX2 2411 37.232988 1.787813 -0.690346
-VERTEX2 2412 36.626528 1.025921 -2.380815
-VERTEX2 2413 35.848260 0.353548 -2.709528
-VERTEX2 2414 34.931124 -0.063211 -2.777017
-VERTEX2 2415 33.998411 -0.436167 -2.596529
-VERTEX2 2416 33.485395 0.423511 2.208442
-VERTEX2 2417 32.933862 1.251411 2.004684
-VERTEX2 2418 32.507598 2.166426 2.004963
-VERTEX2 2419 32.109736 3.031811 2.201028
-VERTEX2 2420 32.894159 3.625573 0.735344
-VERTEX2 2421 33.637378 4.291655 0.293290
-VERTEX2 2422 34.575736 4.553502 0.296125
-VERTEX2 2423 35.526211 4.841121 0.433640
-VERTEX2 2424 35.961170 3.919464 -1.160868
-VERTEX2 2425 36.393531 3.019490 -1.380552
-VERTEX2 2426 36.582711 2.044268 -1.335348
-VERTEX2 2427 36.792099 1.086550 -1.226461
-VERTEX2 2428 36.475953 2.051990 1.819175
-VERTEX2 2429 36.242607 3.038448 1.775086
-VERTEX2 2430 36.027877 4.015707 1.815986
-VERTEX2 2431 35.761827 4.959902 1.866674
-VERTEX2 2432 34.826269 4.634760 -2.743860
-VERTEX2 2433 33.967716 4.237352 -2.605281
-VERTEX2 2434 33.079951 3.700552 -2.634207
-VERTEX2 2435 32.202957 3.188873 -2.809240
-VERTEX2 2436 31.871915 4.082267 2.128329
-VERTEX2 2437 31.332887 4.933251 2.231431
-VERTEX2 2438 30.729000 5.723854 2.039842
-VERTEX2 2439 30.290195 6.594319 2.128631
-VERTEX2 2440 30.842142 5.739187 -0.875360
-VERTEX2 2441 31.407820 4.942962 -1.008677
-VERTEX2 2442 31.955578 4.100399 -0.727630
-VERTEX2 2443 32.721241 3.402627 -0.692838
-VERTEX2 2444 31.953778 4.040437 2.174092
-VERTEX2 2445 31.365927 4.831657 2.257808
-VERTEX2 2446 30.771775 5.621185 2.501995
-VERTEX2 2447 29.989391 6.188973 2.348374
-VERTEX2 2448 29.254234 5.486454 -2.421098
-VERTEX2 2449 28.486683 4.785270 -2.154129
-VERTEX2 2450 27.929893 3.896644 -1.938658
-VERTEX2 2451 27.583557 2.984549 -2.017935
-VERTEX2 2452 26.712662 3.436345 2.618744
-VERTEX2 2453 25.853501 3.937538 2.747436
-VERTEX2 2454 24.912750 4.312964 2.854618
-VERTEX2 2455 23.950829 4.595060 2.919281
-VERTEX2 2456 24.168715 5.514795 1.173405
-VERTEX2 2457 24.576124 6.401943 1.194305
-VERTEX2 2458 24.936965 7.318492 1.302384
-VERTEX2 2459 25.221280 8.276249 1.619481
-VERTEX2 2460 25.315771 7.259663 -1.261099
-VERTEX2 2461 25.635470 6.337079 -1.297394
-VERTEX2 2462 25.925892 5.317070 -1.368935
-VERTEX2 2463 26.113820 4.349350 -1.426112
-VERTEX2 2464 25.085028 4.211344 -2.598144
-VERTEX2 2465 24.222625 3.699939 -2.337440
-VERTEX2 2466 23.534402 2.960515 -1.969661
-VERTEX2 2467 23.134464 2.052389 -1.936084
-VERTEX2 2468 22.198100 2.412048 2.920032
-VERTEX2 2469 21.196887 2.630709 2.578655
-VERTEX2 2470 20.324380 3.155259 2.351411
-VERTEX2 2471 19.604399 3.858892 2.385883
-VERTEX2 2472 20.331588 3.132981 -0.955025
-VERTEX2 2473 20.884751 2.327310 -1.266257
-VERTEX2 2474 21.155923 1.380170 -1.266149
-VERTEX2 2475 21.469672 0.455521 -1.093448
-VERTEX2 2476 20.564001 0.017695 -2.798670
-VERTEX2 2477 19.656305 -0.328902 -2.854116
-VERTEX2 2478 18.705913 -0.642620 -2.410333
-VERTEX2 2479 17.945448 -1.280301 -2.041333
-VERTEX2 2480 18.831337 -1.752823 -0.631097
-VERTEX2 2481 19.655466 -2.348443 -0.625329
-VERTEX2 2482 20.492723 -2.928163 -1.069297
-VERTEX2 2483 20.974911 -3.812192 -0.958511
-VERTEX2 2484 20.412381 -2.971338 2.211728
-VERTEX2 2485 19.779974 -2.184817 2.283164
-VERTEX2 2486 19.134360 -1.449721 2.520225
-VERTEX2 2487 18.335045 -0.876529 2.649156
-VERTEX2 2488 19.240786 -1.355227 -0.771930
-VERTEX2 2489 20.006500 -2.047488 -1.109686
-VERTEX2 2490 20.430765 -2.922015 -1.092020
-VERTEX2 2491 20.840313 -3.804444 -1.176319
-VERTEX2 2492 20.433725 -2.897451 1.805163
-VERTEX2 2493 20.186749 -1.902485 1.356760
-VERTEX2 2494 20.391745 -0.901476 1.221301
-VERTEX2 2495 20.762847 0.008492 1.381678
-VERTEX2 2496 19.786516 0.162914 2.816751
-VERTEX2 2497 18.844730 0.488577 2.846493
-VERTEX2 2498 17.906439 0.788808 2.725464
-VERTEX2 2499 17.031097 1.158900 2.779763
-VERTEX2 2500 16.671993 0.275936 -1.977774
-VERTEX2 2501 16.279494 -0.613822 -1.752305
-VERTEX2 2502 16.121590 -1.619743 -1.929014
-VERTEX2 2503 15.786555 -2.558503 -1.887587
-VERTEX2 2504 14.859699 -2.257981 2.855408
-VERTEX2 2505 13.899018 -1.990290 2.474175
-VERTEX2 2506 13.145405 -1.397744 2.550395
-VERTEX2 2507 12.348670 -0.820060 2.327214
-VERTEX2 2508 11.610287 -1.517074 -2.666147
-VERTEX2 2509 10.731302 -1.978421 -2.641533
-VERTEX2 2510 9.855959 -2.472978 -2.377638
-VERTEX2 2511 9.163210 -3.184324 -2.278591
-VERTEX2 2512 8.396672 -2.542734 2.713062
-VERTEX2 2513 7.515939 -2.167811 2.624462
-VERTEX2 2514 6.619382 -1.661759 2.600369
-VERTEX2 2515 5.765084 -1.190476 2.703240
-VERTEX2 2516 4.880498 -0.776821 2.886805
-VERTEX2 2517 3.918223 -0.503964 2.719061
-VERTEX2 2518 2.994756 -0.103866 2.629961
-VERTEX2 2519 2.103812 0.360113 2.590823
-VERTEX2 2520 2.631819 1.197004 0.995523
-VERTEX2 2521 3.140875 2.005862 1.065691
-VERTEX2 2522 3.590729 2.874948 1.152832
-VERTEX2 2523 4.007532 3.806727 1.083364
-VERTEX2 2524 4.946166 3.354657 -0.431675
-VERTEX2 2525 5.866964 2.957184 -0.285584
-VERTEX2 2526 6.842915 2.706516 -0.545296
-VERTEX2 2527 7.737781 2.203589 -0.650991
-VERTEX2 2528 8.343900 3.049687 0.446000
-VERTEX2 2529 9.267406 3.473697 0.692122
-VERTEX2 2530 10.044996 4.113412 0.869425
-VERTEX2 2531 10.677133 4.826111 0.766222
-VERTEX2 2532 9.958397 4.085785 -2.483828
-VERTEX2 2533 9.144174 3.481751 -2.052947
-VERTEX2 2534 8.676425 2.601756 -2.020280
-VERTEX2 2535 8.241269 1.744419 -2.337948
-VERTEX2 2536 8.943191 1.019478 -0.511367
-VERTEX2 2537 9.819845 0.517258 -0.425534
-VERTEX2 2538 10.738755 0.112276 -0.167436
-VERTEX2 2539 11.729556 -0.069749 0.103991
-VERTEX2 2540 11.859283 -1.026223 -1.924730
-VERTEX2 2541 11.504797 -1.972889 -1.955610
-VERTEX2 2542 11.130502 -2.893070 -1.426429
-VERTEX2 2543 11.297654 -3.882435 -1.389905
-VERTEX2 2544 10.292512 -4.032556 -3.035851
-VERTEX2 2545 9.284776 -4.129410 -3.000816
-VERTEX2 2546 8.294278 -4.306378 -2.738924
-VERTEX2 2547 7.344572 -4.640547 -2.779802
-VERTEX2 2548 7.728413 -5.578453 -0.880158
-VERTEX2 2549 8.375116 -6.319969 -1.103002
-VERTEX2 2550 8.798898 -7.212484 -1.391101
-VERTEX2 2551 8.953881 -8.149781 -1.550984
-VERTEX2 2552 9.950309 -8.135604 -0.129532
-VERTEX2 2553 10.960832 -8.323826 -0.082373
-VERTEX2 2554 11.991826 -8.409896 0.256599
-VERTEX2 2555 12.952917 -8.158676 -0.020069
-VERTEX2 2556 12.974685 -7.160176 1.202684
-VERTEX2 2557 13.354861 -6.234963 1.013099
-VERTEX2 2558 13.865288 -5.401602 0.724046
-VERTEX2 2559 14.616234 -4.737398 0.762586
-VERTEX2 2560 15.318948 -5.462519 -0.818179
-VERTEX2 2561 16.065711 -6.215234 -1.039075
-VERTEX2 2562 16.590885 -7.062957 -0.934787
-VERTEX2 2563 17.163259 -7.863808 -1.180779
-VERTEX2 2564 16.230940 -8.245830 -2.776357
-VERTEX2 2565 15.292722 -8.609906 -2.758864
-VERTEX2 2566 14.383939 -8.985583 -2.792109
-VERTEX2 2567 13.441058 -9.346161 -2.791952
-VERTEX2 2568 12.484243 -9.679550 -2.900561
-VERTEX2 2569 11.516261 -9.935434 -3.046900
-VERTEX2 2570 10.472468 -9.996806 -3.063415
-VERTEX2 2571 9.527587 -10.063834 -3.015401
-VERTEX2 2572 9.404108 -9.060991 1.554539
-VERTEX2 2573 9.457969 -8.085446 1.569473
-VERTEX2 2574 9.451867 -7.074497 1.765170
-VERTEX2 2575 9.260236 -6.087148 1.318628
-VERTEX2 2576 8.262851 -5.832407 2.782337
-VERTEX2 2577 7.352883 -5.497264 2.811212
-VERTEX2 2578 6.391393 -5.153591 2.786569
-VERTEX2 2579 5.446192 -4.825801 -3.121752
-VERTEX2 2580 5.434758 -5.826120 -1.350849
-VERTEX2 2581 5.642313 -6.780312 -1.199898
-VERTEX2 2582 6.055902 -7.714906 -1.120114
-VERTEX2 2583 6.463005 -8.599919 -1.338184
-VERTEX2 2584 6.249892 -7.586831 1.913859
-VERTEX2 2585 5.854594 -6.640897 1.825910
-VERTEX2 2586 5.628330 -5.674311 1.441478
-VERTEX2 2587 5.726016 -4.700604 0.991752
-VERTEX2 2588 5.171558 -5.522991 -2.006368
-VERTEX2 2589 4.748059 -6.410275 -2.116910
-VERTEX2 2590 4.178659 -7.220895 -2.116444
-VERTEX2 2591 3.696833 -8.085107 -2.147008
-VERTEX2 2592 4.283430 -7.233105 0.813960
-VERTEX2 2593 4.972722 -6.513640 0.709566
-VERTEX2 2594 5.693361 -5.847647 0.781716
-VERTEX2 2595 6.405280 -5.129381 0.344163
-VERTEX2 2596 6.068156 -4.172008 1.972600
-VERTEX2 2597 5.644624 -3.255912 1.783495
-VERTEX2 2598 5.407691 -2.254164 2.004907
-VERTEX2 2599 5.005561 -1.352241 1.998590
-VERTEX2 2600 4.084356 -1.745023 -2.734159
-VERTEX2 2601 3.147680 -2.129824 -2.913042
-VERTEX2 2602 2.165400 -2.342759 3.125364
-VERTEX2 2603 1.165407 -2.318229 -2.927057
-VERTEX2 2604 2.154844 -2.127123 0.002884
-VERTEX2 2605 3.169818 -2.147766 0.191184
-VERTEX2 2606 4.176788 -1.976432 0.383042
-VERTEX2 2607 5.136246 -1.593410 0.414764
-VERTEX2 2608 4.742000 -0.683824 2.108708
-VERTEX2 2609 4.258298 0.191277 1.975808
-VERTEX2 2610 3.908837 1.093844 1.884436
-VERTEX2 2611 3.607216 2.012572 2.132002
-VERTEX2 2612 4.407709 2.552954 0.034017
-VERTEX2 2613 5.397129 2.574289 -0.053861
-VERTEX2 2614 6.373740 2.502659 -0.057864
-VERTEX2 2615 7.367396 2.424758 -0.208908
-VERTEX2 2616 7.573201 3.387127 1.632998
-VERTEX2 2617 7.525447 4.403454 1.640999
-VERTEX2 2618 7.491868 5.381605 1.749098
-VERTEX2 2619 7.314631 6.370437 1.617468
-VERTEX2 2620 6.325684 6.331388 -3.008212
-VERTEX2 2621 5.354822 6.198907 2.820554
-VERTEX2 2622 4.434173 6.476618 3.033991
-VERTEX2 2623 3.493507 6.566237 -3.094319
-VERTEX2 2624 3.418072 7.570344 1.498663
-VERTEX2 2625 3.501941 8.551904 1.693475
-VERTEX2 2626 3.387485 9.548347 1.882058
-VERTEX2 2627 3.076395 10.529698 1.643090
-VERTEX2 2628 2.071667 10.441554 -2.920363
-VERTEX2 2629 1.067859 10.207009 -2.840566
-VERTEX2 2630 0.119384 9.928791 -2.928895
-VERTEX2 2631 -0.855371 9.762740 -2.956404
-VERTEX2 2632 -0.683110 8.800233 -1.007582
-VERTEX2 2633 -0.173044 7.964907 -1.053469
-VERTEX2 2634 0.298287 7.087551 -1.176826
-VERTEX2 2635 0.670972 6.139144 -1.068737
-VERTEX2 2636 1.178194 5.263312 -0.835882
-VERTEX2 2637 1.833049 4.515884 -0.817255
-VERTEX2 2638 2.559314 3.818857 -0.909195
-VERTEX2 2639 3.179966 3.058144 -0.698497
-VERTEX2 2640 2.526371 2.290376 -1.934460
-VERTEX2 2641 2.182169 1.361072 -1.976928
-VERTEX2 2642 1.782964 0.439051 -1.844358
-VERTEX2 2643 1.562126 -0.521177 -2.181397
-VERTEX2 2644 2.142230 0.297727 0.817803
-VERTEX2 2645 2.866510 1.016581 0.675333
-VERTEX2 2646 3.641751 1.680502 0.418071
-VERTEX2 2647 4.577348 2.116152 0.436210
-VERTEX2 2648 3.664144 1.694836 -2.864623
-VERTEX2 2649 2.688353 1.432741 -3.097703
-VERTEX2 2650 1.687903 1.392766 -2.868248
-VERTEX2 2651 0.698932 1.131366 -2.822597
-VERTEX2 2652 0.357945 2.067579 2.128643
-VERTEX2 2653 -0.160368 2.873834 2.205130
-VERTEX2 2654 -0.718579 3.660312 2.568861
-VERTEX2 2655 -1.524615 4.213636 2.694914
-VERTEX2 2656 -1.907446 3.352994 -1.902044
-VERTEX2 2657 -2.257340 2.392513 -1.714143
-VERTEX2 2658 -2.400976 1.410118 -1.865191
-VERTEX2 2659 -2.674912 0.483451 -2.089879
-VERTEX2 2660 -2.196706 1.349175 1.231260
-VERTEX2 2661 -1.838113 2.309905 1.404757
-VERTEX2 2662 -1.668409 3.292394 1.258283
-VERTEX2 2663 -1.380774 4.228892 1.257325
-VERTEX2 2664 -0.420891 3.955612 -0.323365
-VERTEX2 2665 0.554234 3.626388 -0.235345
-VERTEX2 2666 1.541338 3.401079 -0.199958
-VERTEX2 2667 2.539651 3.210734 -0.474840
-VERTEX2 2668 2.944841 4.117135 1.281033
-VERTEX2 2669 3.224662 5.049339 1.319756
-VERTEX2 2670 3.481050 6.016743 1.337345
-VERTEX2 2671 3.709956 6.980767 1.423607
-VERTEX2 2672 4.696324 6.856571 -0.111020
-VERTEX2 2673 5.689793 6.745593 -0.159559
-VERTEX2 2674 6.674885 6.585662 -0.382410
-VERTEX2 2675 7.585846 6.211243 -0.414282
-VERTEX2 2676 6.691834 6.613350 2.653823
-VERTEX2 2677 5.811469 7.072713 2.840591
-VERTEX2 2678 4.869350 7.302459 -2.962319
-VERTEX2 2679 3.832138 7.110267 -3.065280
-VERTEX2 2680 3.894102 6.104083 -1.324942
-VERTEX2 2681 4.153437 5.121237 -1.212357
-VERTEX2 2682 4.527205 4.190732 -1.244712
-VERTEX2 2683 4.847152 3.235991 -1.086545
-VERTEX2 2684 3.970641 2.724792 -2.570039
-VERTEX2 2685 3.128444 2.182623 -2.516838
-VERTEX2 2686 2.293343 1.587555 -2.776686
-VERTEX2 2687 1.342883 1.244170 -2.925440
-VERTEX2 2688 2.320839 1.480943 0.524728
-VERTEX2 2689 3.182611 1.980954 0.707377
-VERTEX2 2690 3.947527 2.659690 0.794828
-VERTEX2 2691 4.681531 3.357333 1.052001
-VERTEX2 2692 4.185779 2.521782 -1.980754
-VERTEX2 2693 3.752728 1.608223 -2.010648
-VERTEX2 2694 3.316267 0.726960 -2.104553
-VERTEX2 2695 2.804531 -0.153748 -2.193628
-VERTEX2 2696 3.390750 0.654389 0.912555
-VERTEX2 2697 4.005993 1.473419 0.621922
-VERTEX2 2698 4.862642 2.033735 0.566548
-VERTEX2 2699 5.653543 2.586408 0.782457
-VERTEX2 2700 4.969869 1.886312 -2.406894
-VERTEX2 2701 4.220310 1.168370 -2.344507
-VERTEX2 2702 3.493576 0.419572 -2.301942
-VERTEX2 2703 2.796323 -0.302461 -1.840674
-VERTEX2 2704 3.775139 -0.574889 -0.511595
-VERTEX2 2705 4.622635 -1.041472 -0.779953
-VERTEX2 2706 5.331354 -1.722749 -0.685419
-VERTEX2 2707 6.118985 -2.331580 -0.501823
-VERTEX2 2708 5.662738 -3.215950 -2.175840
-VERTEX2 2709 5.120762 -4.077159 -2.357821
-VERTEX2 2710 4.430491 -4.804360 -2.416089
-VERTEX2 2711 3.675336 -5.474972 -2.545441
-VERTEX2 2712 3.113912 -4.651150 2.298370
-VERTEX2 2713 2.452760 -3.872090 2.361121
-VERTEX2 2714 1.767797 -3.155525 1.669092
-VERTEX2 2715 1.651575 -2.149293 1.707085
-VERTEX2 2716 1.485710 -1.120136 1.531542
-VERTEX2 2717 1.534082 -0.123685 1.412293
-VERTEX2 2718 1.704256 0.878892 1.443867
-VERTEX2 2719 1.879808 1.895326 1.316448
-VERTEX2 2720 1.613428 0.915229 -2.069990
-VERTEX2 2721 1.125221 0.055816 -2.106799
-VERTEX2 2722 0.629118 -0.824140 -2.278881
-VERTEX2 2723 -0.035213 -1.620421 -2.374266
-VERTEX2 2724 0.700004 -0.952925 0.453515
-VERTEX2 2725 1.603264 -0.545251 0.691208
-VERTEX2 2726 2.382574 0.115911 0.619556
-VERTEX2 2727 3.193951 0.651853 0.641064
-VERTEX2 2728 3.803765 -0.179182 -1.199363
-VERTEX2 2729 4.152452 -1.109646 -1.213089
-VERTEX2 2730 4.542329 -2.039422 -0.960540
-VERTEX2 2731 5.131168 -2.899652 -0.993053
-VERTEX2 2732 4.576182 -2.054200 2.378116
-VERTEX2 2733 3.854008 -1.364201 2.536047
-VERTEX2 2734 2.999479 -0.810938 2.595047
-VERTEX2 2735 2.153944 -0.251948 2.603106
-VERTEX2 2736 3.010276 -0.761235 -0.584247
-VERTEX2 2737 3.863879 -1.322932 -0.387544
-VERTEX2 2738 4.808310 -1.683538 -0.251776
-VERTEX2 2739 5.779044 -1.941927 -0.525893
-VERTEX2 2740 4.923197 -1.488479 2.935079
-VERTEX2 2741 3.937359 -1.290043 2.971609
-VERTEX2 2742 2.920407 -1.148257 3.103172
-VERTEX2 2743 1.923323 -1.163564 -3.092221
-VERTEX2 2744 2.926172 -1.119124 0.295503
-VERTEX2 2745 3.886986 -0.826969 0.114470
-VERTEX2 2746 4.837972 -0.714521 0.071741
-VERTEX2 2747 5.850836 -0.675025 0.027700
-VERTEX2 2748 5.907641 -1.636102 -1.251575
-VERTEX2 2749 6.225471 -2.610526 -0.791368
-VERTEX2 2750 6.925343 -3.323177 -0.851814
-VERTEX2 2751 7.582614 -4.058203 -0.752204
-VERTEX2 2752 6.827449 -3.384819 2.096356
-VERTEX2 2753 6.343280 -2.503725 2.068936
-VERTEX2 2754 5.870675 -1.605105 1.911302
-VERTEX2 2755 5.554047 -0.636973 1.978432
-VERTEX2 2756 5.981675 -1.545303 -0.708499
-VERTEX2 2757 6.732536 -2.187314 -0.755179
-VERTEX2 2758 7.464148 -2.856108 -0.663934
-VERTEX2 2759 8.228586 -3.463834 -0.464746
-VERTEX2 2760 7.378960 -2.977468 2.492633
-VERTEX2 2761 6.582354 -2.368502 2.139412
-VERTEX2 2762 6.028251 -1.538501 2.128130
-VERTEX2 2763 5.516150 -0.693311 1.934115
-VERTEX2 2764 6.473174 -0.340001 0.663316
-VERTEX2 2765 7.294714 0.293152 0.961067
-VERTEX2 2766 7.848428 1.133657 0.852202
-VERTEX2 2767 8.539958 1.883986 1.131073
-VERTEX2 2768 8.130866 0.957307 -2.250879
-VERTEX2 2769 7.498961 0.180708 -2.166991
-VERTEX2 2770 6.949884 -0.647001 -2.122665
-VERTEX2 2771 6.403665 -1.506847 -2.113388
-VERTEX2 2772 6.934633 -0.622441 0.937435
-VERTEX2 2773 7.520735 0.204402 0.718660
-VERTEX2 2774 8.305387 0.841714 0.681124
-VERTEX2 2775 9.076729 1.464052 0.751185
-VERTEX2 2776 9.751309 0.724854 -0.713244
-VERTEX2 2777 10.498160 0.095501 -0.468804
-VERTEX2 2778 11.379691 -0.372332 -0.568899
-VERTEX2 2779 12.180947 -0.895930 -0.753055
-VERTEX2 2780 11.513837 -1.639497 -2.259502
-VERTEX2 2781 10.846536 -2.408061 -2.327267
-VERTEX2 2782 10.169024 -3.105641 -2.135228
-VERTEX2 2783 9.570484 -3.935084 -2.467261
-VERTEX2 2784 8.973080 -3.160007 2.155839
-VERTEX2 2785 8.436995 -2.289760 2.182701
-VERTEX2 2786 7.855685 -1.434900 2.073909
-VERTEX2 2787 7.367000 -0.519490 1.871793
-VERTEX2 2788 8.334758 -0.240508 0.545940
-VERTEX2 2789 9.225556 0.309813 0.769511
-VERTEX2 2790 9.972792 1.002999 1.105927
-VERTEX2 2791 10.453516 1.915681 1.266911
-VERTEX2 2792 10.148690 0.986471 -1.962464
-VERTEX2 2793 9.782572 0.062844 -2.025783
-VERTEX2 2794 9.346494 -0.823859 -1.947532
-VERTEX2 2795 8.979420 -1.793132 -1.714541
-VERTEX2 2796 7.996751 -1.661795 2.952612
-VERTEX2 2797 6.995487 -1.452046 2.573842
-VERTEX2 2798 6.152503 -0.904587 2.375795
-VERTEX2 2799 5.436816 -0.168023 2.543337
-VERTEX2 2800 6.042387 0.680500 0.941495
-VERTEX2 2801 6.633504 1.496827 1.104476
-VERTEX2 2802 7.119591 2.365080 1.188626
-VERTEX2 2803 7.482140 3.290264 0.868490
-VERTEX2 2804 6.861108 2.500099 -2.040320
-VERTEX2 2805 6.443231 1.634604 -2.317953
-VERTEX2 2806 5.801830 0.889399 -2.348769
-VERTEX2 2807 5.048865 0.157454 -2.311109
-VERTEX2 2808 5.815094 -0.532957 -0.635174
-VERTEX2 2809 6.623697 -1.126080 -0.343421
-VERTEX2 2810 7.569326 -1.460427 -0.115361
-VERTEX2 2811 8.592000 -1.559150 -0.288934
-VERTEX2 2812 8.294552 -2.504166 -2.053947
-VERTEX2 2813 7.856977 -3.391854 -1.850259
-VERTEX2 2814 7.529015 -4.365139 -1.727987
-VERTEX2 2815 7.415158 -5.358232 -1.697948
-VERTEX2 2816 6.389268 -5.238904 3.034051
-VERTEX2 2817 5.404134 -5.116326 -3.118307
-VERTEX2 2818 4.434651 -5.149317 3.073504
-VERTEX2 2819 3.446773 -5.055216 -2.800910
-VERTEX2 2820 3.750816 -5.968736 -1.267860
-VERTEX2 2821 4.107387 -6.946400 -1.247212
-VERTEX2 2822 4.403690 -7.884513 -1.354119
-VERTEX2 2823 4.592312 -8.862311 -1.280569
-VERTEX2 2824 3.627654 -9.172947 -2.783376
-VERTEX2 2825 2.697785 -9.504690 -2.964127
-VERTEX2 2826 1.691659 -9.676529 -2.751209
-VERTEX2 2827 0.783695 -10.025530 -2.872589
-VERTEX2 2828 1.045740 -10.979822 -1.485880
-VERTEX2 2829 1.100640 -11.941237 -1.404258
-VERTEX2 2830 1.236434 -12.890901 -1.494605
-VERTEX2 2831 1.359804 -13.876771 -1.508925
-VERTEX2 2832 2.358913 -13.848240 -0.249091
-VERTEX2 2833 3.324727 -14.090967 -0.378660
-VERTEX2 2834 4.279399 -14.436367 -0.271609
-VERTEX2 2835 5.261117 -14.734529 -0.301843
-VERTEX2 2836 4.976236 -15.688898 -1.875283
-VERTEX2 2837 4.644669 -16.650988 -2.093829
-VERTEX2 2838 4.144482 -17.502379 -2.434103
-VERTEX2 2839 3.361553 -18.154370 -2.597035
-VERTEX2 2840 3.885011 -18.982026 -1.127648
-VERTEX2 2841 4.327521 -19.923759 -1.183815
-VERTEX2 2842 4.715364 -20.845619 -0.998566
-VERTEX2 2843 5.233307 -21.675980 -1.082916
-VERTEX2 2844 5.726266 -22.571618 -0.857487
-VERTEX2 2845 6.372113 -23.351341 -0.541164
-VERTEX2 2846 7.219168 -23.884252 -0.495850
-VERTEX2 2847 8.105358 -24.348077 -0.561640
-VERTEX2 2848 8.632330 -23.522267 1.032625
-VERTEX2 2849 9.120820 -22.682047 0.943405
-VERTEX2 2850 9.687607 -21.886568 0.868049
-VERTEX2 2851 10.347417 -21.108757 1.051990
-VERTEX2 2852 9.469719 -20.601226 2.749030
-VERTEX2 2853 8.522236 -20.225489 2.473299
-VERTEX2 2854 7.711465 -19.617205 2.232991
-VERTEX2 2855 7.118748 -18.863555 2.467871
-VERTEX2 2856 7.714567 -18.062909 0.798234
-VERTEX2 2857 8.386238 -17.331696 0.661978
-VERTEX2 2858 9.195995 -16.736375 0.974219
-VERTEX2 2859 9.727604 -15.862065 1.104742
-VERTEX2 2860 9.254052 -16.773381 -1.786361
-VERTEX2 2861 9.060758 -17.750853 -1.890323
-VERTEX2 2862 8.751927 -18.697757 -1.595683
-VERTEX2 2863 8.703773 -19.729819 -1.701286
-VERTEX2 2864 8.852611 -18.736753 1.476919
-VERTEX2 2865 8.957937 -17.734955 1.324368
-VERTEX2 2866 9.215277 -16.774220 1.374567
-VERTEX2 2867 9.432613 -15.797900 1.368978
-VERTEX2 2868 10.426995 -15.985379 -0.092785
-VERTEX2 2869 11.436884 -16.089801 -0.258372
-VERTEX2 2870 12.396631 -16.382005 -0.501245
-VERTEX2 2871 13.267612 -16.860729 -0.713193
-VERTEX2 2872 12.606457 -17.601301 -2.435650
-VERTEX2 2873 11.850495 -18.219447 -2.607003
-VERTEX2 2874 10.958474 -18.700449 -2.737942
-VERTEX2 2875 10.013056 -19.111934 -3.071928
-VERTEX2 2876 10.094375 -20.112651 -1.693920
-VERTEX2 2877 9.966884 -21.065979 -2.133356
-VERTEX2 2878 9.390009 -21.918655 -2.431767
-VERTEX2 2879 8.631802 -22.537332 -2.323045
-VERTEX2 2880 7.880620 -21.856250 2.160868
-VERTEX2 2881 7.323691 -21.025558 2.156852
-VERTEX2 2882 6.739567 -20.210758 2.007385
-VERTEX2 2883 6.301983 -19.301126 2.165936
-VERTEX2 2884 5.459289 -19.854547 -2.984880
-VERTEX2 2885 4.438790 -19.989078 -2.996941
-VERTEX2 2886 3.436309 -20.149178 -2.676398
-VERTEX2 2887 2.510283 -20.543361 -2.493674
-VERTEX2 2888 1.889185 -19.725167 2.382528
-VERTEX2 2889 1.130259 -19.024493 2.286881
-VERTEX2 2890 0.498238 -18.265644 2.980543
-VERTEX2 2891 -0.510604 -18.120011 2.980955
-VERTEX2 2892 -0.665668 -19.128513 -1.510500
-VERTEX2 2893 -0.627977 -20.129611 -1.147975
-VERTEX2 2894 -0.217551 -21.046187 -1.205273
-VERTEX2 2895 0.127787 -22.018184 -1.192437
-VERTEX2 2896 -0.841678 -22.360590 -2.847089
-VERTEX2 2897 -1.806070 -22.671540 -3.066129
-VERTEX2 2898 -2.803977 -22.729201 -2.912877
-VERTEX2 2899 -3.772322 -22.990629 -2.915097
-VERTEX2 2900 -2.798778 -22.766428 -0.091250
-VERTEX2 2901 -1.798370 -22.854369 -0.129686
-VERTEX2 2902 -0.797042 -22.988154 0.094798
-VERTEX2 2903 0.213785 -22.896760 0.043046
-VERTEX2 2904 -0.780913 -22.947257 -3.013008
-VERTEX2 2905 -1.804978 -23.060101 -3.012144
-VERTEX2 2906 -2.840160 -23.229511 2.895509
-VERTEX2 2907 -3.815799 -22.960337 2.774250
-VERTEX2 2908 -2.894756 -23.322721 -0.433404
-VERTEX2 2909 -1.967165 -23.738859 -0.751990
-VERTEX2 2910 -1.245296 -24.428232 -1.122383
-VERTEX2 2911 -0.797868 -25.335591 -0.984910
-VERTEX2 2912 0.052591 -24.787973 0.687495
-VERTEX2 2913 0.827523 -24.119487 0.557313
-VERTEX2 2914 1.707333 -23.593491 0.174750
-VERTEX2 2915 2.700434 -23.489864 0.429996
-VERTEX2 2916 1.790214 -23.888790 -2.557017
-VERTEX2 2917 0.983451 -24.490594 -2.705635
-VERTEX2 2918 0.062272 -24.948994 -2.724751
-VERTEX2 2919 -0.885037 -25.375760 -2.656541
-VERTEX2 2920 -1.347604 -24.507282 1.988249
-VERTEX2 2921 -1.764631 -23.585205 2.022107
-VERTEX2 2922 -2.200220 -22.707966 2.439920
-VERTEX2 2923 -2.938382 -22.075654 2.420535
-VERTEX2 2924 -2.278307 -21.305296 1.110699
-VERTEX2 2925 -1.821788 -20.459050 1.331160
-VERTEX2 2926 -1.599868 -19.497033 1.187570
-VERTEX2 2927 -1.238869 -18.575100 1.411176
-VERTEX2 2928 -0.250079 -18.747803 -0.335469
-VERTEX2 2929 0.672596 -19.082960 -0.242106
-VERTEX2 2930 1.669953 -19.305278 -0.232160
-VERTEX2 2931 2.643512 -19.510982 -0.303495
-VERTEX2 2932 1.704706 -19.222858 3.099652
-VERTEX2 2933 0.678833 -19.237913 -3.073205
-VERTEX2 2934 -0.282137 -19.336076 3.131987
-VERTEX2 2935 -1.299726 -19.310274 -2.984893
-VERTEX2 2936 -0.293494 -19.142219 0.246830
-VERTEX2 2937 0.672176 -18.877626 0.395152
-VERTEX2 2938 1.565626 -18.498124 0.388022
-VERTEX2 2939 2.498187 -18.075123 0.040804
-VERTEX2 2940 1.482734 -18.084961 -3.034670
-VERTEX2 2941 0.473540 -18.155984 -2.805618
-VERTEX2 2942 -0.478107 -18.492636 -2.881848
-VERTEX2 2943 -1.429562 -18.732943 -3.036518
-VERTEX2 2944 -1.531836 -17.727615 1.647454
-VERTEX2 2945 -1.627553 -16.720508 1.714192
-VERTEX2 2946 -1.782025 -15.712639 1.822446
-VERTEX2 2947 -2.045218 -14.779402 1.766221
-VERTEX2 2948 -3.012250 -14.979002 -2.606121
-VERTEX2 2949 -3.858063 -15.445134 -2.911623
-VERTEX2 2950 -4.829310 -15.664710 3.084694
-VERTEX2 2951 -5.825730 -15.613734 -2.666364
-VERTEX2 2952 -5.342259 -16.482370 -1.003591
-VERTEX2 2953 -4.795307 -17.309479 -0.800972
-VERTEX2 2954 -4.121803 -18.072891 -0.942306
-VERTEX2 2955 -3.523619 -18.862363 -0.949982
-VERTEX2 2956 -4.095689 -18.058108 2.521581
-VERTEX2 2957 -4.896726 -17.484066 2.473962
-VERTEX2 2958 -5.704433 -16.855770 2.357920
-VERTEX2 2959 -6.437106 -16.136675 2.370988
-VERTEX2 2960 -5.747773 -16.838048 -1.240613
-VERTEX2 2961 -5.468053 -17.763115 -1.090295
-VERTEX2 2962 -5.023683 -18.631480 -1.189002
-VERTEX2 2963 -4.634568 -19.559856 -1.387484
-VERTEX2 2964 -5.644377 -19.751036 -3.010707
-VERTEX2 2965 -6.640132 -19.870350 -3.053801
-VERTEX2 2966 -7.612436 -19.989563 3.048428
-VERTEX2 2967 -8.613541 -19.881043 2.652605
-VERTEX2 2968 -8.190595 -19.027077 0.944080
-VERTEX2 2969 -7.562507 -18.223438 0.999676
-VERTEX2 2970 -7.054234 -17.366142 0.786163
-VERTEX2 2971 -6.332830 -16.654996 0.696250
-VERTEX2 2972 -7.095165 -17.294606 -2.513589
-VERTEX2 2973 -7.899320 -17.896781 -2.474297
-VERTEX2 2974 -8.690417 -18.545478 -2.473718
-VERTEX2 2975 -9.464695 -19.137719 -2.559084
-VERTEX2 2976 -10.007989 -18.303843 1.907677
-VERTEX2 2977 -10.309994 -17.384579 1.650048
-VERTEX2 2978 -10.405763 -16.389819 1.686349
-VERTEX2 2979 -10.522217 -15.386092 1.586120
-VERTEX2 2980 -10.503836 -16.407850 -1.474024
-VERTEX2 2981 -10.420704 -17.408409 -1.380348
-VERTEX2 2982 -10.249949 -18.424222 -1.153809
-VERTEX2 2983 -9.873088 -19.337495 -1.044694
-VERTEX2 2984 -10.348208 -18.476720 2.049062
-VERTEX2 2985 -10.805043 -17.612370 2.050466
-VERTEX2 2986 -11.270422 -16.710621 2.132908
-VERTEX2 2987 -11.842248 -15.830626 2.130850
-VERTEX2 2988 -10.990026 -15.273685 0.368018
-VERTEX2 2989 -10.111956 -14.933318 0.270653
-VERTEX2 2990 -9.175554 -14.688185 0.678622
-VERTEX2 2991 -8.418771 -14.073057 0.621411
-VERTEX2 2992 -8.993102 -13.269308 1.939349
-VERTEX2 2993 -9.367813 -12.315205 1.953047
-VERTEX2 2994 -9.722495 -11.409389 1.883190
-VERTEX2 2995 -10.056614 -10.481362 2.228972
-VERTEX2 2996 -10.879057 -11.136347 -2.357353
-VERTEX2 2997 -11.623024 -11.824760 -2.269572
-VERTEX2 2998 -12.229922 -12.599062 -2.406700
-VERTEX2 2999 -12.942947 -13.316883 -2.231786
-VERTEX2 3000 -13.771790 -12.687309 2.345874
-VERTEX2 3001 -14.428633 -12.001782 2.535603
-VERTEX2 3002 -15.260699 -11.416500 2.084260
-VERTEX2 3003 -15.768630 -10.538758 2.378555
-VERTEX2 3004 -15.092806 -9.764836 0.637540
-VERTEX2 3005 -14.258538 -9.178207 0.605824
-VERTEX2 3006 -13.447592 -8.629081 0.601842
-VERTEX2 3007 -12.625924 -8.064304 0.645798
-VERTEX2 3008 -13.427632 -8.652216 -2.406459
-VERTEX2 3009 -14.124895 -9.316565 -2.088333
-VERTEX2 3010 -14.619054 -10.227799 -2.103750
-VERTEX2 3011 -15.170737 -11.078156 -2.075274
-VERTEX2 3012 -14.665847 -10.184968 1.170270
-VERTEX2 3013 -14.227977 -9.268524 1.227257
-VERTEX2 3014 -13.874888 -8.319021 1.152315
-VERTEX2 3015 -13.503976 -7.398812 1.287918
-VERTEX2 3016 -12.524284 -7.637219 -0.456644
-VERTEX2 3017 -11.657126 -8.069359 -0.520438
-VERTEX2 3018 -10.803643 -8.549907 -0.404620
-VERTEX2 3019 -9.889260 -8.936283 -0.451497
-VERTEX2 3020 -10.752743 -8.507306 2.806870
-VERTEX2 3021 -11.716686 -8.140024 2.694106
-VERTEX2 3022 -12.610879 -7.724309 2.535428
-VERTEX2 3023 -13.408854 -7.163465 2.521890
-VERTEX2 3024 -12.844832 -6.388776 0.912636
-VERTEX2 3025 -12.263981 -5.599107 0.624977
-VERTEX2 3026 -11.470980 -5.013597 0.621191
-VERTEX2 3027 -10.677877 -4.414368 0.656269
-VERTEX2 3028 -11.454881 -5.020604 -2.809019
-VERTEX2 3029 -12.366723 -5.351855 3.074845
-VERTEX2 3030 -13.373376 -5.341373 2.974051
-VERTEX2 3031 -14.336114 -5.181306 -2.992818
-VERTEX2 3032 -13.349884 -5.015566 0.243226
-VERTEX2 3033 -12.379790 -4.782073 0.624451
-VERTEX2 3034 -11.564740 -4.140302 0.412233
-VERTEX2 3035 -10.641404 -3.734957 0.423984
-VERTEX2 3036 -11.043540 -2.851587 1.813541
-VERTEX2 3037 -11.280558 -1.907903 2.130672
-VERTEX2 3038 -11.798970 -1.027349 1.844761
-VERTEX2 3039 -12.087170 -0.081676 1.864622
-VERTEX2 3040 -11.163170 0.196342 0.362040
-VERTEX2 3041 -10.216565 0.542173 0.156456
-VERTEX2 3042 -9.226240 0.711098 0.405731
-VERTEX2 3043 -8.336916 1.159965 0.296133
-VERTEX2 3044 -9.275813 0.875039 -3.083385
-VERTEX2 3045 -10.269642 0.811722 -3.068421
-VERTEX2 3046 -11.268447 0.791096 -3.128600
-VERTEX2 3047 -12.274444 0.758982 3.135932
-VERTEX2 3048 -12.292022 -0.259695 -1.681992
-VERTEX2 3049 -12.380216 -1.245570 -1.438796
-VERTEX2 3050 -12.219603 -2.202948 -1.421655
-VERTEX2 3051 -12.079998 -3.179715 -1.286311
-VERTEX2 3052 -11.809697 -4.140739 -1.570244
-VERTEX2 3053 -11.807516 -5.141288 -1.379528
-VERTEX2 3054 -11.588481 -6.087324 -1.485307
-VERTEX2 3055 -11.506936 -7.070428 -1.228955
-VERTEX2 3056 -12.444696 -7.392700 -3.035222
-VERTEX2 3057 -13.431325 -7.501936 -3.043634
-VERTEX2 3058 -14.422602 -7.575338 2.779063
-VERTEX2 3059 -15.360461 -7.243099 2.662286
-VERTEX2 3060 -14.898985 -6.308917 1.254954
-VERTEX2 3061 -14.606213 -5.351666 1.297776
-VERTEX2 3062 -14.333536 -4.379856 1.549366
-VERTEX2 3063 -14.328707 -3.360426 1.667323
-VERTEX2 3064 -15.287071 -3.473873 3.007718
-VERTEX2 3065 -16.330851 -3.377078 3.109778
-VERTEX2 3066 -17.335453 -3.323119 3.087501
-VERTEX2 3067 -18.318654 -3.242990 -2.787660
-VERTEX2 3068 -18.652412 -2.318816 2.037144
-VERTEX2 3069 -19.089876 -1.401553 2.194147
-VERTEX2 3070 -19.658635 -0.599504 2.286143
-VERTEX2 3071 -20.284037 0.167312 2.176882
-VERTEX2 3072 -21.061394 -0.425012 -2.372344
-VERTEX2 3073 -21.764007 -1.107051 -2.208840
-VERTEX2 3074 -22.385090 -1.910732 -2.037513
-VERTEX2 3075 -22.798905 -2.768550 -2.086383
-VERTEX2 3076 -21.909172 -3.302454 -0.734888
-VERTEX2 3077 -21.175844 -3.965787 -0.595544
-VERTEX2 3078 -20.341129 -4.547660 -0.690502
-VERTEX2 3079 -19.558123 -5.187815 -0.464234
-VERTEX2 3080 -19.102131 -4.298381 1.053828
-VERTEX2 3081 -18.623209 -3.446792 1.296037
-VERTEX2 3082 -18.380828 -2.455428 1.658160
-VERTEX2 3083 -18.475142 -1.470770 1.477173
-VERTEX2 3084 -19.474110 -1.394114 -3.132723
-VERTEX2 3085 -20.453670 -1.407187 -2.913006
-VERTEX2 3086 -21.438437 -1.574193 -2.842455
-VERTEX2 3087 -22.397112 -1.866458 -2.997977
-VERTEX2 3088 -22.564461 -0.876872 1.476430
-VERTEX2 3089 -22.466477 0.058652 1.694664
-VERTEX2 3090 -22.570674 1.027870 1.726758
-VERTEX2 3091 -22.691873 2.024612 1.424225
-VERTEX2 3092 -21.721037 1.875356 -0.366939
-VERTEX2 3093 -20.789909 1.504503 -0.149613
-VERTEX2 3094 -19.785438 1.333422 -0.277888
-VERTEX2 3095 -18.863428 1.060309 -0.095224
-VERTEX2 3096 -19.853273 1.147603 -3.036658
-VERTEX2 3097 -20.874371 1.063791 3.093926
-VERTEX2 3098 -21.889052 1.134147 -2.950681
-VERTEX2 3099 -22.899493 0.933750 -2.775675
-VERTEX2 3100 -23.252369 1.835428 1.608296
-VERTEX2 3101 -23.317732 2.831295 1.680350
-VERTEX2 3102 -23.461375 3.830620 1.863590
-VERTEX2 3103 -23.725814 4.791752 2.057948
-VERTEX2 3104 -23.254027 3.938601 -1.179755
-VERTEX2 3105 -22.904552 3.015129 -0.974623
-VERTEX2 3106 -22.364593 2.178868 -1.218256
-VERTEX2 3107 -22.054066 1.246132 -1.427794
-VERTEX2 3108 -22.198009 2.262493 1.734719
-VERTEX2 3109 -22.379235 3.248820 1.710996
-VERTEX2 3110 -22.528467 4.254232 1.644379
-VERTEX2 3111 -22.603327 5.244856 1.814193
-VERTEX2 3112 -22.829931 6.221140 1.975715
-VERTEX2 3113 -23.281405 7.129619 1.844600
-VERTEX2 3114 -23.543335 8.050534 1.763583
-VERTEX2 3115 -23.748485 9.009167 2.015469
-VERTEX2 3116 -22.848586 9.418239 0.577461
-VERTEX2 3117 -21.993024 9.979453 0.448785
-VERTEX2 3118 -21.075328 10.373382 0.146195
-VERTEX2 3119 -20.047862 10.514202 0.238652
-VERTEX2 3120 -20.250085 11.525089 1.701571
-VERTEX2 3121 -20.364868 12.492374 1.478635
-VERTEX2 3122 -20.287436 13.524026 1.471560
-VERTEX2 3123 -20.202929 14.493042 1.446340
-VERTEX2 3124 -19.196263 14.328537 -0.121199
-VERTEX2 3125 -18.186756 14.221194 -0.277847
-VERTEX2 3126 -17.233151 13.954242 -0.280496
-VERTEX2 3127 -16.286316 13.695229 -0.275818
-VERTEX2 3128 -15.353360 13.429218 -0.312282
-VERTEX2 3129 -14.386026 13.106393 -0.320243
-VERTEX2 3130 -13.488087 12.821994 -0.424057
-VERTEX2 3131 -12.590340 12.378674 -0.152704
-VERTEX2 3132 -12.710266 11.375917 -1.598888
-VERTEX2 3133 -12.740431 10.359694 -1.761736
-VERTEX2 3134 -12.971654 9.356881 -1.468204
-VERTEX2 3135 -12.837955 8.330889 -1.857544
-VERTEX2 3136 -13.832052 8.641294 2.743783
-VERTEX2 3137 -14.765994 9.019011 2.865593
-VERTEX2 3138 -15.739050 9.290234 -3.064912
-VERTEX2 3139 -16.728195 9.220279 3.075544
-VERTEX2 3140 -16.633966 10.219849 1.891583
-VERTEX2 3141 -16.968287 11.215831 2.198113
-VERTEX2 3142 -17.569836 12.004104 2.382682
-VERTEX2 3143 -18.251249 12.711438 2.337540
-VERTEX2 3144 -17.560333 12.013517 -0.789905
-VERTEX2 3145 -16.864594 11.292244 -0.422317
-VERTEX2 3146 -15.936896 10.847649 -0.130445
-VERTEX2 3147 -14.948894 10.726473 -0.086321
-VERTEX2 3148 -13.958498 10.623468 -0.037374
-VERTEX2 3149 -12.903613 10.597727 0.290759
-VERTEX2 3150 -11.937747 10.939136 0.406663
-VERTEX2 3151 -11.027986 11.341495 0.434904
-VERTEX2 3152 -10.613551 10.440050 -1.079826
-VERTEX2 3153 -10.158465 9.572587 -0.986390
-VERTEX2 3154 -9.599003 8.741689 -1.098323
-VERTEX2 3155 -9.135174 7.824287 -1.193467
-VERTEX2 3156 -10.089974 7.465725 -2.902854
-VERTEX2 3157 -11.074154 7.193336 -2.847809
-VERTEX2 3158 -12.035659 6.907811 -2.894589
-VERTEX2 3159 -12.983535 6.635532 -2.775134
-VERTEX2 3160 -12.583841 5.693487 -1.466492
-VERTEX2 3161 -12.485977 4.664715 -1.593505
-VERTEX2 3162 -12.512424 3.643852 -1.364349
-VERTEX2 3163 -12.314435 2.662089 -1.331450
-VERTEX2 3164 -11.354732 2.864579 0.496887
-VERTEX2 3165 -10.469233 3.339444 0.692777
-VERTEX2 3166 -9.751472 3.979156 0.677628
-VERTEX2 3167 -8.922237 4.579023 0.789996
-VERTEX2 3168 -9.590436 5.285850 2.363977
-VERTEX2 3169 -10.301012 5.956453 2.307817
-VERTEX2 3170 -10.970323 6.710410 2.132191
-VERTEX2 3171 -11.514953 7.549790 2.287846
-VERTEX2 3172 -10.877023 6.767692 -0.692251
-VERTEX2 3173 -10.118423 6.118129 -0.721380
-VERTEX2 3174 -9.349079 5.461558 -0.751732
-VERTEX2 3175 -8.605646 4.732720 -0.725466
-VERTEX2 3176 -9.378339 5.396081 2.544928
-VERTEX2 3177 -10.225347 5.972981 2.264153
-VERTEX2 3178 -10.851392 6.767451 2.228637
-VERTEX2 3179 -11.477601 7.556063 2.337254
-VERTEX2 3180 -10.751337 6.874814 -0.727221
-VERTEX2 3181 -10.065985 6.186724 -0.592684
-VERTEX2 3182 -9.229275 5.642705 -0.551533
-VERTEX2 3183 -8.384731 5.108077 -0.672471
-VERTEX2 3184 -9.200461 5.782235 2.395626
-VERTEX2 3185 -9.950947 6.463775 2.649886
-VERTEX2 3186 -10.851295 6.929340 2.770973
-VERTEX2 3187 -11.806189 7.291937 2.955693
-VERTEX2 3188 -11.640806 8.283859 1.657982
-VERTEX2 3189 -11.706358 9.270209 1.683556
-VERTEX2 3190 -11.781933 10.279706 1.600923
-VERTEX2 3191 -11.826801 11.249043 1.639833
-VERTEX2 3192 -10.843186 11.320893 0.215130
-VERTEX2 3193 -9.867857 11.515987 -0.153451
-VERTEX2 3194 -8.913470 11.366565 0.026846
-VERTEX2 3195 -7.875903 11.404445 -0.310040
-VERTEX2 3196 -8.194915 10.457934 -1.919155
-VERTEX2 3197 -8.519942 9.523232 -1.967399
-VERTEX2 3198 -8.882131 8.586240 -2.369505
-VERTEX2 3199 -9.621459 7.867102 -2.426521
-VERTEX2 3200 -8.855815 8.497050 0.836041
-VERTEX2 3201 -8.178801 9.236835 1.009362
-VERTEX2 3202 -7.638747 10.105661 0.797024
-VERTEX2 3203 -6.956039 10.807638 0.558165
-VERTEX2 3204 -7.474222 11.676902 2.424508
-VERTEX2 3205 -8.214185 12.316842 2.454895
-VERTEX2 3206 -8.983147 12.962894 2.318153
-VERTEX2 3207 -9.657157 13.718999 2.256624
-VERTEX2 3208 -10.422826 13.083241 -2.602185
-VERTEX2 3209 -11.289881 12.590482 -2.542681
-VERTEX2 3210 -12.122303 12.027104 -2.403203
-VERTEX2 3211 -12.873384 11.351102 -2.709512
-VERTEX2 3212 -13.313782 12.242311 1.602870
-VERTEX2 3213 -13.392514 13.226302 1.552721
-VERTEX2 3214 -13.349876 14.262051 1.501873
-VERTEX2 3215 -13.290232 15.242667 1.817143
-VERTEX2 3216 -14.288549 14.938467 -2.917622
-VERTEX2 3217 -15.241722 14.705092 -2.958947
-VERTEX2 3218 -16.223575 14.541280 3.083630
-VERTEX2 3219 -17.208046 14.602819 -2.931208
-VERTEX2 3220 -17.416058 15.549393 1.987947
-VERTEX2 3221 -17.808645 16.465268 1.917757
-VERTEX2 3222 -18.150696 17.384781 1.814809
-VERTEX2 3223 -18.412042 18.369604 1.978652
-VERTEX2 3224 -18.030328 17.424450 -1.408654
-VERTEX2 3225 -17.870616 16.414151 -1.506654
-VERTEX2 3226 -17.827829 15.425273 -1.621630
-VERTEX2 3227 -17.870034 14.444168 -1.671149
-VERTEX2 3228 -16.826865 14.328784 0.386637
-VERTEX2 3229 -15.894050 14.698187 0.128994
-VERTEX2 3230 -14.891655 14.818317 -0.184633
-VERTEX2 3231 -13.912140 14.626922 0.142077
-VERTEX2 3232 -13.778853 13.686675 -1.440378
-VERTEX2 3233 -13.632646 12.690066 -1.268760
-VERTEX2 3234 -13.359235 11.728165 -1.472129
-VERTEX2 3235 -13.254729 10.704722 -1.601628
-VERTEX2 3236 -12.251970 10.664282 0.007272
-VERTEX2 3237 -11.256948 10.652925 -0.064511
-VERTEX2 3238 -10.288136 10.598997 0.281260
-VERTEX2 3239 -9.310883 10.872360 0.435850
-VERTEX2 3240 -9.714056 11.788750 1.893577
-VERTEX2 3241 -10.035113 12.729829 1.770298
-VERTEX2 3242 -10.233468 13.731271 1.636422
-VERTEX2 3243 -10.307827 14.739786 1.792348
-VERTEX2 3244 -9.349869 14.926281 0.362457
-VERTEX2 3245 -8.390887 15.293806 0.522728
-VERTEX2 3246 -7.526970 15.768778 0.283409
-VERTEX2 3247 -6.550154 15.999660 0.206843
-VERTEX2 3248 -6.359451 15.024005 -1.171765
-VERTEX2 3249 -5.997402 14.099760 -1.501091
-VERTEX2 3250 -5.949488 13.094596 -1.419780
-VERTEX2 3251 -5.803015 12.081694 -1.604796
-VERTEX2 3252 -4.765908 12.019252 0.135014
-VERTEX2 3253 -3.796813 12.199496 0.072581
-VERTEX2 3254 -2.773472 12.320474 0.434302
-VERTEX2 3255 -1.823928 12.799701 0.361673
-VERTEX2 3256 -2.183088 13.738521 2.034889
-VERTEX2 3257 -2.644892 14.618959 1.828809
-VERTEX2 3258 -2.921483 15.547810 1.784544
-VERTEX2 3259 -3.127770 16.528285 1.842722
-VERTEX2 3260 -4.093545 16.265048 2.923192
-VERTEX2 3261 -5.043692 16.462613 2.962705
-VERTEX2 3262 -6.060469 16.676114 2.823193
-VERTEX2 3263 -6.965098 17.009918 2.743761
-VERTEX2 3264 -6.047090 16.606570 -0.397809
-VERTEX2 3265 -5.145228 16.206622 -0.003381
-VERTEX2 3266 -4.137028 16.207775 -0.222592
-VERTEX2 3267 -3.151560 15.976325 -0.318218
-VERTEX2 3268 -2.796842 16.902520 1.138967
-VERTEX2 3269 -2.374623 17.808541 1.016315
-VERTEX2 3270 -1.863487 18.652358 1.071044
-VERTEX2 3271 -1.382765 19.523515 1.258169
-VERTEX2 3272 -2.303101 19.819199 2.847127
-VERTEX2 3273 -3.250420 20.124359 2.666669
-VERTEX2 3274 -4.143783 20.550167 2.500911
-VERTEX2 3275 -4.952503 21.145856 2.635806
-VERTEX2 3276 -5.809419 21.610294 2.566344
-VERTEX2 3277 -6.648920 22.146593 2.625789
-VERTEX2 3278 -7.510417 22.662225 2.863881
-VERTEX2 3279 -8.528554 22.909173 2.987408
-VERTEX2 3280 -7.573615 22.749443 -0.468716
-VERTEX2 3281 -6.701258 22.292962 -0.292530
-VERTEX2 3282 -5.712557 21.991507 -0.398391
-VERTEX2 3283 -4.789806 21.599555 -0.311014
-VERTEX2 3284 -5.074162 20.645439 -2.019796
-VERTEX2 3285 -5.517531 19.745366 -1.918396
-VERTEX2 3286 -5.912596 18.820381 -2.186628
-VERTEX2 3287 -6.471290 17.978788 -1.881462
-VERTEX2 3288 -6.838134 17.011159 -2.190981
-VERTEX2 3289 -7.424851 16.201377 -2.463292
-VERTEX2 3290 -8.211090 15.569568 -2.715629
-VERTEX2 3291 -9.112442 15.103515 -2.676690
-VERTEX2 3292 -9.535709 15.997707 2.224650
-VERTEX2 3293 -10.150798 16.799679 2.205259
-VERTEX2 3294 -10.735849 17.608832 2.279883
-VERTEX2 3295 -11.395830 18.349311 2.110161
-VERTEX2 3296 -12.255423 17.838882 -2.680833
-VERTEX2 3297 -13.164197 17.411497 -2.573773
-VERTEX2 3298 -14.041082 16.855076 -2.137921
-VERTEX2 3299 -14.595505 16.054949 -2.247736
-VERTEX2 3300 -13.962921 16.821857 0.567012
-VERTEX2 3301 -13.137803 17.356191 0.319028
-VERTEX2 3302 -12.174452 17.622598 0.447162
-VERTEX2 3303 -11.270689 18.084098 0.686933
-VERTEX2 3304 -10.649491 17.294923 -0.638111
-VERTEX2 3305 -9.901769 16.715057 -0.445346
-VERTEX2 3306 -8.989374 16.262800 -0.395048
-VERTEX2 3307 -8.052431 15.894290 -0.144623
-VERTEX2 3308 -8.220300 14.914104 -1.580547
-VERTEX2 3309 -8.230982 13.914843 -1.502671
-VERTEX2 3310 -8.119392 12.915852 -1.318437
-VERTEX2 3311 -7.879147 11.931480 -1.490901
-VERTEX2 3312 -6.884957 12.014632 -0.066427
-VERTEX2 3313 -5.881198 11.930951 -0.036257
-VERTEX2 3314 -4.866842 11.931779 -0.160370
-VERTEX2 3315 -3.924246 11.759597 -0.280121
-VERTEX2 3316 -4.893851 12.017773 3.094157
-VERTEX2 3317 -5.882768 12.087629 3.003134
-VERTEX2 3318 -6.854596 12.242344 -3.140416
-VERTEX2 3319 -7.833110 12.227193 2.812394
-VERTEX2 3320 -6.916234 11.926768 -0.169626
-VERTEX2 3321 -5.926346 11.725491 -0.299611
-VERTEX2 3322 -4.968742 11.415964 -0.729121
-VERTEX2 3323 -4.204143 10.763258 -0.857535
-VERTEX2 3324 -3.556107 10.035260 -0.908250
-VERTEX2 3325 -2.959024 9.234613 -0.679067
-VERTEX2 3326 -2.188520 8.578672 -0.618614
-VERTEX2 3327 -1.367207 8.045637 -0.698639
-VERTEX2 3328 -2.125610 8.699404 2.453723
-VERTEX2 3329 -2.889552 9.318439 2.380570
-VERTEX2 3330 -3.667459 10.003635 2.366748
-VERTEX2 3331 -4.396355 10.673627 2.517861
-VERTEX2 3332 -3.592133 10.094844 -0.484195
-VERTEX2 3333 -2.716321 9.657610 -0.836544
-VERTEX2 3334 -2.014067 8.903936 -0.537577
-VERTEX2 3335 -1.168731 8.394132 -0.339737
-VERTEX2 3336 -0.854940 9.361476 1.205482
-VERTEX2 3337 -0.507998 10.268577 0.998048
-VERTEX2 3338 0.075892 11.115247 0.948673
-VERTEX2 3339 0.689826 11.948790 0.982819
-VERTEX2 3340 1.276466 12.744231 0.691133
-VERTEX2 3341 2.041889 13.367715 0.622007
-VERTEX2 3342 2.850263 13.902548 0.639810
-VERTEX2 3343 3.638467 14.507839 0.753664
-VERTEX2 3344 4.293708 13.769743 -0.624595
-VERTEX2 3345 5.118170 13.173069 -0.356053
-VERTEX2 3346 6.060649 12.831070 -0.012054
-VERTEX2 3347 7.052593 12.816694 -0.182505
-VERTEX2 3348 7.228101 13.794776 1.189346
-VERTEX2 3349 7.638470 14.695762 1.439783
-VERTEX2 3350 7.699718 15.668662 1.123930
-VERTEX2 3351 8.075940 16.569783 0.981153
-VERTEX2 3352 7.266170 17.120072 2.755384
-VERTEX2 3353 6.355103 17.462021 2.416920
-VERTEX2 3354 5.604553 18.135604 2.557357
-VERTEX2 3355 4.795613 18.694381 2.723961
-VERTEX2 3356 5.201043 19.595954 0.913812
-VERTEX2 3357 5.829407 20.394158 1.100245
-VERTEX2 3358 6.235721 21.328276 0.734010
-VERTEX2 3359 6.990805 21.998997 0.603361
-VERTEX2 3360 7.557330 21.167581 -1.004644
-VERTEX2 3361 8.110089 20.322508 -1.093701
-VERTEX2 3362 8.562096 19.459980 -1.000375
-VERTEX2 3363 9.072740 18.633546 -0.994457
-VERTEX2 3364 8.209038 18.075122 -2.400855
-VERTEX2 3365 7.459153 17.399597 -2.598676
-VERTEX2 3366 6.586763 16.905776 -2.704568
-VERTEX2 3367 5.665752 16.497362 -2.424372
-VERTEX2 3368 6.335694 15.764064 -0.800992
-VERTEX2 3369 7.030898 15.031212 -0.717591
-VERTEX2 3370 7.780107 14.370682 -0.687983
-VERTEX2 3371 8.573888 13.738629 -0.400311
-VERTEX2 3372 7.662745 14.140246 3.118737
-VERTEX2 3373 6.665595 14.156198 2.862376
-VERTEX2 3374 5.711693 14.432679 2.932024
-VERTEX2 3375 4.741851 14.660240 -3.065856
-VERTEX2 3376 4.837540 13.634801 -1.397365
-VERTEX2 3377 5.030272 12.650828 -1.907373
-VERTEX2 3378 4.715157 11.710978 -2.044686
-VERTEX2 3379 4.258075 10.805898 -2.171697
-VERTEX2 3380 5.082440 10.238177 -0.663447
-VERTEX2 3381 5.849234 9.611349 -0.382733
-VERTEX2 3382 6.762557 9.233812 0.053372
-VERTEX2 3383 7.786605 9.326799 0.148960
-VERTEX2 3384 7.914822 8.378969 -1.234539
-VERTEX2 3385 8.248654 7.461167 -1.005116
-VERTEX2 3386 8.761010 6.641943 -1.127756
-VERTEX2 3387 9.182232 5.712766 -1.259832
-VERTEX2 3388 10.125199 6.008582 0.339604
-VERTEX2 3389 11.092491 6.367118 0.436870
-VERTEX2 3390 11.971125 6.783041 0.184450
-VERTEX2 3391 12.950812 6.961259 0.352434
-VERTEX2 3392 12.580360 7.874264 1.854120
-VERTEX2 3393 12.337089 8.802039 1.738061
-VERTEX2 3394 12.163764 9.766741 1.908148
-VERTEX2 3395 11.848571 10.695359 1.920768
-VERTEX2 3396 12.192291 9.742564 -1.275092
-VERTEX2 3397 12.534836 8.779570 -1.326482
-VERTEX2 3398 12.781226 7.815608 -1.428101
-VERTEX2 3399 12.924417 6.840179 -1.270197
-VERTEX2 3400 13.901976 7.168337 -0.051516
-VERTEX2 3401 14.878574 7.087291 -0.182492
-VERTEX2 3402 15.879282 6.894825 -0.582389
-VERTEX2 3403 16.717350 6.305108 -0.425726
-VERTEX2 3404 17.140231 7.173403 0.871889
-VERTEX2 3405 17.782454 7.967510 0.699388
-VERTEX2 3406 18.518595 8.595280 0.498262
-VERTEX2 3407 19.391333 9.075620 0.711627
-VERTEX2 3408 20.018764 8.335802 -0.854599
-VERTEX2 3409 20.648797 7.579323 -0.895416
-VERTEX2 3410 21.312931 6.781845 -0.977443
-VERTEX2 3411 21.868773 5.958302 -0.799027
-VERTEX2 3412 22.603227 6.659148 0.489656
-VERTEX2 3413 23.467978 7.137864 0.423479
-VERTEX2 3414 24.379216 7.588634 0.568110
-VERTEX2 3415 25.210094 8.107961 0.229260
-VERTEX2 3416 25.444849 7.156696 -1.359688
-VERTEX2 3417 25.680468 6.192020 -1.467197
-VERTEX2 3418 25.837274 5.165171 -1.137003
-VERTEX2 3419 26.262035 4.270758 -0.900117
-VERTEX2 3420 25.634156 5.035452 2.268769
-VERTEX2 3421 24.987389 5.812803 2.362977
-VERTEX2 3422 24.288211 6.526190 2.293947
-VERTEX2 3423 23.611416 7.282495 2.921665
-VERTEX2 3424 23.851808 8.237075 1.279527
-VERTEX2 3425 24.127329 9.207915 0.864345
-VERTEX2 3426 24.816170 9.959932 0.768036
-VERTEX2 3427 25.564487 10.626907 0.659733
-VERTEX2 3428 24.806452 9.999693 -2.813418
-VERTEX2 3429 23.860914 9.670441 -2.742213
-VERTEX2 3430 22.936269 9.308822 -2.477086
-VERTEX2 3431 22.133799 8.712350 -2.570252
-VERTEX2 3432 22.666874 7.881834 -1.253752
-VERTEX2 3433 22.962199 6.951849 -1.383187
-VERTEX2 3434 23.135404 5.980042 -1.312164
-VERTEX2 3435 23.419182 4.998682 -1.351775
-VERTEX2 3436 23.174274 5.980023 1.398169
-VERTEX2 3437 23.347684 6.994149 1.486192
-VERTEX2 3438 23.397773 7.986512 1.593041
-VERTEX2 3439 23.375312 8.974853 1.201507
-VERTEX2 3440 24.296761 8.592067 -0.335869
-VERTEX2 3441 25.223191 8.257320 -0.234992
-VERTEX2 3442 26.238162 8.033153 -0.127966
-VERTEX2 3443 27.221986 7.886757 -0.238508
-VERTEX2 3444 26.227714 8.118264 3.090095
-VERTEX2 3445 25.230412 8.154566 2.826793
-VERTEX2 3446 24.307964 8.449747 2.392182
-VERTEX2 3447 23.558281 9.129665 2.573636
-VERTEX2 3448 24.087901 9.972576 1.045198
-VERTEX2 3449 24.580995 10.856979 1.166944
-VERTEX2 3450 24.987008 11.784942 0.999582
-VERTEX2 3451 25.534722 12.608627 1.289056
-VERTEX2 3452 25.282153 11.675339 -1.966044
-VERTEX2 3453 24.920471 10.721923 -1.858084
-VERTEX2 3454 24.683526 9.760701 -2.095826
-VERTEX2 3455 24.209799 8.873643 -2.291399
-VERTEX2 3456 24.864850 9.585476 0.656594
-VERTEX2 3457 25.606504 10.221221 0.601950
-VERTEX2 3458 26.405777 10.751299 0.717892
-VERTEX2 3459 27.152587 11.449108 0.916189
-VERTEX2 3460 27.964959 10.812272 -0.452059
-VERTEX2 3461 28.871529 10.354909 -0.305340
-VERTEX2 3462 29.819920 10.018777 -0.210015
-VERTEX2 3463 30.804446 9.843582 -0.245475
-VERTEX2 3464 30.590410 8.877019 -1.957615
-VERTEX2 3465 30.201201 7.992352 -1.982838
-VERTEX2 3466 29.778827 7.102927 -1.895254
-VERTEX2 3467 29.473439 6.182565 -2.027456
-VERTEX2 3468 29.940091 7.095547 1.493639
-VERTEX2 3469 30.018150 8.127711 1.811531
-VERTEX2 3470 29.843222 9.096677 1.981165
-VERTEX2 3471 29.436225 10.018165 2.097130
-VERTEX2 3472 29.942797 9.138767 -1.220048
-VERTEX2 3473 30.316871 8.187289 -1.684765
-VERTEX2 3474 30.189404 7.197634 -1.814972
-VERTEX2 3475 29.945923 6.220533 -2.107732
-VERTEX2 3476 29.123881 6.735619 2.757357
-VERTEX2 3477 28.177093 7.104933 3.035276
-VERTEX2 3478 27.169830 7.190345 3.091268
-VERTEX2 3479 26.144202 7.242440 3.030613
-VERTEX2 3480 26.051336 6.234756 -1.860923
-VERTEX2 3481 25.750577 5.260280 -1.967975
-VERTEX2 3482 25.404511 4.334588 -1.816128
-VERTEX2 3483 25.138614 3.357749 -1.840262
-VERTEX2 3484 24.195991 3.638066 -3.032851
-VERTEX2 3485 23.178055 3.549184 3.090173
-VERTEX2 3486 22.190897 3.586182 2.555277
-VERTEX2 3487 21.365234 4.164002 2.605225
-VERTEX2 3488 21.866954 5.017541 0.825512
-VERTEX2 3489 22.550964 5.755711 0.957101
-VERTEX2 3490 23.122261 6.571984 0.997532
-VERTEX2 3491 23.656968 7.450613 1.351484
-VERTEX2 3492 22.706416 7.665401 2.586575
-VERTEX2 3493 21.832858 8.189486 2.694875
-VERTEX2 3494 20.909372 8.646807 2.828597
-VERTEX2 3495 19.956116 8.977382 2.549816
-VERTEX2 3496 20.516817 9.805745 1.196147
-VERTEX2 3497 20.885958 10.751125 1.653777
-VERTEX2 3498 20.815471 11.802831 1.690265
-VERTEX2 3499 20.659287 12.742348 1.927744
-EDGE2 0 1 1.030390 0.011350 0.387567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1 2 1.013900 -0.058639 0.083793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2 3 1.027650 -0.007456 -0.303543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3 4 -0.012016 1.004360 1.307363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 4 5 1.016030 0.014565 0.202022 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 5 6 1.023890 0.006808 0.063774 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 6 7 0.957734 0.003159 0.144125 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 7 8 -1.023820 -0.013668 2.954592 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 8 9 1.023440 0.013984 -0.144962 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 9 10 1.003350 0.022250 0.090414 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 10 11 0.977245 0.019042 -0.379963 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 11 12 -0.996880 -0.025512 -3.125117 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 12 13 0.990646 0.018396 0.073068 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 13 14 0.945873 0.008893 -0.199401 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 14 15 1.000010 0.006428 -0.038997 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 15 16 0.037872 -1.026090 -1.303629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 16 17 0.983790 0.019891 0.314943 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 17 18 0.957199 0.029587 -0.220804 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 18 19 0.992140 0.019201 0.069219 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 19 20 -0.045921 -1.016320 -1.509999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 20 21 0.998450 -0.005232 -0.254026 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 21 22 0.988728 0.009034 0.174000 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 22 23 0.989422 0.006982 -0.128950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 23 24 -1.002010 -0.006263 -3.019748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 24 25 1.015350 0.004913 -0.248541 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 25 26 1.032990 -0.001727 0.018096 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 26 27 0.989137 -0.008571 0.116915 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 27 28 -0.048400 0.981715 1.661036 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 28 29 1.030820 -0.021271 -0.160267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 29 30 1.011920 0.016448 0.072745 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 30 31 0.991338 0.007812 -0.118333 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 31 32 0.008611 -0.974025 -1.793211 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 32 33 1.042560 0.010669 0.084960 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 33 34 0.990826 0.016695 0.104744 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 34 35 0.995988 0.029526 0.069772 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 35 36 -0.010774 0.996051 1.400713 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 36 37 1.004990 0.011086 0.116079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 37 38 1.038430 0.014678 -0.001867 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 38 39 1.006250 0.006744 -0.022270 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 39 40 0.056163 0.984988 1.521664 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 40 41 0.984656 -0.031925 -0.040647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 41 42 1.002660 0.030635 0.231040 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 42 43 0.986417 -0.013098 -0.131869 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 43 44 0.978720 0.012078 -0.163104 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 44 45 0.996113 -0.040731 0.019277 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 45 46 1.002550 -0.002163 0.090734 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 46 47 0.999641 -0.033650 -0.017381 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 47 48 -0.949748 0.011758 -2.850349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 48 49 1.017390 0.012380 0.173240 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 49 50 1.015480 0.027402 0.216561 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 50 51 1.052270 0.014738 0.228464 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 51 52 -0.010814 -0.984360 -1.611748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 52 53 1.030710 0.008959 -0.562278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 53 54 0.983420 0.009794 0.220811 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 54 55 1.012040 -0.015331 -0.096368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 55 56 -0.003658 -0.984986 -1.181651 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 56 57 1.031000 -0.016325 0.040309 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 57 58 0.983393 -0.011345 -0.238541 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 58 59 1.010240 0.011576 -0.258508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 59 60 0.020108 -1.008590 -1.657019 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 60 61 0.992544 -0.004063 -0.019529 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 61 62 0.980911 -0.012678 0.126626 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 62 63 1.007650 -0.037094 -0.193089 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 63 64 -0.014542 -0.998609 -1.265651 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 64 65 1.037940 -0.016831 0.168723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 65 66 0.991200 0.011571 -0.103281 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 66 67 0.949443 -0.015492 -0.426527 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 67 68 -0.978361 -0.009274 3.130876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 68 69 1.003670 -0.035297 0.086808 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 69 70 1.029810 0.002555 0.177498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 70 71 1.036520 0.011807 -0.106617 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 71 72 0.003982 -0.993979 -1.742070 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 72 73 0.969371 -0.030602 0.105616 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 73 74 0.985691 0.011144 0.178298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 74 75 0.981205 -0.005965 -0.068382 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 75 76 -0.008260 0.981841 1.657284 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 76 77 1.013990 0.033209 -0.091034 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 77 78 1.027950 0.009841 0.053251 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 78 79 1.002650 -0.007743 0.020039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 79 80 -0.010210 -0.978673 -1.720566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 80 81 1.012650 0.019201 -0.192303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 81 82 0.994241 -0.031908 0.008178 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 82 83 1.009250 0.005910 0.056856 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 83 84 -0.018483 1.033070 1.307103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 84 85 0.984696 0.019624 0.132336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 85 86 0.993027 0.010799 -0.160219 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 86 87 0.992905 0.021361 0.244947 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 87 88 0.001218 1.040310 1.491466 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 88 89 1.007670 -0.015099 -0.099532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 89 90 1.012260 -0.005391 0.217573 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 90 91 1.034570 0.002973 -0.050112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 91 92 -0.015952 0.972423 1.559483 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 92 93 0.990753 0.062025 -0.388269 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 93 94 0.971423 0.014250 -0.295555 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 94 95 1.022720 -0.027882 0.088801 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 95 96 -0.019324 1.049340 1.415895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 96 97 1.039310 -0.013089 0.058706 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 97 98 0.993004 0.039366 0.075800 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 98 99 1.038970 -0.023896 -0.384221 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 99 100 -0.985853 -0.009798 -3.007483 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 100 101 1.024650 0.031728 0.142021 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 101 102 0.993960 0.020257 -0.098189 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 102 103 1.001500 0.024148 0.162534 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 103 104 -1.008670 -0.003535 2.928941 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 104 105 1.003860 0.004822 0.279019 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 105 106 0.963589 0.002141 -0.058662 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 106 107 1.006700 0.025897 -0.375709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 107 108 -0.024236 -1.039040 -1.838842 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 108 109 1.008390 0.024656 -0.118427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 109 110 0.995059 -0.012998 0.089733 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 110 111 0.982381 -0.004349 -0.229710 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 111 112 -0.993280 0.037499 3.046431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 112 113 1.021940 -0.003415 0.147650 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 113 114 1.022380 -0.007102 -0.133984 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 114 115 1.011560 -0.014506 -0.179682 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 115 116 -0.008907 0.980541 1.757380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 116 117 1.009140 -0.030308 -0.122829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 117 118 0.983711 -0.005317 0.005154 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 118 119 0.985152 0.005509 0.092673 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 119 120 -1.010150 -0.053177 -3.080818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 120 121 1.010440 -0.028117 -0.044117 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 121 122 0.989826 -0.015204 -0.087761 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 122 123 1.021360 0.035312 -0.185182 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 123 124 -0.008542 0.950933 1.880962 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 124 125 1.004770 0.016793 -0.039632 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 125 126 1.026090 -0.019208 -0.236127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 126 127 0.963220 0.033064 0.222643 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 127 128 -0.041013 1.015960 1.704018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 128 129 1.005380 -0.013793 -0.219984 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 129 130 1.006360 -0.026231 -0.330025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 130 131 1.003810 -0.010202 0.039948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 131 132 0.040168 -1.012670 -1.521564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 132 133 1.004030 -0.009232 0.043653 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 133 134 0.983715 -0.003329 -0.425725 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 134 135 0.989074 0.039947 0.124082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 135 136 -1.031750 0.032475 -3.047064 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 136 137 0.994739 -0.018182 0.236105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 137 138 1.021410 0.006933 0.041994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 138 139 1.036590 -0.008815 -0.198741 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 139 140 -1.000710 0.023227 2.971641 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 140 141 1.014140 0.004372 -0.294304 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 141 142 1.036630 0.020324 0.353521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 142 143 1.020940 0.021674 -0.086596 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 143 144 -1.001210 0.035493 2.986153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 144 145 0.975633 0.019668 -0.330174 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 145 146 0.990986 -0.026062 0.259512 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 146 147 1.012840 -0.012720 0.153278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 147 148 0.011961 -1.030700 -1.673923 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 148 149 1.007850 0.060060 0.216921 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 149 150 1.002510 0.001280 -0.133093 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 150 151 0.975827 0.040302 -0.091282 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 151 152 0.006114 1.031330 1.509122 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 152 153 0.984407 0.004756 -0.306806 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 153 154 0.984021 -0.025557 -0.170134 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 154 155 0.958219 0.007405 0.010989 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 155 156 -0.979396 0.020563 3.109419 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 156 157 1.014320 -0.042657 -0.051543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 157 158 1.004040 -0.019473 0.071395 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 158 159 1.001800 -0.042299 0.178752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 159 160 -1.015340 -0.004710 -3.019960 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 160 161 0.977003 0.000202 -0.193466 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 161 162 1.028150 -0.003961 0.177549 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 162 163 0.964528 0.014735 0.136128 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 163 164 -0.956570 0.008850 -3.090959 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 164 165 1.002870 0.021287 0.421430 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 165 166 0.952639 -0.035047 0.490918 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 166 167 1.030070 0.021021 0.285448 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 167 168 0.018523 -1.007660 -1.815007 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 168 169 0.996015 -0.000706 0.123252 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 169 170 0.968562 -0.006568 0.174118 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 170 171 1.001360 -0.009948 0.160340 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 171 172 0.006632 -1.005130 -1.610103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 172 173 0.987193 0.002466 0.510854 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 173 174 0.971984 0.012405 -0.199906 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 174 175 1.005660 0.036410 -0.059548 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 175 176 -1.027960 -0.002102 2.929025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 176 177 1.007170 0.015267 -0.135308 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 177 178 0.976492 0.021056 -0.074169 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 178 179 0.966935 -0.001725 -0.056312 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 179 180 1.007850 -0.019587 0.177623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 180 181 0.988927 0.018030 -0.011619 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 181 182 1.006010 0.006320 0.024457 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 182 183 1.011060 -0.019970 -0.025316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 183 184 0.003868 -0.968408 -1.286399 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 184 185 1.030370 0.007996 0.204341 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 185 186 0.982633 0.028736 -0.201322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 186 187 1.001960 0.013313 0.144686 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 187 188 0.010186 -0.988517 -1.412918 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 188 189 0.988964 0.014161 -0.121015 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 189 190 1.027400 0.011345 -0.225671 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 190 191 0.986312 0.013994 -0.031368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 191 192 0.047125 0.984631 1.571653 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 192 193 0.984000 0.052823 -0.005067 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 193 194 0.993458 0.016820 -0.041587 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 194 195 1.004520 0.011934 -0.066876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 195 196 0.004544 0.990061 1.239694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 196 197 1.014900 -0.011568 -0.174851 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 197 198 0.993741 0.003423 -0.082457 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 198 199 1.019660 -0.032184 -0.146313 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 199 200 -0.022337 -1.015730 -1.851792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 200 201 0.963457 0.038375 -0.038623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 201 202 0.988326 -0.001105 0.524499 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 202 203 0.990433 -0.006104 0.086745 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 203 204 0.027934 1.023810 1.394831 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 204 205 0.963901 0.007727 -0.074299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 205 206 1.003560 -0.018077 0.192543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 206 207 1.010020 -0.019375 0.091496 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 207 208 -1.003500 0.035123 -2.997773 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 208 209 1.018180 0.001993 0.402633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 209 210 0.978662 -0.010505 -0.279567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 210 211 1.014680 0.006353 0.021709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 211 212 0.004473 -0.990545 -1.688508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 212 213 0.966053 -0.004520 -0.079298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 213 214 1.020000 0.017710 -0.074940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 214 215 0.998138 -0.039972 -0.139054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 215 216 0.036956 0.980448 1.434505 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 216 217 1.013180 -0.025324 -0.491598 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 217 218 0.984727 0.037051 -0.115705 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 218 219 0.990957 -0.004934 -0.199256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 219 220 -1.004870 0.001790 -3.089458 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 220 221 1.011350 -0.033106 0.067253 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 221 222 1.013830 0.013607 0.100137 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 222 223 1.021400 -0.005130 0.024221 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 223 224 -0.985132 0.022097 3.056197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 224 225 1.013020 -0.029178 -0.024671 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 225 226 1.032940 0.024333 -0.122638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 226 227 0.960381 -0.029303 0.126206 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 227 228 -0.002465 -1.020600 -1.363197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 228 229 0.936984 0.013013 -0.142481 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 229 230 0.986469 -0.023988 -0.008667 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 230 231 0.982838 -0.008844 -0.126085 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 231 232 0.023141 -0.987827 -1.654948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 232 233 1.000570 0.033697 -0.038132 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 233 234 0.976144 -0.018401 0.306880 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 234 235 1.019670 0.000884 0.202983 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 235 236 -0.009167 1.008660 1.401843 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 236 237 1.027540 -0.010776 0.234917 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 237 238 1.023980 0.039600 0.044324 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 238 239 0.990301 -0.016679 -0.213706 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 239 240 -0.989796 0.019005 -2.969498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 240 241 0.963382 -0.000603 -0.364334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 241 242 0.994306 -0.019836 0.061647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 242 243 1.038120 -0.016867 0.327589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 243 244 -0.007591 0.961465 1.317760 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 244 245 0.960155 -0.030124 -0.132878 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 245 246 1.003970 -0.011043 -0.239919 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 246 247 0.988968 -0.017772 0.038505 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 247 248 1.004690 -0.003173 -0.358038 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 248 249 0.992121 0.037331 0.001075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 249 250 0.992702 0.036234 -0.288698 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 250 251 1.000130 0.011351 -0.010546 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 251 252 -1.029820 0.024493 -3.134419 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 252 253 1.029540 -0.017626 0.061103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 253 254 0.998637 -0.021484 0.026970 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 254 255 1.016920 -0.015167 -0.258380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 255 256 -1.004760 0.003429 -3.000537 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 256 257 0.980232 -0.011121 0.144140 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 257 258 1.005180 -0.020542 0.034950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 258 259 0.958879 -0.010994 0.334940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 259 260 -0.998203 -0.025294 2.905897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 260 261 1.003920 0.049092 0.338113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 261 262 0.976826 0.027270 0.055355 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 262 263 0.970914 0.018372 -0.267493 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 263 264 0.010073 -0.994126 -1.609994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 264 265 1.032040 0.014787 -0.127663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 265 266 0.995474 -0.002145 0.040658 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 266 267 0.981731 0.031345 0.286826 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 267 268 0.036493 -1.003890 -1.611480 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 268 269 0.972807 -0.018199 -0.463444 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 269 270 1.026630 -0.003313 0.179792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 270 271 0.982247 -0.008291 -0.098166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 271 272 -0.022896 1.026330 1.651888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 272 273 1.056030 0.011267 0.032100 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 273 274 1.003210 -0.000326 0.126554 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 274 275 0.974854 -0.026080 0.186329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 275 276 -0.015059 1.063790 1.523410 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 276 277 0.987675 -0.006952 0.003672 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 277 278 0.980151 0.011189 0.014200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 278 279 1.013510 0.018151 0.000069 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 279 280 -0.018242 -0.967324 -1.401729 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 280 281 0.988024 -0.025696 0.129242 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 281 282 0.986859 -0.025799 -0.107897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 282 283 0.995969 0.030756 -0.064848 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 283 284 -1.016320 0.013961 -3.113035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 284 285 0.982895 -0.027410 -0.115994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 285 286 0.962236 0.031205 0.179516 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 286 287 1.006220 0.043033 -0.225143 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 287 288 0.029808 0.982415 1.541285 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 288 289 0.995180 0.000684 -0.144239 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 289 290 1.030300 -0.014156 -0.014891 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 290 291 0.966349 -0.017528 -0.000136 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 291 292 0.002010 -0.993415 -1.825309 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 292 293 1.030760 0.006563 0.058182 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 293 294 0.963984 -0.009316 -0.105136 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 294 295 0.984375 -0.042848 0.036835 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 295 296 0.022162 1.025920 1.779092 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 296 297 1.014890 0.001096 0.222494 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 297 298 0.963497 -0.031833 -0.008766 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 298 299 1.002440 -0.003957 0.019263 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 299 300 -0.020829 -0.995117 -1.462275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 300 301 0.967824 -0.033065 0.045204 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 301 302 0.975523 -0.013762 0.304631 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 302 303 0.973258 -0.027176 0.017606 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 303 304 0.009594 -0.986178 -1.258902 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 304 305 0.996325 -0.002570 0.015152 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 305 306 0.998945 0.006985 -0.065849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 306 307 0.996313 -0.023051 -0.289346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 307 308 -0.005678 1.004080 1.685607 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 308 309 1.009140 0.008062 0.029950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 309 310 1.012000 0.015903 0.149824 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 310 311 1.033710 0.005938 -0.171279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 311 312 -0.002452 1.016190 1.920110 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 312 313 0.987756 0.025956 0.191165 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 313 314 0.972527 -0.004009 0.079151 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 314 315 1.033540 -0.028663 -0.039691 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 315 316 -0.017823 -0.995791 -1.459334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 316 317 1.035760 -0.007201 -0.002063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 317 318 0.976907 -0.014573 0.226653 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 318 319 1.046220 0.041694 -0.250262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 319 320 0.004385 -1.006310 -1.583006 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 320 321 0.976909 0.036323 0.282343 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 321 322 1.015010 -0.000486 0.407866 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 322 323 0.980532 -0.008712 0.008677 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 323 324 0.010977 1.013790 1.642172 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 324 325 0.997856 0.013397 0.064700 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 325 326 1.049260 -0.016047 -0.243731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 326 327 0.976012 -0.013066 -0.151369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 327 328 -1.012850 0.041109 -3.114609 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 328 329 1.008060 -0.009449 -0.111182 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 329 330 0.980827 -0.006926 0.007458 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 330 331 0.988125 0.010222 0.125902 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 331 332 0.031120 -0.967009 -1.397238 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 332 333 1.012640 0.021056 -0.283628 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 333 334 1.015500 -0.021394 -0.055456 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 334 335 1.016650 0.041173 -0.037850 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 335 336 -0.035399 1.036450 1.857174 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 336 337 0.992504 0.019780 0.066488 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 337 338 0.962712 -0.019183 -0.226278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 338 339 1.028740 -0.040955 -0.116603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 339 340 -0.013820 0.999977 1.437644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 340 341 1.004210 -0.007730 -0.072374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 341 342 0.996881 0.003835 0.178851 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 342 343 1.010460 0.000203 0.009806 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 343 344 -0.010031 -1.037880 -1.636967 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 344 345 1.022070 0.014897 0.111607 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 345 346 1.025640 0.002468 0.112636 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 346 347 1.010310 -0.001033 -0.470635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 347 348 -0.033171 0.960247 1.508561 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 348 349 0.983643 0.000838 0.412245 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 349 350 1.035200 -0.000208 0.047439 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 350 351 1.001640 -0.037102 -0.174446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 351 352 -1.005830 -0.005212 3.105764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 352 353 1.008620 0.001802 0.226228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 353 354 0.984953 0.013647 -0.063195 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 354 355 1.019830 0.023016 0.102805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 355 356 -1.001160 0.000383 -3.136055 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 356 357 1.018030 0.023418 -0.003535 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 357 358 1.004940 0.013965 -0.079145 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 358 359 0.959382 -0.002248 -0.067898 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 359 360 -1.065100 0.031582 3.101400 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 360 361 0.997879 0.013305 0.134623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 361 362 1.060970 0.041019 0.218415 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 362 363 0.994578 0.007803 -0.140957 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 363 364 -1.026400 -0.007016 3.119185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 364 365 1.041980 0.010581 -0.045648 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 365 366 0.974737 -0.001861 -0.203121 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 366 367 1.015520 0.019847 0.286092 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 367 368 0.028487 1.005090 1.572521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 368 369 0.991958 -0.028192 -0.026643 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 369 370 1.019980 -0.003041 0.377908 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 370 371 0.944588 -0.016759 0.066880 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 371 372 0.022769 1.008830 1.586646 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 372 373 1.037910 0.014339 -0.038214 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 373 374 1.016480 0.007954 -0.025841 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 374 375 0.987573 0.019284 -0.223279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 375 376 -0.007452 -0.986353 -1.492473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 376 377 0.953791 0.002023 0.367738 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 377 378 0.966315 0.048608 -0.503559 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 378 379 1.011660 0.012792 0.218478 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 379 380 -0.012450 0.989569 1.708235 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 380 381 1.006950 -0.024919 0.283641 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 381 382 0.998709 -0.014644 0.091237 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 382 383 0.984657 0.049566 0.073656 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 383 384 -0.039898 1.022750 1.753179 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 384 385 0.982066 -0.000536 -0.160050 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 385 386 0.996833 0.028911 -0.610213 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 386 387 0.996147 0.007413 -0.000761 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 387 388 0.028114 0.968717 1.486541 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 388 389 1.002060 -0.006186 0.247474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 389 390 0.999832 0.018561 -0.321879 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 390 391 1.002970 0.017992 -0.224156 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 391 392 -0.015487 -1.002530 -1.557964 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 392 393 0.982092 0.013666 0.046426 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 393 394 1.011670 -0.028848 0.178816 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 394 395 1.012710 0.026098 0.124743 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 395 396 -0.039685 -1.002000 -1.423003 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 396 397 0.983208 -0.027103 0.043757 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 397 398 0.992204 -0.012832 -0.003355 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 398 399 0.995244 0.030791 0.176581 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 399 400 0.023748 -1.019090 -1.589503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 400 401 1.038320 -0.009211 -0.396712 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 401 402 1.004590 0.033654 0.339708 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 402 403 1.006620 -0.016318 0.167615 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 403 404 -1.023650 0.012934 -3.068830 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 404 405 1.007640 0.005695 -0.088899 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 405 406 1.008700 -0.012120 0.037972 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 406 407 0.971751 0.012841 0.260608 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 407 408 -0.015585 -0.987421 -1.353756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 408 409 0.990590 -0.000610 0.205582 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 409 410 1.005240 -0.020540 -0.180299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 410 411 0.976917 -0.011584 0.101703 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 411 412 0.025285 0.998297 1.458675 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 412 413 0.981726 -0.010608 -0.170139 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 413 414 0.969233 -0.003648 -0.369766 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 414 415 1.016620 -0.016343 0.139156 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 415 416 -1.004830 0.006344 -2.894513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 416 417 0.982766 -0.035210 -0.406285 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 417 418 1.019270 -0.019337 -0.252315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 418 419 1.003280 0.014598 0.400321 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 419 420 -0.030091 1.047650 1.474312 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 420 421 0.975289 -0.032809 0.259285 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 421 422 0.951750 0.022674 -0.098115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 422 423 1.006470 -0.051107 0.234379 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 423 424 0.979416 0.008789 -0.123713 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 424 425 0.982439 0.046866 0.080627 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 425 426 0.968721 -0.037773 -0.272334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 426 427 0.998414 -0.023554 -0.257687 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 427 428 -0.003846 1.027870 1.712517 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 428 429 0.967217 0.005981 -0.030160 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 429 430 1.029180 0.006210 0.021393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 430 431 0.969919 -0.004921 0.084062 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 431 432 0.025001 -0.963741 -1.222318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 432 433 0.998646 0.039816 -0.137977 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 433 434 0.998116 0.019787 -0.321561 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 434 435 1.008830 0.005636 -0.190736 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 435 436 -0.042064 -0.993330 -1.333535 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 436 437 0.940503 0.012754 -0.183729 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 437 438 0.990735 -0.006990 0.019723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 438 439 0.960341 0.018591 0.189116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 439 440 -0.007788 0.996636 2.059508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 440 441 1.001200 -0.000750 0.068270 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 441 442 1.002400 0.025122 -0.138102 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 442 443 1.018670 0.014033 0.070425 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 443 444 -0.952078 -0.023855 -3.105223 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 444 445 1.006350 0.001737 0.364775 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 445 446 1.016670 0.020104 -0.286264 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 446 447 0.978185 -0.009521 0.299080 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 447 448 -0.003031 -0.985923 -1.266208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 448 449 1.039490 0.005503 0.262996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 449 450 1.003640 0.028308 0.033896 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 450 451 0.999867 -0.023452 0.213686 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 451 452 -1.024970 0.032897 2.777645 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 452 453 1.013370 -0.000974 -0.180181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 453 454 1.012290 0.025029 0.230181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 454 455 0.999904 -0.005136 0.243052 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 455 456 -1.012020 0.058263 2.928624 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 456 457 1.004080 0.005425 0.172829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 457 458 1.001440 -0.005360 -0.114826 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 458 459 1.016520 -0.009668 -0.338353 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 459 460 -1.004420 0.014216 -2.844396 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 460 461 0.994726 -0.030121 -0.040637 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 461 462 1.049190 0.015648 0.136077 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 462 463 0.997902 -0.008807 -0.377765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 463 464 0.015936 0.981001 1.623020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 464 465 0.966331 -0.029990 0.439896 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 465 466 1.020520 0.034792 0.031172 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 466 467 1.005420 -0.007639 0.235999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 467 468 0.013268 -0.990512 -1.696429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 468 469 1.016840 0.007976 0.176661 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 469 470 0.953815 0.024356 -0.109828 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 470 471 0.992383 -0.020576 0.156580 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 471 472 -0.984259 -0.004032 3.128185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 472 473 1.018140 0.014553 0.073705 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 473 474 1.022920 -0.031999 0.100824 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 474 475 1.024770 0.002970 -0.190376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 475 476 -1.017360 0.024313 3.133062 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 476 477 0.997066 0.023113 -0.156330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 477 478 1.022230 0.029008 0.174347 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 478 479 0.994768 0.012139 0.016934 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 479 480 -1.017500 0.009742 -3.073746 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 480 481 0.978153 -0.019200 0.174206 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 481 482 0.989011 0.005481 0.150923 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 482 483 1.040520 -0.026045 0.088349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 483 484 0.033499 1.028000 1.697499 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 484 485 1.058610 -0.005959 -0.137807 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 485 486 0.979688 0.019445 0.407498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 486 487 1.025130 0.001318 0.356620 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 487 488 0.023272 -0.939869 -1.085198 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 488 489 0.981954 -0.004126 -0.327851 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 489 490 1.016350 0.005731 0.051126 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 490 491 1.001550 0.005493 -0.060009 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 491 492 -0.047867 -0.991252 -1.712562 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 492 493 0.994269 0.009541 0.247783 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 493 494 0.964862 -0.001774 0.041157 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 494 495 0.978857 0.026045 -0.330732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 495 496 -0.966547 0.029122 -2.966727 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 496 497 0.981790 -0.047632 -0.222900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 497 498 0.975014 -0.025418 0.305952 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 498 499 1.008240 -0.038639 -0.298152 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 499 500 -0.993365 -0.011848 -3.093632 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 500 501 0.985881 -0.017931 -0.008696 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 501 502 0.983942 -0.008486 -0.036810 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 502 503 1.012350 -0.002216 0.146004 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 503 504 -0.975362 0.004588 2.942226 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 504 505 0.941082 0.012088 0.023874 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 505 506 0.999673 0.011588 0.392807 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 506 507 0.986716 -0.048182 0.134780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 507 508 0.010333 -0.975784 -1.633510 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 508 509 1.001840 -0.004398 -0.114914 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 509 510 1.029530 -0.010107 0.247042 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 510 511 1.028260 -0.001349 -0.013530 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 511 512 -0.009511 -1.011600 -1.296232 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 512 513 0.991876 0.018149 0.158830 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 513 514 1.035620 -0.002330 -0.077336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 514 515 0.998747 0.018136 0.181039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 515 516 -0.986452 0.023140 2.832921 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 516 517 0.960760 -0.022505 0.284647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 517 518 1.027650 0.025763 -0.057277 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 518 519 1.047970 0.033121 0.073054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 519 520 0.020790 0.979847 1.462199 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 520 521 0.987088 0.044353 0.050517 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 521 522 1.006620 -0.031561 0.308369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 522 523 1.022710 -0.011468 -0.079769 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 523 524 -0.972122 0.052501 -3.083230 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 524 525 0.954726 -0.002495 -0.165430 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 525 526 1.012880 0.020793 -0.016759 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 526 527 1.004890 -0.019865 0.049350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 527 528 -1.017210 0.017228 -3.088141 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 528 529 1.000990 -0.012724 0.238997 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 529 530 1.013720 -0.018745 0.224916 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 530 531 1.037470 0.018059 0.217976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 531 532 -0.056600 1.004360 1.548678 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 532 533 1.039940 0.014157 -0.030498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 533 534 0.997099 0.014517 0.302897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 534 535 1.002710 -0.000324 -0.115446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 535 536 -0.993886 -0.012421 -3.064673 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 536 537 1.002900 -0.006539 -0.086146 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 537 538 0.950702 0.007097 0.198887 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 538 539 1.001820 -0.027064 0.077141 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 539 540 -1.011180 0.045848 3.124450 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 540 541 1.009020 0.033468 0.248116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 541 542 1.014220 0.040327 0.201481 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 542 543 1.045150 -0.004510 0.066329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 543 544 -0.019705 0.977433 1.142179 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 544 545 0.999211 -0.010477 -0.138103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 545 546 0.965988 -0.069469 -0.206271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 546 547 1.002810 -0.033404 -0.048846 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 547 548 -0.018015 1.030500 1.438322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 548 549 0.969801 -0.006510 -0.467489 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 549 550 1.022280 0.035108 -0.104403 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 550 551 1.016250 -0.011384 -0.333917 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 551 552 -0.004135 0.988278 1.457740 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 552 553 0.988505 0.034792 0.014071 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 553 554 0.988890 -0.020331 0.334375 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 554 555 1.039530 0.002386 -0.253831 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 555 556 -0.975244 -0.044017 3.105616 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 556 557 0.997493 0.011532 0.267764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 557 558 1.034600 0.002405 -0.037878 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 558 559 1.005780 -0.007456 0.269452 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 559 560 0.014353 0.993729 1.534706 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 560 561 0.945928 0.030304 -0.192911 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 561 562 1.049830 0.000941 -0.139908 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 562 563 0.978300 -0.028669 0.235067 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 563 564 0.979431 -0.011205 -0.104622 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 564 565 1.011580 0.039604 0.300673 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 565 566 0.998037 0.033279 -0.140684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 566 567 0.992544 0.018243 -0.078270 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 567 568 -1.034940 -0.016931 -3.123961 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 568 569 0.991157 -0.004422 0.174469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 569 570 1.023690 0.005726 0.262368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 570 571 1.011190 -0.055261 -0.073465 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 571 572 0.009183 1.033210 1.796684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 572 573 1.009230 -0.029052 -0.080563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 573 574 0.983956 0.023784 0.212424 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 574 575 1.008510 -0.015364 -0.054376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 575 576 -0.005013 0.985986 1.559815 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 576 577 1.004740 0.015413 -0.442584 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 577 578 0.972946 -0.010204 -0.008346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 578 579 1.011840 0.015859 -0.075090 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 579 580 0.012159 1.005810 1.677675 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 580 581 0.960362 -0.003000 0.260543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 581 582 0.984156 0.038546 0.050362 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 582 583 1.009430 -0.022616 -0.070175 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 583 584 0.016539 0.970553 1.878656 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 584 585 0.994916 0.060433 -0.309239 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 585 586 0.976859 0.022960 -0.118485 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 586 587 1.037400 0.017438 0.165167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 587 588 -0.011946 -1.001510 -1.732185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 588 589 1.010220 -0.003385 0.172709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 589 590 1.023730 0.021342 -0.151199 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 590 591 0.996862 0.003930 -0.035223 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 591 592 -0.022830 -0.995505 -1.759205 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 592 593 0.982909 0.031980 -0.160914 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 593 594 1.000920 -0.020177 -0.091997 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 594 595 1.008590 -0.006726 0.119864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 595 596 0.019768 -0.985265 -1.934271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 596 597 1.034010 0.019502 0.021346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 597 598 1.011450 0.016735 -0.275463 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 598 599 1.019050 0.022481 -0.061472 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 599 600 0.016243 -0.973859 -1.552008 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 600 601 1.005280 -0.031103 0.256052 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 601 602 1.013270 -0.013873 -0.222051 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 602 603 0.993865 -0.003195 -0.069138 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 603 604 -0.038145 1.012470 1.787847 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 604 605 1.015250 -0.037720 -0.207386 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 605 606 0.982427 0.001353 -0.315900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 606 607 1.007390 -0.003452 0.101988 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 607 608 -0.019641 1.019490 1.463597 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 608 609 1.003030 0.028594 0.327640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 609 610 1.002450 0.022603 -0.231011 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 610 611 0.990604 0.000695 -0.001955 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 611 612 -0.986214 -0.007386 -3.107991 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 612 613 1.001640 -0.035231 0.001375 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 613 614 1.060820 -0.011337 -0.053995 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 614 615 1.040500 0.022684 -0.224420 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 615 616 -1.011760 0.003575 3.023809 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 616 617 0.992042 -0.006514 0.106329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 617 618 0.979442 -0.010875 0.246741 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 618 619 0.965590 0.019012 0.025520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 619 620 0.044668 -0.973957 -1.455240 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 620 621 1.013880 0.033922 0.087255 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 621 622 1.038920 0.033279 0.382429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 622 623 1.000810 -0.026895 -0.128764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 623 624 -1.006610 0.032005 3.134072 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 624 625 1.037040 -0.021139 -0.243708 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 625 626 1.018190 -0.005540 -0.145419 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 626 627 0.991018 -0.030638 0.016492 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 627 628 0.029371 0.979917 1.673724 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 628 629 1.007170 0.018316 0.135870 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 629 630 0.977038 -0.008609 0.109682 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 630 631 0.992497 0.011267 -0.163286 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 631 632 -0.042641 0.992474 1.690781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 632 633 0.975863 -0.029947 -0.080903 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 633 634 0.992509 -0.002072 -0.053534 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 634 635 0.945528 0.005322 -0.497000 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 635 636 -0.975416 -0.006170 2.911564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 636 637 0.995577 0.007725 -0.382888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 637 638 1.003610 -0.018862 -0.054780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 638 639 1.012210 -0.010822 -0.167712 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 639 640 0.003714 -0.982351 -1.655682 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 640 641 1.003540 -0.015078 0.030858 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 641 642 1.042620 -0.017805 -0.270725 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 642 643 0.976767 -0.049280 0.156976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 643 644 -1.022690 -0.028873 -2.973475 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 644 645 0.995782 -0.031711 -0.032979 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 645 646 0.942105 0.006327 -0.040391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 646 647 0.998559 -0.012811 -0.041821 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 647 648 0.025428 1.015770 1.691640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 648 649 1.001080 -0.026210 0.024516 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 649 650 1.023100 0.007027 -0.155905 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 650 651 1.003000 0.041838 -0.210927 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 651 652 -0.003830 1.063120 1.492218 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 652 653 0.954376 0.002667 -0.101180 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 653 654 1.020920 0.049047 -0.131755 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 654 655 0.964102 -0.012500 0.049813 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 655 656 -0.997866 0.024020 3.032560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 656 657 0.975973 -0.046230 0.131844 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 657 658 0.975451 -0.018069 -0.098484 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 658 659 0.997989 0.012739 0.114561 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 659 660 0.019394 1.024830 1.496134 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 660 661 0.989672 0.001585 0.110557 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 661 662 0.991001 0.008239 -0.229288 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 662 663 0.945483 -0.011529 -0.307183 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 663 664 0.008842 -1.004320 -1.757393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 664 665 1.028750 0.001385 0.464433 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 665 666 1.002250 -0.035436 0.347818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 666 667 0.986362 -0.008567 0.126792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 667 668 -0.968012 0.000649 3.127684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 668 669 1.009510 0.022850 -0.080900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 669 670 1.000450 -0.001354 -0.035866 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 670 671 1.029680 -0.026154 0.035795 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 671 672 -0.986864 0.005666 -2.827855 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 672 673 1.017910 0.004486 0.181589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 673 674 0.991045 0.010156 0.293564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 674 675 0.969293 0.008976 -0.215994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 675 676 -1.011910 0.001695 2.671833 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 676 677 0.993077 -0.020763 -0.067102 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 677 678 0.983459 0.005819 -0.269415 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 678 679 0.968561 -0.028992 0.071035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 679 680 -0.005768 0.997766 1.281375 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 680 681 0.934790 0.031256 0.113159 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 681 682 1.028430 0.005228 -0.122397 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 682 683 0.991521 0.013558 0.062678 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 683 684 0.007229 -1.028960 -1.611226 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 684 685 1.010280 0.009158 0.261295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 685 686 0.994562 0.033297 -0.213650 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 686 687 0.985423 0.013237 -0.022512 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 687 688 0.025280 0.992311 1.245079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 688 689 0.970739 0.007772 -0.289715 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 689 690 1.011840 -0.013893 0.288399 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 690 691 0.979903 0.009947 -0.069135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 691 692 0.021886 -0.981125 -1.630694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 692 693 1.001850 0.032024 -0.125866 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 693 694 0.992716 0.018271 0.071075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 694 695 1.009200 -0.002560 -0.016490 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 695 696 0.006115 -0.980741 -1.559949 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 696 697 1.020080 -0.019358 -0.169305 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 697 698 0.987487 -0.044401 -0.054064 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 698 699 0.982945 -0.043590 0.312912 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 699 700 0.020969 1.031980 1.752711 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 700 701 1.009090 0.018698 0.018360 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 701 702 0.986430 0.007926 -0.206122 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 702 703 1.006550 0.000959 0.051299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 703 704 0.007158 1.016300 1.143643 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 704 705 0.966333 -0.030530 0.180787 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 705 706 1.039260 -0.015495 -0.031121 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 706 707 1.042810 -0.019428 0.001934 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 707 708 0.017015 0.997363 2.007492 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 708 709 0.985795 0.019493 0.145589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 709 710 1.005200 0.027450 0.216072 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 710 711 1.012760 0.033621 0.154003 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 711 712 0.043873 -0.976466 -1.652563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 712 713 0.990925 0.008425 -0.012365 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 713 714 0.997585 -0.016563 -0.003286 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 714 715 0.979214 -0.007571 -0.071766 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 715 716 -0.971446 0.009047 3.117425 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 716 717 0.991090 0.008793 -0.372550 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 717 718 1.012700 -0.005897 0.004844 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 718 719 1.012630 -0.003270 -0.051356 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 719 720 0.005524 0.997428 1.230456 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 720 721 0.997620 -0.014073 -0.104268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 721 722 0.995845 -0.016613 -0.260763 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 722 723 1.018200 -0.028338 0.039272 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 723 724 -1.000240 0.007227 2.897197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 724 725 0.978300 0.007168 0.063771 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 725 726 0.980414 -0.004706 -0.055338 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 726 727 0.990521 -0.012905 0.082915 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 727 728 0.008932 1.037480 1.722563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 728 729 1.002080 0.023710 -0.220725 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 729 730 0.997136 -0.006320 -0.038275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 730 731 1.003010 0.041649 -0.349497 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 731 732 0.000834 0.969341 1.360458 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 732 733 0.995749 -0.022235 0.012982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 733 734 0.999684 0.009323 -0.153673 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 734 735 1.006030 0.026924 0.187146 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 735 736 -0.001394 1.050880 1.342566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 736 737 1.023900 0.062019 -0.121816 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 737 738 0.981820 -0.003497 0.318452 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 738 739 1.022740 0.000711 0.170303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 739 740 -1.009400 -0.026484 2.787796 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 740 741 0.998688 0.037253 0.015728 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 741 742 0.997672 0.044447 0.035189 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 742 743 0.996682 -0.000123 -0.226098 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 743 744 0.022514 -1.007280 -1.208014 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 744 745 0.985984 0.015937 -0.138013 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 745 746 1.024470 0.026028 0.155020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 746 747 0.984640 -0.005815 0.199025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 747 748 -1.003450 -0.017299 -3.049979 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 748 749 1.032330 0.022276 0.013045 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 749 750 1.001240 0.069896 0.110043 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 750 751 0.991926 0.008162 -0.197024 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 751 752 -0.963229 0.040862 2.946025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 752 753 0.979305 -0.040392 -0.063304 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 753 754 0.990029 -0.020602 -0.502358 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 754 755 0.971995 0.049572 -0.126504 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 755 756 -0.002653 0.956387 2.041044 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 756 757 1.014970 -0.010330 0.058496 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 757 758 0.983225 -0.000971 0.066648 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 758 759 0.959020 0.054866 -0.006733 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 759 760 0.032087 0.982395 1.566952 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 760 761 0.970381 -0.032533 -0.144080 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 761 762 1.004590 -0.022634 0.279994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 762 763 1.016810 -0.031793 -0.076932 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 763 764 -0.997108 0.016051 -3.083316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 764 765 1.025330 0.011796 -0.176368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 765 766 1.017660 0.008162 -0.164246 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 766 767 1.001140 -0.024474 -0.116657 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 767 768 0.993169 0.018425 -0.160206 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 768 769 1.006810 -0.028884 -0.325640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 769 770 0.985958 0.019859 0.002216 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 770 771 1.016570 0.028870 -0.286306 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 771 772 -0.012213 -1.018400 -1.570725 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 772 773 0.951753 0.031002 0.115777 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 773 774 0.980789 -0.016029 0.137229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 774 775 1.005590 -0.006939 0.207004 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 775 776 -0.003607 -1.002060 -1.176572 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 776 777 1.001210 -0.014193 -0.136758 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 777 778 0.971573 0.016274 -0.182666 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 778 779 1.005350 -0.026215 0.203839 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 779 780 0.033801 -0.972418 -1.287043 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 780 781 1.016220 0.064318 -0.120055 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 781 782 1.007470 -0.008757 -0.121515 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 782 783 1.021860 -0.032050 -0.124780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 783 784 -0.011854 -1.011250 -1.830682 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 784 785 0.995259 -0.025972 0.043806 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 785 786 1.010820 0.062326 0.122359 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 786 787 0.988623 -0.074377 0.467350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 787 788 0.027532 1.013360 1.487349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 788 789 0.974831 0.014700 -0.051509 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 789 790 0.997521 -0.032829 0.060298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 790 791 0.994170 0.007855 0.077980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 791 792 0.003983 -0.927390 -1.128080 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 792 793 1.009220 0.029037 0.056236 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 793 794 1.020020 0.015056 0.396181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 794 795 0.997417 0.053276 0.218100 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 795 796 -0.020532 0.997830 1.892954 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 796 797 1.000990 0.011217 -0.171016 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 797 798 1.027330 -0.015621 0.276360 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 798 799 0.988796 -0.008200 -0.208521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 799 800 0.007906 -1.048560 -1.304863 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 800 801 1.011930 0.019082 -0.248734 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 801 802 1.018650 -0.002702 0.595330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 802 803 0.978156 0.026794 0.155813 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 803 804 0.006908 1.003810 1.599583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 804 805 0.981410 -0.036441 -0.007366 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 805 806 1.020650 0.002740 0.017289 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 806 807 1.030630 0.002836 0.022207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 807 808 0.029692 -1.011180 -1.524471 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 808 809 1.006860 -0.032231 0.068486 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 809 810 0.979691 0.019649 -0.211242 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 810 811 1.026410 -0.007872 -0.088470 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 811 812 -0.010088 0.964580 1.669521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 812 813 1.038790 -0.012791 0.016053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 813 814 0.985668 -0.009410 -0.101923 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 814 815 1.031460 0.019294 0.152787 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 815 816 -0.022429 0.946896 1.711419 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 816 817 0.998178 0.030521 -0.024263 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 817 818 0.977319 -0.004254 -0.193724 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 818 819 1.036280 -0.012442 0.212400 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 819 820 -0.982892 0.046713 3.085322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 820 821 0.990217 0.007036 -0.129672 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 821 822 0.985782 -0.018741 0.369681 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 822 823 0.994255 -0.011819 0.245661 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 823 824 0.009135 -1.017030 -1.773349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 824 825 0.972358 -0.046804 0.339007 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 825 826 1.037470 0.023217 -0.354050 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 826 827 1.022980 0.013021 0.101315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 827 828 0.955055 -0.002222 0.222410 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 828 829 0.996371 0.011154 -0.047693 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 829 830 0.964886 -0.008406 -0.352647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 830 831 0.977235 0.017999 -0.101895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 831 832 -1.013730 0.046226 3.036996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 832 833 1.025160 -0.014840 0.129151 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 833 834 0.983204 0.009940 -0.077118 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 834 835 0.956283 0.003696 -0.033727 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 835 836 -0.002052 0.984820 1.739875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 836 837 1.010010 -0.008587 -0.139893 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 837 838 1.007870 -0.028311 -0.222956 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 838 839 1.016570 0.015337 -0.086027 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 839 840 -0.009880 -1.040200 -1.852005 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 840 841 1.010090 -0.005629 -0.205157 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 841 842 0.979822 -0.012693 -0.046455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 842 843 1.009610 -0.026547 -0.148252 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 843 844 -0.030466 1.016170 1.371335 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 844 845 0.961012 -0.007716 -0.082237 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 845 846 1.034480 0.024719 0.256121 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 846 847 1.009230 0.007096 0.277679 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 847 848 -1.023170 0.025991 -3.047080 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 848 849 0.994705 -0.048113 -0.005977 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 849 850 1.008590 -0.015833 -0.132094 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 850 851 0.989280 0.021248 -0.171145 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 851 852 0.986647 -0.003991 -0.116010 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 852 853 1.021190 0.012375 0.261635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 853 854 1.029550 0.029288 0.170389 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 854 855 0.996823 -0.001816 0.201101 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 855 856 0.046535 -0.948973 -1.624030 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 856 857 1.001230 0.004266 0.207978 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 857 858 0.972099 0.022451 -0.018718 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 858 859 1.000820 -0.004003 -0.182404 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 859 860 -0.970762 -0.015314 -2.890348 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 860 861 0.945903 -0.037483 -0.099608 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 861 862 0.973033 -0.026668 -0.134044 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 862 863 1.010870 0.000362 -0.110980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 863 864 -1.010660 -0.029732 -2.929961 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 864 865 0.985097 0.004563 -0.271500 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 865 866 1.003460 0.014730 0.050841 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 866 867 0.986940 0.017076 0.059088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 867 868 -1.015950 0.014525 -2.984569 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 868 869 0.988295 0.006107 0.176370 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 869 870 1.030860 0.007870 0.229539 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 870 871 0.982089 -0.027989 0.201610 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 871 872 1.015240 0.001306 -0.092669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 872 873 0.963504 -0.000748 0.181225 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 873 874 1.020320 -0.039293 0.032558 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 874 875 1.006250 0.013510 0.258642 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 875 876 0.005939 0.998728 1.807771 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 876 877 0.996531 -0.010340 0.167559 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 877 878 1.010440 -0.010915 -0.065159 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 878 879 1.005120 0.005034 0.036949 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 879 880 -1.011660 -0.017694 -3.131894 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 880 881 1.013760 0.026357 0.026847 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 881 882 0.992255 -0.018599 -0.147897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 882 883 1.010790 -0.024490 -0.010385 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 883 884 -0.980522 -0.009670 -3.137888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 884 885 0.971092 0.025422 0.252492 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 885 886 1.015010 -0.006540 0.003854 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 886 887 1.007030 0.025566 -0.317309 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 887 888 -0.013630 -0.990318 -1.724238 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 888 889 1.008030 -0.017260 0.067095 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 889 890 1.041970 -0.010797 -0.201219 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 890 891 0.997600 -0.017352 -0.067570 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 891 892 -1.014390 0.002833 -2.912180 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 892 893 0.988582 0.020303 0.195655 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 893 894 0.991366 0.012744 -0.036828 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 894 895 1.011680 -0.027796 0.142135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 895 896 -0.021879 -1.013650 -1.678644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 896 897 1.008310 0.014436 -0.231600 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 897 898 1.012700 0.018705 0.151827 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 898 899 1.001580 -0.018989 0.191532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 899 900 -1.013340 0.033524 2.755270 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 900 901 0.985189 0.012052 0.094172 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 901 902 1.008790 -0.062573 -0.366242 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 902 903 0.983117 0.010422 0.080368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 903 904 -0.010605 1.007890 1.877438 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 904 905 0.994859 0.038067 0.044052 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 905 906 1.009490 0.005822 0.152334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 906 907 0.983410 0.019986 -0.131600 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 907 908 -0.011362 0.985697 1.363238 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 908 909 0.977553 0.023940 -0.272801 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 909 910 0.981520 -0.024928 0.168347 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 910 911 1.047180 0.003709 -0.109888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 911 912 -0.973460 0.018141 -2.954567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 912 913 1.006320 -0.001288 0.032048 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 913 914 0.988292 0.035110 -0.215905 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 914 915 1.017410 0.017055 0.190363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 915 916 0.013883 0.984675 1.650137 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 916 917 0.983509 -0.001759 -0.023790 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 917 918 0.985475 -0.004414 -0.189153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 918 919 1.007020 0.002466 -0.278716 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 919 920 -0.974853 0.017800 -2.928173 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 920 921 0.993411 0.011663 0.138247 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 921 922 0.998893 0.001665 0.423532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 922 923 1.010580 -0.016701 0.200456 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 923 924 -1.006320 0.001963 3.065180 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 924 925 0.966906 -0.004740 -0.278332 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 925 926 0.985308 -0.017149 -0.133685 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 926 927 0.993717 0.005518 0.416197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 927 928 0.039567 -1.013200 -1.465794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 928 929 0.991153 0.006457 0.098424 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 929 930 0.984083 0.050227 0.343376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 930 931 1.029210 0.045379 -0.174068 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 931 932 0.011641 1.044140 1.478692 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 932 933 0.990539 0.019388 -0.189793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 933 934 1.001440 0.059989 0.241037 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 934 935 1.003610 0.019905 0.391411 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 935 936 -1.015840 -0.042586 -2.936716 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 936 937 0.973764 0.017339 -0.252197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 937 938 1.030580 -0.014930 0.173784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 938 939 0.992721 -0.018886 -0.122922 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 939 940 1.027390 -0.017703 -0.030556 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 940 941 1.014510 0.005764 -0.117111 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 941 942 0.957423 0.015383 0.151525 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 942 943 1.014740 -0.027093 -0.214846 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 943 944 0.018189 -1.007440 -1.484291 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 944 945 0.999112 -0.037905 0.311790 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 945 946 1.006160 -0.004750 -0.218768 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 946 947 0.957966 0.016582 0.255498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 947 948 0.051989 -1.002980 -1.817768 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 948 949 1.006310 -0.048732 -0.272781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 949 950 0.987867 -0.039697 0.172298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 950 951 0.975686 0.009267 -0.077150 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 951 952 -1.009150 0.006354 -2.793959 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 952 953 0.994810 -0.014083 0.118991 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 953 954 1.035920 -0.032541 0.179805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 954 955 0.998326 0.012573 -0.184018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 955 956 0.016093 1.003390 1.438636 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 956 957 1.037130 -0.036420 0.425045 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 957 958 1.029130 0.002036 0.123683 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 958 959 1.014620 -0.003183 -0.286759 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 959 960 -1.006980 -0.032221 2.816160 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 960 961 1.013900 0.037610 -0.351170 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 961 962 0.998172 0.007257 -0.073383 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 962 963 1.035800 0.008178 0.542962 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 963 964 -0.979138 -0.005215 2.734898 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 964 965 0.970731 -0.003109 0.041468 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 965 966 1.027580 0.002667 0.379621 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 966 967 1.008660 0.001904 0.173891 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 967 968 -1.003200 -0.000799 -2.675422 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 968 969 1.044560 -0.004844 -0.350598 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 969 970 0.991370 0.005882 -0.054612 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 970 971 1.015070 -0.023658 -0.151243 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 971 972 0.017434 0.985974 1.456116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 972 973 1.000220 -0.021453 0.266075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 973 974 0.961596 -0.001908 -0.133479 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 974 975 0.983545 0.002732 0.085404 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 975 976 0.952546 -0.006796 0.249469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 976 977 1.014950 -0.043991 0.095262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 977 978 0.965748 0.046823 0.290505 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 978 979 0.999882 -0.062399 -0.113703 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 979 980 -1.024780 -0.010908 -2.932771 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 980 981 1.008640 0.043769 -0.309610 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 981 982 0.978881 -0.013855 0.081135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 982 983 0.987210 0.006621 0.110698 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 983 984 -1.005460 0.021479 3.074376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 984 985 0.972659 -0.004309 0.094550 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 985 986 0.975939 -0.000117 -0.435633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 986 987 0.989461 0.011060 0.374267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 987 988 -0.009158 -0.980544 -1.767122 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 988 989 1.009980 -0.023095 0.083144 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 989 990 1.024430 -0.010808 0.159477 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 990 991 1.028330 -0.035048 -0.346632 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 991 992 0.009767 -1.013340 -1.623814 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 992 993 1.048240 0.005430 -0.325967 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 993 994 0.970856 0.025011 0.166329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 994 995 0.982229 -0.009626 0.065985 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 995 996 0.036548 0.999152 1.785003 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 996 997 1.004190 0.028832 -0.191690 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 997 998 0.988454 -0.004041 0.154575 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 998 999 1.045830 -0.040305 0.081344 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 999 1000 -1.008730 0.018329 3.107539 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1000 1001 0.996060 0.013441 0.244441 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1001 1002 1.015270 0.008671 -0.361165 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1002 1003 1.056570 -0.003286 -0.121345 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1003 1004 0.015299 -0.948416 -1.608319 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1004 1005 1.024300 -0.017376 -0.139541 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1005 1006 0.993608 0.013316 -0.092207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1006 1007 1.026200 -0.020910 0.451247 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1007 1008 -0.005112 -0.966305 -1.481168 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1008 1009 1.000840 -0.008642 0.046346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1009 1010 1.012310 -0.008460 0.046514 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1010 1011 1.043980 -0.016093 -0.050862 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1011 1012 -0.015606 1.010210 1.565295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1012 1013 0.995509 -0.004987 0.174949 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1013 1014 1.020270 -0.075242 -0.017193 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1014 1015 0.998617 0.001806 -0.295843 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1015 1016 -0.014452 -0.958189 -1.457160 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1016 1017 0.982184 -0.013000 -0.288849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1017 1018 1.004360 0.019792 0.037058 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1018 1019 0.973472 0.015100 0.174202 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1019 1020 -0.990701 -0.029397 3.026892 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1020 1021 0.996209 -0.010105 0.080867 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1021 1022 0.983504 -0.011672 0.348105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1022 1023 0.959857 -0.007815 0.057176 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1023 1024 -0.959429 0.029045 3.120228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1024 1025 0.990506 -0.033231 0.239811 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1025 1026 1.027880 0.029842 -0.126110 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1026 1027 0.994491 -0.018112 0.280877 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1027 1028 -0.005900 -0.960301 -1.160563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1028 1029 0.982557 -0.035708 0.053963 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1029 1030 0.962889 0.011240 0.160627 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1030 1031 1.007790 0.002768 -0.197368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1031 1032 -0.004540 1.002160 1.560407 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1032 1033 0.999924 0.008348 0.352039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1033 1034 1.004900 0.010037 0.066321 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1034 1035 0.969195 -0.009363 -0.003046 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1035 1036 -0.026784 1.015390 1.988057 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1036 1037 0.975133 0.057348 -0.083316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1037 1038 0.999632 0.009426 -0.039124 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1038 1039 0.987938 -0.002811 -0.006429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1039 1040 -0.022064 1.004440 1.858349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1040 1041 1.007050 0.016724 0.170445 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1041 1042 0.986172 -0.003955 -0.105827 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1042 1043 1.000040 -0.017642 0.122134 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1043 1044 0.977709 -0.022104 -0.087030 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1044 1045 0.980447 -0.024326 0.377088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1045 1046 1.009830 0.035565 -0.424786 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1046 1047 0.992586 -0.002268 0.106068 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1047 1048 0.009983 0.969131 1.819303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1048 1049 1.043130 -0.005630 -0.035983 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1049 1050 1.014870 -0.007756 -0.263975 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1050 1051 1.050030 -0.023386 0.233756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1051 1052 0.004770 -0.989537 -1.719580 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1052 1053 0.970495 0.002081 0.136589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1053 1054 0.996124 0.011043 -0.045592 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1054 1055 0.950608 -0.002890 0.173629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1055 1056 -1.046250 0.007421 3.090911 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1056 1057 0.959592 -0.028664 0.200061 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1057 1058 1.001220 0.018862 -0.098891 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1058 1059 0.997045 -0.025753 0.442181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1059 1060 -0.988929 -0.028543 -2.881695 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1060 1061 0.999090 -0.015268 0.160684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1061 1062 1.000980 -0.020996 -0.189925 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1062 1063 0.969544 0.007409 0.208246 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1063 1064 -0.015952 1.013600 1.473415 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1064 1065 1.002310 0.007504 0.333978 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1065 1066 0.981333 -0.049274 0.155317 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1066 1067 1.007070 -0.013808 0.253116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1067 1068 -1.026630 0.017566 3.118644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1068 1069 0.994086 -0.019100 -0.072872 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1069 1070 0.969184 0.001911 0.020218 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1070 1071 1.020090 -0.015151 -0.094127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1071 1072 -1.049050 -0.044531 3.021337 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1072 1073 0.979133 0.034741 0.243228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1073 1074 1.010110 -0.042504 -0.183471 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1074 1075 1.000510 -0.020815 0.049133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1075 1076 -1.021050 -0.012908 -2.849633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1076 1077 1.042460 -0.001380 -0.249443 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1077 1078 1.000320 0.028654 0.228545 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1078 1079 1.016540 -0.004008 -0.033044 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1079 1080 -1.009220 0.013220 2.917822 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1080 1081 0.998449 -0.015993 -0.248579 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1081 1082 0.987184 0.047099 0.098372 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1082 1083 1.005020 0.001204 0.592561 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1083 1084 0.017132 -0.986391 -1.482090 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1084 1085 0.994636 -0.051423 -0.075413 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1085 1086 1.022660 -0.018906 0.454154 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1086 1087 1.044440 -0.000320 -0.129870 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1087 1088 -0.979173 0.006805 3.057924 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1088 1089 1.015740 0.019419 0.175393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1089 1090 0.984655 0.062479 -0.409305 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1090 1091 0.979540 0.023452 -0.197938 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1091 1092 -0.990541 0.013650 -3.114390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1092 1093 0.974107 0.010266 0.120801 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1093 1094 1.049750 -0.003664 -0.161176 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1094 1095 0.995265 -0.005797 0.055116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1095 1096 -0.996664 -0.013451 -3.103649 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1096 1097 0.928559 -0.008583 -0.010387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1097 1098 0.980898 -0.013911 0.177400 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1098 1099 1.003360 -0.001140 -0.073032 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1099 1100 0.000388 -1.022850 -1.417246 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1100 1101 1.011520 0.062454 -0.061503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1101 1102 1.031280 -0.009827 -0.239583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1102 1103 0.979061 0.006857 0.084480 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1103 1104 -0.002786 -1.048110 -2.403098 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1104 1105 1.032210 0.028150 -0.088827 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1105 1106 0.985504 0.002617 -0.133564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1106 1107 1.008920 0.010137 0.092281 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1107 1108 0.021868 -0.953140 -1.575661 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1108 1109 1.013190 -0.003016 0.232909 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1109 1110 0.963566 -0.009122 -0.129593 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1110 1111 0.980749 0.028007 -0.044509 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1111 1112 -0.046965 0.998927 1.507429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1112 1113 0.998647 0.050206 0.348848 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1113 1114 0.971759 0.016259 -0.009635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1114 1115 0.986426 -0.014056 -0.195576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1115 1116 -0.003133 1.017990 1.875335 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1116 1117 1.005440 0.000506 -0.389371 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1117 1118 0.997818 0.022847 -0.126373 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1118 1119 1.009660 0.007074 -0.177333 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1119 1120 -1.034750 0.041926 2.969598 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1120 1121 0.953584 -0.012311 -0.081832 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1121 1122 0.957693 0.006129 0.306998 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1122 1123 1.021150 0.027101 0.089930 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1123 1124 -0.023512 -1.024950 -1.061972 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1124 1125 0.993878 0.010954 -0.096606 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1125 1126 0.975737 -0.011738 -0.006029 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1126 1127 1.060710 0.013408 0.018325 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1127 1128 1.005250 -0.045761 0.229751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1128 1129 1.001480 0.016120 0.014351 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1129 1130 1.035390 0.000006 -0.001040 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1130 1131 0.995665 -0.004105 0.001252 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1131 1132 -1.003890 -0.004558 -2.906893 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1132 1133 0.987341 0.026664 0.065965 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1133 1134 1.009270 -0.010944 -0.086937 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1134 1135 1.022550 0.027905 0.020936 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1135 1136 -0.937087 -0.026520 -2.675564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1136 1137 0.981041 -0.029142 0.306510 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1137 1138 0.991418 0.002140 -0.062495 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1138 1139 0.985974 0.005203 -0.039167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1139 1140 -1.032110 -0.031052 -3.012900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1140 1141 0.979984 -0.005948 -0.224027 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1141 1142 1.019210 0.024729 -0.059405 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1142 1143 1.016690 -0.005843 -0.238995 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1143 1144 -1.007200 0.005236 2.740305 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1144 1145 0.978286 0.018182 -0.096207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1145 1146 1.042160 -0.009514 -0.052963 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1146 1147 0.994549 -0.035888 0.118190 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1147 1148 -0.001948 0.999185 1.533498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1148 1149 0.980950 -0.044730 -0.360248 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1149 1150 1.020040 0.002597 0.109221 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1150 1151 1.006580 0.039824 -0.146050 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1151 1152 0.023704 1.011730 1.272414 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1152 1153 1.003750 0.032793 0.306899 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1153 1154 0.977671 0.016802 0.115283 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1154 1155 0.978817 -0.013134 -0.002334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1155 1156 -0.022796 0.990207 1.630555 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1156 1157 1.012630 -0.015689 0.310701 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1157 1158 0.948356 0.001158 -0.059449 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1158 1159 0.982034 0.025611 -0.082709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1159 1160 -0.998861 0.019019 2.797271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1160 1161 0.984300 0.020698 0.222329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1161 1162 0.964275 0.008289 0.112256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1162 1163 1.015020 0.014830 0.088430 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1163 1164 0.969237 -0.002624 -0.179846 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1164 1165 1.029730 0.007079 -0.048392 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1165 1166 0.968063 0.015674 0.092708 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1166 1167 1.036980 -0.045606 0.089353 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1167 1168 -1.000230 0.030524 3.067390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1168 1169 0.989771 0.037089 -0.421990 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1169 1170 0.996752 0.032619 -0.154097 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1170 1171 0.952762 -0.042870 0.044817 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1171 1172 -0.983345 -0.004009 3.113634 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1172 1173 0.972854 0.001247 0.355000 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1173 1174 0.986233 -0.005153 -0.368778 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1174 1175 1.015300 -0.035567 -0.321418 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1175 1176 -0.047994 0.966016 1.652692 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1176 1177 1.009500 -0.006545 -0.289577 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1177 1178 1.026880 0.031292 0.240863 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1178 1179 1.032650 0.040008 0.148183 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1179 1180 -0.015463 -1.004370 -1.619187 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1180 1181 0.983435 0.012259 -0.250409 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1181 1182 1.015360 0.011984 -0.005863 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1182 1183 0.969706 0.003147 0.118968 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1183 1184 -0.971977 0.032665 -3.052008 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1184 1185 0.972366 -0.004279 -0.098461 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1185 1186 1.078550 -0.011461 -0.057215 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1186 1187 1.017720 0.016913 0.270582 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1187 1188 0.028667 1.030350 1.777450 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1188 1189 0.998206 0.018375 0.103330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1189 1190 1.036120 -0.001509 -0.296330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1190 1191 0.970824 -0.003530 -0.545359 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1191 1192 0.016365 0.987876 1.392800 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1192 1193 1.035590 -0.007834 -0.188060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1193 1194 1.027600 0.001145 -0.087870 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1194 1195 1.010680 0.015078 -0.117925 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1195 1196 -0.958027 -0.039351 -3.038414 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1196 1197 0.968675 -0.008191 -0.068499 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1197 1198 0.988229 -0.007498 0.305991 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1198 1199 0.964896 -0.048469 -0.011013 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1199 1200 0.026977 -0.993768 -1.559497 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1200 1201 0.985941 0.031106 -0.404906 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1201 1202 1.016970 -0.041060 -0.168393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1202 1203 1.011410 -0.004761 -0.471681 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1203 1204 -0.003859 -1.037160 -1.673247 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1204 1205 0.983339 -0.001543 0.157234 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1205 1206 0.983264 0.023938 0.280168 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1206 1207 0.989014 0.004697 -0.365400 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1207 1208 -0.998725 0.005490 2.730520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1208 1209 1.026190 0.015324 0.036810 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1209 1210 0.980552 -0.022986 0.068969 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1210 1211 1.030910 -0.036836 -0.226208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1211 1212 -0.999003 -0.035617 -3.016220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1212 1213 0.994331 0.013569 -0.055045 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1213 1214 1.042180 -0.002264 0.062723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1214 1215 0.978535 0.012923 -0.065799 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1215 1216 -0.023122 -1.018750 -1.736065 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1216 1217 1.015850 -0.025471 0.223591 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1217 1218 1.017820 -0.024111 0.260734 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1218 1219 1.009340 -0.000786 0.264371 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1219 1220 0.987541 0.041726 -0.083916 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1220 1221 0.981730 -0.003377 -0.088839 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1221 1222 1.012340 0.000163 0.060303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1222 1223 0.981558 -0.037447 -0.122738 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1223 1224 0.019821 1.021450 1.662298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1224 1225 0.985280 -0.015006 -0.144931 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1225 1226 1.008070 -0.035717 -0.051446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1226 1227 0.979316 0.001058 -0.311305 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1227 1228 0.005351 -0.967638 -1.949259 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1228 1229 1.020410 0.036598 0.039271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1229 1230 0.990477 -0.003746 -0.071281 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1230 1231 1.049840 0.009740 -0.167042 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1231 1232 0.006625 0.989405 1.513474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1232 1233 0.989102 0.036173 -0.036475 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1233 1234 0.987472 -0.012203 0.281473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1234 1235 1.007640 0.012787 -0.109473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1235 1236 0.973421 -0.009802 0.211876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1236 1237 0.979765 0.002553 -0.147170 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1237 1238 0.969815 -0.029525 -0.187603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1238 1239 0.967017 0.069294 -0.368083 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1239 1240 -1.004420 -0.011574 -3.049389 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1240 1241 0.976640 0.012665 0.130363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1241 1242 1.021220 0.007657 0.318665 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1242 1243 1.031680 -0.001409 0.477556 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1243 1244 -0.027309 -0.985469 -1.289756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1244 1245 0.984654 -0.009268 0.073357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1245 1246 0.980131 -0.003334 0.184444 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1246 1247 0.991002 0.038779 0.009728 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1247 1248 -0.008224 -1.040790 -1.651743 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1248 1249 0.994066 -0.015269 -0.100639 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1249 1250 1.040460 0.004721 0.124945 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1250 1251 0.984536 0.007309 0.087375 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1251 1252 -1.025620 -0.007512 2.927978 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1252 1253 1.004660 0.005793 0.094334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1253 1254 0.975288 0.022046 0.203278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1254 1255 0.983383 -0.002039 0.035168 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1255 1256 -0.046395 -1.008020 -1.539920 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1256 1257 1.015720 0.056645 0.048048 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1257 1258 1.028860 -0.010680 -0.188435 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1258 1259 0.989889 0.029195 -0.126806 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1259 1260 -0.020717 0.973317 1.901742 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1260 1261 0.992467 -0.024286 0.157139 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1261 1262 1.020700 -0.020656 0.236987 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1262 1263 0.992303 -0.006569 -0.099068 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1263 1264 -1.009850 0.028145 -2.969169 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1264 1265 1.004420 -0.012888 0.191164 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1265 1266 1.010050 0.014716 0.182869 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1266 1267 1.003840 0.015372 -0.026373 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1267 1268 -0.995165 -0.022189 3.140583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1268 1269 1.020650 -0.002387 0.370991 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1269 1270 0.994205 0.003771 0.095629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1270 1271 0.962387 0.002274 -0.307814 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1271 1272 0.001592 -1.013770 -1.632880 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1272 1273 1.042080 -0.014500 -0.397798 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1273 1274 1.002490 -0.033513 0.160752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1274 1275 0.992732 0.024383 -0.166805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1275 1276 -0.011957 0.979233 1.527226 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1276 1277 1.019450 -0.043588 0.085656 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1277 1278 0.987590 0.015465 0.063390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1278 1279 1.011320 -0.010167 -0.262263 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1279 1280 -0.039908 1.005220 1.454951 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1280 1281 0.999202 -0.010630 -0.186304 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1281 1282 1.038760 -0.042884 -0.092504 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1282 1283 1.007110 -0.011486 0.375925 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1283 1284 0.046015 1.003230 1.593588 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1284 1285 1.016980 -0.018742 -0.302895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1285 1286 1.015040 0.024289 0.134263 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1286 1287 1.016340 0.043250 -0.077008 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1287 1288 -0.013501 0.979548 1.734538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1288 1289 1.004890 0.000199 -0.059940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1289 1290 0.986816 -0.021576 0.014865 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1290 1291 1.003660 0.045398 0.283793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1291 1292 -0.993987 -0.012146 -3.078733 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1292 1293 1.016470 -0.017505 0.269987 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1293 1294 0.990284 0.009376 -0.061096 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1294 1295 0.991857 -0.006269 -0.025965 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1295 1296 -0.009398 -0.998936 -1.478611 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1296 1297 1.005200 0.000518 -0.062473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1297 1298 1.022820 -0.013248 0.073824 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1298 1299 1.004450 0.014522 0.129333 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1299 1300 0.012775 -0.984144 -1.656053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1300 1301 0.947576 0.033246 -0.240919 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1301 1302 1.005260 -0.004915 -0.103895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1302 1303 1.002280 0.001625 -0.082727 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1303 1304 -1.016980 -0.013945 -2.988846 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1304 1305 0.988037 0.003341 0.021931 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1305 1306 0.984677 0.041441 0.235264 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1306 1307 0.990384 0.019010 -0.168503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1307 1308 -0.013104 -0.966744 -1.515677 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1308 1309 0.996919 0.069674 0.176625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1309 1310 0.987620 0.028901 0.159139 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1310 1311 1.029420 0.063164 -0.167840 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1311 1312 0.035108 -1.009310 -1.517843 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1312 1313 0.978029 -0.025188 -0.137348 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1313 1314 0.984232 -0.018086 0.026971 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1314 1315 0.989851 -0.015855 -0.031907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1315 1316 -0.979803 0.042985 -2.952078 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1316 1317 0.993803 0.010301 -0.369832 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1317 1318 1.026180 0.023539 0.014118 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1318 1319 0.981608 0.035528 0.011346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1319 1320 -0.049731 1.038100 1.362538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1320 1321 0.993476 -0.009581 0.019248 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1321 1322 1.041100 0.009389 -0.095275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1322 1323 1.041830 0.017488 0.243022 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1323 1324 -1.009500 0.003852 2.791279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1324 1325 0.951241 -0.002176 0.027856 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1325 1326 1.021790 -0.008877 0.127010 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1326 1327 1.019340 0.003809 -0.439980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1327 1328 0.030688 -0.967907 -1.410623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1328 1329 0.993261 -0.033701 0.255428 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1329 1330 1.001980 -0.006573 0.142445 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1330 1331 1.040300 -0.015143 0.339255 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1331 1332 0.004438 0.995642 1.685578 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1332 1333 0.991863 -0.005123 -0.387941 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1333 1334 1.003480 0.013770 -0.199598 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1334 1335 0.987106 -0.043817 0.261655 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1335 1336 0.001073 1.023410 1.542735 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1336 1337 1.030270 -0.027570 -0.213965 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1337 1338 0.987853 -0.034408 -0.454562 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1338 1339 1.021640 0.051948 -0.218088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1339 1340 -1.017270 -0.022497 3.000127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1340 1341 1.006230 -0.000612 0.118731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1341 1342 1.033440 0.022271 0.087549 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1342 1343 0.975484 -0.014927 0.348059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1343 1344 -0.020677 1.001400 1.785701 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1344 1345 0.995794 0.015589 -0.138334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1345 1346 0.986945 -0.012995 0.025122 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1346 1347 1.009590 -0.021559 0.050201 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1347 1348 -0.043411 0.998147 1.676093 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1348 1349 1.016420 -0.013346 -0.250762 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1349 1350 1.048500 -0.009541 -0.165429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1350 1351 0.995383 0.015271 -0.035848 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1351 1352 0.000561 -1.003560 -1.589458 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1352 1353 0.983075 -0.003561 0.347366 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1353 1354 0.990409 0.003042 0.085035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1354 1355 1.003190 -0.017097 -0.149251 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1355 1356 0.004829 0.948759 1.574469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1356 1357 1.008050 0.006103 -0.012784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1357 1358 0.968066 0.021545 -0.095839 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1358 1359 0.990584 0.000987 0.295231 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1359 1360 -0.013480 1.028940 1.960830 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1360 1361 1.021880 0.016726 0.055853 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1361 1362 0.963309 -0.027615 0.069050 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1362 1363 1.000570 -0.007291 -0.223817 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1363 1364 -0.008771 -1.011080 -1.352281 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1364 1365 0.948623 -0.012780 -0.202398 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1365 1366 0.990198 0.009585 0.049586 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1366 1367 1.024630 -0.006676 0.050574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1367 1368 -0.026658 -1.018060 -1.564939 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1368 1369 0.994776 0.030519 0.079394 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1369 1370 1.014880 0.001783 0.014455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1370 1371 0.963000 0.008112 0.422205 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1371 1372 0.060241 -1.018220 -1.284291 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1372 1373 0.996363 0.021727 0.413751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1373 1374 0.991617 -0.006790 0.177855 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1374 1375 1.018230 0.008975 0.014692 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1375 1376 0.037441 -1.039630 -1.542996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1376 1377 1.030330 -0.021877 0.021123 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1377 1378 0.955760 -0.034781 0.196268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1378 1379 1.015960 0.033090 -0.012856 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1379 1380 -0.983075 -0.004110 -3.089076 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1380 1381 0.964802 -0.027792 -0.191560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1381 1382 0.985202 -0.011182 0.187347 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1382 1383 1.031990 -0.027124 -0.087204 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1383 1384 -0.038687 -1.010140 -1.843446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1384 1385 1.002320 -0.036154 -0.044882 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1385 1386 0.971960 0.025033 -0.063197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1386 1387 1.004410 0.004381 -0.175492 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1387 1388 -0.003192 1.001370 1.458871 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1388 1389 0.989516 0.008064 0.120785 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1389 1390 1.010210 0.016654 -0.046445 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1390 1391 0.957199 -0.009505 -0.314515 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1391 1392 -1.009980 -0.021543 -2.812793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1392 1393 1.029710 -0.012046 0.056390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1393 1394 0.957875 0.003427 0.154868 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1394 1395 0.973835 0.018489 -0.237613 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1395 1396 -0.955031 -0.046422 3.114283 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1396 1397 1.007290 -0.022493 0.269054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1397 1398 0.980453 0.004656 -0.257653 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1398 1399 1.021970 0.011511 0.082091 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1399 1400 -1.003870 0.017078 -2.824310 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1400 1401 0.948228 0.002977 -0.106369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1401 1402 0.974946 0.012592 -0.417473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1402 1403 1.000800 -0.012112 -0.372018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1403 1404 -0.953617 0.049332 3.114799 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1404 1405 0.988796 0.011720 0.315385 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1405 1406 0.981351 0.017804 -0.051764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1406 1407 1.017770 0.020025 -0.047982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1407 1408 -0.004520 0.971860 1.239387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1408 1409 1.015650 0.011144 0.039792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1409 1410 1.028500 -0.032952 0.013986 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1410 1411 1.053390 -0.040917 0.114198 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1411 1412 -0.993406 -0.035584 2.979718 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1412 1413 1.028680 -0.020486 0.298025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1413 1414 1.001860 0.023520 -0.028762 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1414 1415 0.978709 -0.010311 -0.352605 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1415 1416 -0.013838 -0.983870 -1.641533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1416 1417 0.982531 -0.021927 0.262763 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1417 1418 1.010330 -0.030124 0.089732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1418 1419 0.982628 -0.026409 -0.358293 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1419 1420 -0.984445 0.002955 -2.745675 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1420 1421 0.994456 0.009118 0.131103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1421 1422 0.977550 0.005274 0.037558 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1422 1423 1.003200 0.001364 -0.025086 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1423 1424 -0.003861 1.000770 1.491534 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1424 1425 1.001040 0.024354 -0.029497 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1425 1426 0.981584 -0.002349 -0.222130 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1426 1427 0.982537 0.021726 0.225135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1427 1428 0.008449 -0.997785 -1.591716 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1428 1429 0.958707 0.002946 0.218142 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1429 1430 0.985873 -0.017291 0.144964 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1430 1431 1.030660 -0.004323 0.009717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1431 1432 0.004899 -1.027260 -1.513504 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1432 1433 0.981493 0.025855 -0.196359 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1433 1434 1.020370 0.014109 0.102705 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1434 1435 1.014970 -0.025306 0.308541 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1435 1436 0.004535 -0.944284 -1.334138 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1436 1437 1.018360 -0.000042 0.101447 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1437 1438 0.977533 -0.020501 0.072950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1438 1439 0.964881 -0.006273 -0.025825 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1439 1440 -0.027530 1.012300 1.382527 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1440 1441 1.017090 0.004831 -0.137073 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1441 1442 0.996494 -0.014058 -0.159303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1442 1443 0.961168 0.004693 -0.132350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1443 1444 0.047287 1.018750 1.784315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1444 1445 0.979826 0.008648 0.206844 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1445 1446 1.010490 -0.012515 0.289658 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1446 1447 0.994038 -0.000773 0.153985 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1447 1448 -0.963683 0.008799 -2.903759 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1448 1449 1.002140 -0.013533 -0.408982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1449 1450 1.019880 0.028866 0.036723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1450 1451 1.024390 -0.011836 -0.518496 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1451 1452 1.007350 0.016495 0.215569 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1452 1453 1.017450 0.001773 -0.166471 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1453 1454 0.981097 -0.003543 0.200804 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1454 1455 1.006880 0.009365 0.202035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1455 1456 -0.041951 -1.033590 -1.668576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1456 1457 1.002390 -0.007469 -0.178165 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1457 1458 0.964275 0.012199 -0.078029 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1458 1459 0.983656 -0.011549 -0.028295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1459 1460 -0.013641 0.984744 1.651655 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1460 1461 1.036060 -0.011236 -0.107389 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1461 1462 1.023570 -0.013343 0.088065 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1462 1463 1.022040 -0.018247 -0.245053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1463 1464 -0.013518 -1.007350 -1.603081 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1464 1465 0.990615 0.026093 -0.072086 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1465 1466 0.967833 0.012969 -0.032229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1466 1467 0.991001 0.022606 -0.117857 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1467 1468 -0.033745 0.932076 1.454636 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1468 1469 0.998997 -0.046303 0.021919 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1469 1470 1.003720 0.038006 0.062646 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1470 1471 0.984733 -0.015195 0.100227 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1471 1472 0.009358 -0.962591 -1.624690 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1472 1473 1.007760 0.001143 0.165423 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1473 1474 0.977403 -0.028016 0.099899 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1474 1475 1.000680 0.016118 -0.075534 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1475 1476 -0.970623 -0.002361 3.051336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1476 1477 1.005020 0.018907 0.100017 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1477 1478 1.016940 0.007230 -0.442104 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1478 1479 0.999150 0.011673 0.217347 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1479 1480 -0.013306 1.014900 1.741167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1480 1481 0.992240 0.008823 -0.263508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1481 1482 0.982501 -0.025613 0.357902 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1482 1483 1.008360 -0.008256 0.003622 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1483 1484 0.004330 0.992158 1.799209 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1484 1485 0.982446 -0.035905 0.603300 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1485 1486 0.992756 -0.005239 -0.149925 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1486 1487 1.035630 -0.035510 0.038424 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1487 1488 -0.008640 -1.023380 -2.134715 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1488 1489 1.023660 -0.000898 -0.217583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1489 1490 0.979739 0.003116 -0.101020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1490 1491 0.974236 -0.002847 0.167367 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1491 1492 -0.044432 1.008460 1.431291 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1492 1493 1.029640 -0.005769 0.034319 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1493 1494 0.998992 -0.032157 -0.055683 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1494 1495 1.004070 0.019825 0.119928 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1495 1496 0.034683 -0.990539 -1.040296 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1496 1497 0.980598 -0.020439 -0.023391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1497 1498 0.986133 0.023707 -0.184498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1498 1499 1.024420 -0.001179 -0.116791 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1499 1500 -0.038681 0.950642 1.462104 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1500 1501 1.017110 0.012400 -0.165265 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1501 1502 0.968953 0.021069 0.425120 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1502 1503 0.981147 -0.032644 0.039949 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1503 1504 -0.989920 -0.008611 -3.135179 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1504 1505 0.991806 0.010290 -0.140053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1505 1506 1.038220 0.020040 -0.159014 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1506 1507 1.044840 -0.030657 0.181548 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1507 1508 -0.978349 0.002028 -3.111143 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1508 1509 0.983788 0.002684 0.209512 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1509 1510 1.017460 -0.007410 0.015612 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1510 1511 1.029410 -0.008501 -0.112697 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1511 1512 -1.004180 0.035673 -2.815675 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1512 1513 1.003390 0.011339 0.025104 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1513 1514 1.005440 -0.017906 0.557829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1514 1515 1.004160 -0.015867 0.224673 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1515 1516 0.021257 0.953973 1.292450 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1516 1517 1.007890 -0.005422 -0.238699 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1517 1518 0.962996 -0.027640 -0.189112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1518 1519 0.990419 0.005229 -0.071897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1519 1520 -1.008730 -0.015866 -3.129664 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1520 1521 1.025330 -0.014998 0.163200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1521 1522 1.074830 0.024874 0.262175 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1522 1523 0.976156 -0.025147 0.008283 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1523 1524 0.001428 -1.005290 -1.494656 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1524 1525 0.984426 0.020850 -0.160538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1525 1526 1.006840 0.013677 0.029794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1526 1527 0.994404 -0.005124 0.052161 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1527 1528 -1.036270 -0.014836 -3.033629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1528 1529 1.015450 -0.001209 0.249702 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1529 1530 1.012050 -0.014517 -0.233915 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1530 1531 1.010820 0.028896 0.111060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1531 1532 -0.974704 0.039935 2.894624 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1532 1533 1.030910 0.029047 0.262414 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1533 1534 1.011560 -0.021934 0.566175 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1534 1535 0.997734 -0.010520 0.191102 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1535 1536 -0.056470 -0.989155 -1.867964 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1536 1537 1.004430 0.009485 -0.010566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1537 1538 1.007080 0.042969 -0.113694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1538 1539 1.012570 -0.011900 -0.279228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1539 1540 -0.985286 -0.014881 2.950734 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1540 1541 1.013600 -0.007309 -0.388368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1541 1542 0.995628 -0.005780 -0.171101 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1542 1543 0.999192 0.016088 0.193539 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1543 1544 -1.017290 -0.014581 -3.084195 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1544 1545 0.982481 0.023421 -0.524260 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1545 1546 0.999431 -0.003669 0.216806 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1546 1547 0.998763 0.045269 0.156326 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1547 1548 -1.001410 0.006477 2.929976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1548 1549 0.992004 0.024739 -0.136259 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1549 1550 1.007840 -0.019298 0.061522 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1550 1551 0.977769 -0.019129 -0.225276 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1551 1552 -0.007233 1.010460 1.349684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1552 1553 0.975610 -0.003070 -0.237469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1553 1554 1.054900 -0.009195 0.115955 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1554 1555 1.001620 -0.003338 -0.033517 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1555 1556 0.047144 -1.021550 -1.683295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1556 1557 0.990535 -0.010480 -0.262212 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1557 1558 0.954123 -0.016601 -0.037354 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1558 1559 0.980786 -0.018831 -0.189547 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1559 1560 -0.980744 -0.003011 -2.860427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1560 1561 0.968235 -0.039988 -0.390267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1561 1562 1.030410 0.021793 -0.144483 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1562 1563 0.996621 0.028377 -0.268615 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1563 1564 -0.011874 -0.967631 -1.296348 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1564 1565 0.996068 -0.020715 0.121184 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1565 1566 1.010030 -0.012148 -0.157386 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1566 1567 1.007930 -0.006101 -0.091374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1567 1568 0.009093 1.061550 1.682170 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1568 1569 1.011250 0.007961 0.039223 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1569 1570 1.006870 -0.055088 -0.519621 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1570 1571 1.037410 0.020233 -0.409197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1571 1572 -0.930369 -0.006105 3.056705 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1572 1573 0.986586 -0.025545 0.246879 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1573 1574 0.986377 0.005645 0.099694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1574 1575 0.999273 -0.000295 -0.189043 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1575 1576 -0.033154 -0.979764 -1.468277 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1576 1577 0.986718 -0.017490 -0.140803 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1577 1578 1.017860 -0.000942 -0.252250 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1578 1579 0.979914 0.010159 0.006647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1579 1580 0.042555 0.995086 1.493976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1580 1581 0.962798 0.023137 -0.064456 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1581 1582 1.010090 0.036780 -0.192221 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1582 1583 1.008460 0.052117 0.003137 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1583 1584 0.006563 0.976710 1.410220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1584 1585 0.977982 -0.024269 -0.199001 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1585 1586 0.968510 0.018876 0.154767 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1586 1587 1.016770 -0.021714 -0.008469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1587 1588 -0.004758 -0.919520 -1.811306 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1588 1589 1.021940 -0.004654 0.263166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1589 1590 1.013410 0.006089 -0.188751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1590 1591 0.996754 0.011503 0.025009 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1591 1592 0.023880 1.016320 1.310638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1592 1593 0.984254 -0.006808 0.410693 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1593 1594 1.010220 -0.014410 0.398455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1594 1595 0.990495 0.022572 0.001848 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1595 1596 -0.042333 0.982218 1.443247 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1596 1597 1.037160 0.019536 0.192752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1597 1598 1.006320 -0.018464 0.070191 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1598 1599 1.056300 -0.009781 0.001401 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1599 1600 -0.029975 1.010220 1.892425 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1600 1601 1.017130 -0.012479 0.231617 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1601 1602 1.011220 0.014216 0.071726 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1602 1603 0.999048 0.013131 0.115912 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1603 1604 0.001569 0.995845 1.556693 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1604 1605 1.056620 0.031770 0.080079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1605 1606 0.974195 -0.001392 0.171336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1606 1607 1.000190 0.055441 0.251623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1607 1608 0.005065 1.008730 1.389327 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1608 1609 1.008850 -0.022244 0.006531 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1609 1610 0.994171 -0.042701 0.133877 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1610 1611 0.981118 0.006502 -0.240367 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1611 1612 -1.012450 -0.000818 3.135698 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1612 1613 1.045370 -0.012226 -0.112871 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1613 1614 1.000740 0.015016 0.000829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1614 1615 0.975453 0.008173 0.031752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1615 1616 0.001770 -1.056990 -1.745447 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1616 1617 1.009850 0.026620 0.169332 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1617 1618 0.984002 0.041809 0.036870 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1618 1619 1.031250 -0.030722 0.309928 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1619 1620 -1.002870 0.008739 3.102686 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1620 1621 1.002130 -0.024203 -0.065449 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1621 1622 0.984657 0.005815 0.036818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1622 1623 0.982604 0.002611 0.089082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1623 1624 0.001631 -1.016900 -1.349885 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1624 1625 0.967685 -0.007268 -0.200212 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1625 1626 1.011680 0.018857 0.128206 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1626 1627 1.010590 -0.007930 -0.267749 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1627 1628 0.002061 -1.011320 -1.422154 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1628 1629 1.018320 0.014726 -0.246050 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1629 1630 0.995268 -0.020465 -0.013957 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1630 1631 1.062170 0.001774 0.343726 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1631 1632 -0.000578 -0.999908 -1.526127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1632 1633 1.015080 -0.010284 -0.139561 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1633 1634 1.017120 -0.007332 -0.113443 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1634 1635 0.956753 0.027898 0.026949 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1635 1636 -0.982103 -0.032313 -3.061612 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1636 1637 0.974985 -0.012898 -0.116136 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1637 1638 0.947081 -0.016836 0.097077 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1638 1639 0.995909 -0.035130 -0.144897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1639 1640 0.003865 0.986515 1.539560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1640 1641 1.000750 -0.034008 0.093666 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1641 1642 0.994679 0.021656 -0.053219 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1642 1643 1.028970 -0.011593 -0.049339 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1643 1644 0.014573 1.050650 1.709469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1644 1645 1.011010 0.024118 -0.061841 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1645 1646 0.970294 -0.027190 -0.398502 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1646 1647 0.979149 0.024695 0.123958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1647 1648 -0.007660 1.018980 1.346835 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1648 1649 0.983018 -0.010593 0.074926 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1649 1650 0.942341 -0.010755 -0.103678 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1650 1651 0.994700 0.033144 0.223967 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1651 1652 0.017902 -0.983445 -1.841771 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1652 1653 0.971490 -0.010593 -0.149313 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1653 1654 1.003090 0.013129 0.085922 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1654 1655 0.991054 0.009127 0.122784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1655 1656 0.016157 0.995881 1.560087 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1656 1657 1.001420 -0.014689 -0.107660 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1657 1658 1.009400 -0.011638 -0.227343 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1658 1659 0.981706 -0.030364 0.027875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1659 1660 1.007910 -0.005825 -0.256111 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1660 1661 0.997556 0.029960 0.453827 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1661 1662 0.961442 -0.010992 -0.067520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1662 1663 1.010460 0.024943 0.163475 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1663 1664 0.014451 -0.985931 -1.482688 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1664 1665 1.022180 -0.007839 0.173829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1665 1666 1.010350 -0.025080 -0.056156 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1666 1667 0.995971 -0.001193 0.332412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1667 1668 -1.022660 0.028506 -3.075395 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1668 1669 1.003590 -0.023924 0.048269 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1669 1670 0.992949 -0.010496 0.025391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1670 1671 0.994560 0.033397 0.239038 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1671 1672 -0.038966 1.008840 1.784053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1672 1673 0.976257 0.008825 -0.005851 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1673 1674 0.995977 -0.017957 -0.340875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1674 1675 0.999946 0.012091 -0.177959 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1675 1676 0.041092 -0.999682 -1.902291 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1676 1677 0.938244 -0.000490 -0.299633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1677 1678 1.051710 0.000698 0.075257 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1678 1679 0.972268 -0.028710 -0.167539 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1679 1680 -0.032146 0.979130 1.897147 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1680 1681 0.992889 0.012387 0.177699 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1681 1682 1.016490 -0.009785 -0.118464 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1682 1683 0.988877 0.009833 -0.010154 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1683 1684 -0.019090 1.016480 1.736565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1684 1685 0.982982 0.004125 0.045847 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1685 1686 1.005920 0.012335 -0.135741 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1686 1687 0.974653 -0.041624 0.320485 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1687 1688 -0.002995 -1.016640 -1.493450 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1688 1689 0.983727 0.046453 0.410380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1689 1690 1.001770 0.024936 0.225207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1690 1691 1.003880 -0.007180 -0.009508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1691 1692 0.003239 0.995087 1.510532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1692 1693 0.982345 -0.019461 -0.219921 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1693 1694 0.988540 0.025591 0.257256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1694 1695 0.992746 0.013168 0.292758 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1695 1696 -0.049447 -1.010340 -1.572233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1696 1697 1.016660 -0.005417 -0.107858 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1697 1698 0.971966 -0.010679 0.059573 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1698 1699 0.987217 -0.001098 -0.136346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1699 1700 1.019480 0.009561 -0.065474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1700 1701 0.995452 -0.046214 -0.043462 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1701 1702 1.021080 0.021119 0.029480 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1702 1703 0.973328 -0.004029 -0.012180 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1703 1704 -0.015273 1.038180 1.767577 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1704 1705 1.015840 -0.055772 -0.157808 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1705 1706 0.992684 0.004210 0.209387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1706 1707 0.993053 -0.004884 0.233134 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1707 1708 -1.018650 0.009103 -3.064367 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1708 1709 0.978209 -0.029946 0.036549 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1709 1710 1.002470 0.042989 -0.226263 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1710 1711 1.005910 0.014421 0.189870 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1711 1712 0.975055 0.005045 -0.007417 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1712 1713 1.010940 -0.022719 -0.112614 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1713 1714 0.982129 0.025079 0.118741 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1714 1715 0.999992 0.018932 0.154620 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1715 1716 -0.024141 -0.947419 -1.142225 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1716 1717 1.022070 -0.013028 0.028329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1717 1718 1.002630 -0.042007 -0.317153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1718 1719 0.995103 -0.011417 -0.309669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1719 1720 -0.986584 -0.003785 2.883808 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1720 1721 0.999703 -0.007211 -0.234737 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1721 1722 1.012740 0.021078 -0.108248 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1722 1723 1.006700 0.003548 0.175604 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1723 1724 -0.023818 -0.983498 -1.680798 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1724 1725 1.025280 -0.015731 -0.198605 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1725 1726 1.039110 0.029595 -0.340722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1726 1727 1.032760 0.016480 0.075727 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1727 1728 -0.041613 -1.004240 -1.518747 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1728 1729 0.977472 -0.007653 -0.067497 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1729 1730 0.985860 -0.013407 -0.033237 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1730 1731 1.011770 -0.036761 -0.125946 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1731 1732 -0.975468 0.003389 -3.021339 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1732 1733 0.980827 0.057743 0.401043 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1733 1734 1.013700 0.038218 0.077736 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1734 1735 0.979555 -0.010377 0.215634 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1735 1736 -0.024428 1.004820 1.654079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1736 1737 1.036660 0.028552 0.030103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1737 1738 1.027510 0.003206 0.421145 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1738 1739 1.014860 -0.009493 -0.300526 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1739 1740 1.002220 -0.002544 -0.119471 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1740 1741 0.964259 -0.002777 0.225563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1741 1742 1.018490 -0.019079 -0.035842 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1742 1743 0.967421 -0.044353 0.080087 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1743 1744 0.009139 -1.000570 -1.668617 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1744 1745 0.999631 -0.028653 0.263938 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1745 1746 0.941822 0.016927 0.178157 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1746 1747 0.973019 0.024674 -0.117131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1747 1748 0.978967 0.048412 -0.055076 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1748 1749 1.003590 0.031981 -0.135048 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1749 1750 0.996025 0.020760 0.154383 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1750 1751 1.011750 0.016698 -0.472233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1751 1752 0.013910 -0.989052 -1.366577 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1752 1753 0.986670 0.035839 -0.324114 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1753 1754 0.975632 -0.021363 -0.427990 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1754 1755 0.993338 0.003556 -0.048380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1755 1756 -0.021775 -0.992473 -1.296596 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1756 1757 0.995517 0.007761 0.381044 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1757 1758 1.023910 0.002119 0.308243 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1758 1759 0.983247 -0.001412 -0.270537 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1759 1760 0.027218 -0.999338 -1.382558 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1760 1761 0.965215 0.008711 -0.117253 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1761 1762 1.018780 0.036461 -0.471671 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1762 1763 0.994133 0.000537 0.223428 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1763 1764 0.001562 0.972227 1.332093 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1764 1765 1.003760 0.038340 -0.252576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1765 1766 0.992540 0.011223 -0.339526 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1766 1767 0.958831 -0.040146 -0.050787 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1767 1768 0.019099 -0.978209 -1.296860 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1768 1769 0.972542 -0.006816 -0.277151 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1769 1770 1.021690 -0.015925 0.390671 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1770 1771 1.018970 -0.032978 -0.103670 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1771 1772 -0.971108 -0.032772 3.120642 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1772 1773 0.992219 -0.010425 0.101693 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1773 1774 1.000360 0.008063 0.302080 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1774 1775 1.030470 -0.005846 -0.151598 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1775 1776 0.056940 -0.999835 -1.219564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1776 1777 0.952765 -0.064164 0.115330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1777 1778 1.022310 0.005763 -0.259410 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1778 1779 1.020890 0.011783 -0.353095 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1779 1780 0.004131 0.963550 1.412370 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1780 1781 0.957538 -0.005472 -0.150239 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1781 1782 0.961750 0.019297 0.424625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1782 1783 0.971965 0.021710 -0.263578 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1783 1784 0.026461 -1.009080 -0.994624 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1784 1785 0.961514 0.026669 -0.528010 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1785 1786 0.969123 -0.022763 -0.026634 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1786 1787 1.054740 -0.001293 -0.097128 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1787 1788 0.002906 1.000750 1.619913 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1788 1789 0.989949 0.030519 0.144018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1789 1790 0.978811 -0.016695 -0.159302 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1790 1791 0.999211 -0.024870 0.060227 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1791 1792 -0.991450 0.013199 2.818539 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1792 1793 1.013680 -0.009616 -0.178233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1793 1794 1.020690 -0.007120 0.321529 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1794 1795 0.988451 -0.041932 0.202625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1795 1796 -0.992272 -0.003356 2.916928 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1796 1797 1.015210 -0.025659 0.277512 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1797 1798 0.993933 0.005799 -0.043338 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1798 1799 0.983679 -0.004110 0.058639 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1799 1800 -0.024830 0.975395 1.663751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1800 1801 1.017240 -0.026203 0.065387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1801 1802 0.989772 0.002603 0.444020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1802 1803 0.979138 0.007411 0.050342 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1803 1804 0.039484 0.994646 1.597748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1804 1805 1.020490 0.007235 -0.055955 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1805 1806 1.013020 0.016633 -0.272228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1806 1807 0.981497 0.008408 0.043311 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1807 1808 0.003716 -0.986025 -1.480528 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1808 1809 1.022180 0.018680 0.109474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1809 1810 1.000740 -0.002997 0.147047 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1810 1811 0.996342 -0.013393 0.204769 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1811 1812 1.019160 0.018552 -0.116453 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1812 1813 1.019160 0.023153 0.104917 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1813 1814 0.969921 0.000494 0.164373 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1814 1815 1.042410 -0.005742 -0.092361 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1815 1816 -0.000942 -0.985089 -1.571233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1816 1817 0.951129 -0.000482 -0.189601 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1817 1818 1.015340 -0.009014 0.049170 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1818 1819 0.994177 0.002920 -0.046085 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1819 1820 -0.993884 0.005270 -2.739094 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1820 1821 1.018980 -0.025608 0.146316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1821 1822 0.954541 0.008470 -0.081579 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1822 1823 1.005450 -0.043286 -0.160430 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1823 1824 -0.004426 -1.027350 -2.106907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1824 1825 0.988441 0.022422 0.036865 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1825 1826 1.002530 0.002413 -0.183570 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1826 1827 1.017720 -0.018900 0.051315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1827 1828 -0.989965 -0.004048 2.849065 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1828 1829 1.008210 0.008764 0.161586 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1829 1830 1.014000 0.027351 -0.141784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1830 1831 1.000500 -0.019047 0.087905 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1831 1832 -0.005468 1.020750 1.411363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1832 1833 1.010900 0.030364 0.022802 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1833 1834 0.977782 0.017499 0.221470 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1834 1835 1.019230 -0.020166 0.023502 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1835 1836 -0.000109 0.965589 1.613076 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1836 1837 1.050560 -0.006795 0.293210 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1837 1838 0.989980 0.013143 -0.073882 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1838 1839 0.967221 -0.001408 0.125711 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1839 1840 -0.031984 0.949347 1.772166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1840 1841 1.020570 0.006895 0.251070 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1841 1842 0.979065 0.021748 0.078318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1842 1843 0.962561 0.030637 0.495637 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1843 1844 1.040730 0.015841 -0.260568 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1844 1845 1.005720 -0.009102 -0.149978 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1845 1846 0.991756 0.008111 0.096637 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1846 1847 0.971311 0.003796 -0.091323 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1847 1848 -0.012302 -0.959548 -1.459573 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1848 1849 0.982197 -0.006409 -0.270950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1849 1850 1.005470 0.012332 0.028406 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1850 1851 0.983517 0.007817 0.169403 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1851 1852 -0.015410 1.008950 1.378266 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1852 1853 0.991010 -0.012132 -0.058195 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1853 1854 1.009470 -0.014518 -0.392079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1854 1855 1.004860 0.008045 -0.145885 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1855 1856 -0.011021 -0.971943 -1.513945 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1856 1857 1.012640 0.010363 0.265987 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1857 1858 0.964445 0.011772 0.299723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1858 1859 0.975092 -0.013957 0.040535 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1859 1860 -0.977867 0.027029 3.001742 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1860 1861 1.041070 0.032575 -0.303862 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1861 1862 0.995103 -0.003864 0.178733 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1862 1863 1.023810 0.031081 0.087780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1863 1864 -0.031269 0.991941 1.488608 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1864 1865 1.010430 -0.022713 -0.064752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1865 1866 0.961861 -0.023818 0.033868 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1866 1867 0.995316 -0.034185 -0.142311 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1867 1868 0.018653 1.052330 1.444429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1868 1869 0.971838 0.001451 -0.038159 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1869 1870 1.006330 -0.013311 -0.113792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1870 1871 1.012280 -0.005593 0.261476 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1871 1872 0.010825 -0.980461 -1.538503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1872 1873 0.975423 0.003730 0.119267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1873 1874 0.961301 -0.034868 0.456083 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1874 1875 1.022180 0.005440 -0.048249 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1875 1876 0.013706 0.987050 1.495947 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1876 1877 1.010360 -0.010268 -0.250630 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1877 1878 1.003260 -0.005112 0.063158 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1878 1879 0.940250 -0.037432 -0.088998 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1879 1880 -0.021389 0.988447 1.433996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1880 1881 1.018190 -0.004343 0.123017 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1881 1882 0.974757 -0.044462 -0.246935 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1882 1883 1.033790 0.011501 -0.520455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1883 1884 -1.048460 -0.034484 -2.830202 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1884 1885 1.013850 0.016637 0.257996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1885 1886 1.009910 0.021475 0.130418 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1886 1887 1.007150 0.016675 -0.350608 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1887 1888 0.009653 0.990512 1.469179 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1888 1889 1.048430 -0.008036 0.453577 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1889 1890 0.972008 -0.063256 0.163750 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1890 1891 1.040260 -0.016407 -0.141646 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1891 1892 0.018421 -1.001720 -1.684593 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1892 1893 1.011270 0.022831 -0.048041 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1893 1894 1.004990 -0.012281 -0.029733 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1894 1895 0.998691 -0.049117 -0.209315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1895 1896 -0.013282 -0.997564 -1.151662 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1896 1897 0.946457 -0.029009 0.219354 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1897 1898 0.994255 0.030064 0.080733 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1898 1899 1.031700 -0.026189 0.259800 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1899 1900 -1.051200 -0.018969 2.971452 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1900 1901 0.968351 0.025239 -0.014441 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1901 1902 1.015020 -0.005390 0.039336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1902 1903 1.027150 -0.025621 -0.230619 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1903 1904 0.013783 -1.001070 -1.415427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1904 1905 0.991977 0.002636 -0.055267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1905 1906 0.970135 -0.014302 0.259618 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1906 1907 1.011100 -0.005782 0.141624 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1907 1908 0.023789 1.065030 1.447238 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1908 1909 0.985849 -0.018981 0.266899 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1909 1910 1.040330 0.039629 0.214176 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1910 1911 0.969525 -0.006903 0.180018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1911 1912 -0.030946 1.001070 1.756791 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1912 1913 1.006680 -0.003869 -0.086623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1913 1914 1.012320 -0.008238 -0.324093 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1914 1915 0.981227 0.003851 0.122193 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1915 1916 -0.015771 1.008660 1.767357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1916 1917 0.995758 -0.005949 0.224644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1917 1918 1.009090 -0.034097 -0.209034 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1918 1919 0.997316 -0.040255 -0.137525 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1919 1920 0.021714 -1.043370 -1.416988 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1920 1921 1.016470 -0.009130 0.159578 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1921 1922 0.997911 0.029678 0.177186 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1922 1923 0.967911 0.034560 -0.209232 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1923 1924 -0.004873 0.991019 1.691093 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1924 1925 1.017470 0.010660 0.070271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1925 1926 0.984683 -0.059702 0.182600 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1926 1927 0.964688 0.016487 -0.478114 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1927 1928 -0.023318 -1.003450 -1.764566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1928 1929 1.026350 -0.033868 0.323388 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1929 1930 1.050260 0.010754 0.196731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1930 1931 0.961452 0.037439 -0.341709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1931 1932 0.019849 -0.997366 -1.748855 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1932 1933 1.019590 0.039934 -0.448505 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1933 1934 1.028170 -0.005051 0.007886 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1934 1935 1.008830 0.067951 0.016105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1935 1936 -0.995970 -0.019982 3.020127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1936 1937 0.987172 0.032078 -0.160384 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1937 1938 1.038230 0.014006 -0.184382 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1938 1939 0.988626 -0.032983 -0.357966 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1939 1940 1.016620 -0.029160 -0.269567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1940 1941 0.982077 0.007146 0.108752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1941 1942 1.001440 0.015425 0.017764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1942 1943 0.983561 0.018599 0.212219 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1943 1944 0.985794 0.027860 0.133082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1944 1945 1.018290 0.006961 0.008520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1945 1946 0.982856 -0.009942 -0.070781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1946 1947 1.000690 0.004596 -0.313003 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1947 1948 -0.003779 1.050160 1.464678 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1948 1949 0.988862 -0.003627 -0.025085 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1949 1950 1.006900 -0.016227 -0.065576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1950 1951 1.007820 -0.010573 -0.345004 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1951 1952 0.025756 -0.987220 -1.742651 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1952 1953 0.949943 -0.004763 -0.188084 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1953 1954 0.998962 -0.028612 0.091328 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1954 1955 1.050640 -0.056139 0.007080 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1955 1956 0.038517 0.963756 1.486229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1956 1957 0.938361 0.026511 -0.212802 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1957 1958 0.997767 0.047833 -0.205033 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1958 1959 0.995216 -0.017076 -0.410634 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1959 1960 0.016038 1.002660 1.873558 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1960 1961 1.051010 0.031449 -0.221647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1961 1962 1.003100 -0.020270 -0.491363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1962 1963 0.978209 -0.031376 -0.079308 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1963 1964 -1.003280 0.019054 -2.972433 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1964 1965 0.978539 0.020003 -0.194897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1965 1966 1.020270 0.033554 -0.043582 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1966 1967 0.990613 -0.005521 0.013500 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1967 1968 -0.014273 -0.964791 -1.579587 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1968 1969 1.005750 -0.016200 0.228404 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1969 1970 1.011880 -0.001514 -0.291818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1970 1971 0.996474 -0.039021 0.244738 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1971 1972 -0.965259 0.022969 -2.886616 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1972 1973 1.021340 -0.025372 0.078464 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1973 1974 0.989554 -0.024904 -0.182213 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1974 1975 0.997283 0.008209 -0.240998 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1975 1976 0.015782 0.997843 1.441208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1976 1977 0.996526 0.027942 -0.130582 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1977 1978 1.022830 -0.015197 0.055674 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1978 1979 1.009760 0.025564 0.191706 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1979 1980 -0.006370 -1.024710 -1.347349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1980 1981 0.981545 0.002806 0.177040 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1981 1982 1.007710 -0.015773 -0.037363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1982 1983 1.002870 -0.015626 -0.011278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1983 1984 0.009221 -1.011090 -1.539657 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1984 1985 1.008430 -0.020619 0.122177 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1985 1986 1.018970 -0.014423 -0.170223 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1986 1987 1.011310 -0.021775 0.116236 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1987 1988 1.025120 0.001791 -0.035105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1988 1989 0.981354 0.016810 -0.375405 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1989 1990 1.016950 -0.014146 -0.147682 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1990 1991 0.971500 -0.024429 0.033105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1991 1992 -1.020570 0.034192 -3.081870 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1992 1993 0.983384 -0.025637 0.109391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1993 1994 0.987247 -0.010559 0.082855 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1994 1995 1.004440 0.028290 0.143755 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1995 1996 -0.035746 1.010730 1.646334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1996 1997 1.010580 0.003843 -0.106504 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1997 1998 0.974704 0.013275 0.184402 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1998 1999 1.030440 0.010950 -0.000187 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1999 2000 0.992546 -0.008772 -0.043576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2000 2001 0.940349 -0.006631 -0.178748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2001 2002 1.068410 0.007070 0.164397 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2002 2003 1.042480 -0.007161 -0.161657 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2003 2004 -1.008860 -0.034767 -2.969573 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2004 2005 1.002220 -0.017538 -0.275872 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2005 2006 1.006190 -0.012172 0.005630 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2006 2007 1.014580 0.022707 -0.122760 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2007 2008 -1.015040 -0.016273 3.105569 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2008 2009 1.023270 0.001952 -0.260318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2009 2010 0.980896 0.040167 -0.117082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2010 2011 1.015000 -0.023482 0.193494 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2011 2012 -0.981526 -0.020971 -2.664883 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2012 2013 0.979661 0.025197 0.176540 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2013 2014 1.008670 -0.000253 -0.354353 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2014 2015 0.955407 0.043069 -0.118395 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2015 2016 -0.010939 -1.035910 -1.436789 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2016 2017 1.019780 0.013410 -0.121030 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2017 2018 1.012700 0.014139 0.423967 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2018 2019 1.015350 0.038756 -0.029182 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2019 2020 0.029734 -0.943963 -1.322295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2020 2021 0.993207 0.006947 0.054954 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2021 2022 0.984309 -0.025611 0.015831 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2022 2023 1.041560 0.022534 0.350675 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2023 2024 0.006455 1.005940 1.347547 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2024 2025 0.976628 0.010311 -0.085320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2025 2026 1.020090 -0.027414 -0.286652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2026 2027 0.976556 0.041592 0.028133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2027 2028 -0.983270 -0.013815 -3.076417 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2028 2029 0.991677 -0.010706 0.100632 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2029 2030 1.027050 -0.001327 0.192031 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2030 2031 1.021830 -0.058484 -0.258846 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2031 2032 0.012301 -0.995639 -1.308850 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2032 2033 1.022900 -0.000438 -0.397893 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2033 2034 0.989433 0.008658 0.072936 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2034 2035 0.995488 0.048026 -0.392394 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2035 2036 0.019810 1.008250 1.720162 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2036 2037 1.029170 -0.021064 0.046035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2037 2038 0.999018 -0.020791 0.185146 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2038 2039 0.981758 -0.006766 0.161242 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2039 2040 -1.015480 0.026702 -2.847483 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2040 2041 0.980184 0.023174 -0.123329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2041 2042 0.971523 0.009725 -0.281142 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2042 2043 1.007400 -0.026936 -0.442115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2043 2044 0.003289 -1.012940 -1.148452 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2044 2045 1.035850 0.039481 0.239723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2045 2046 0.933262 -0.034209 0.136000 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2046 2047 0.998488 0.001196 0.092565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2047 2048 -1.009510 0.010345 2.989684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2048 2049 1.014390 0.016811 0.000862 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2049 2050 1.005950 0.010759 -0.057518 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2050 2051 0.957288 -0.005589 0.084063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2051 2052 -0.006942 -0.985960 -1.714910 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2052 2053 1.016960 0.007085 0.486383 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2053 2054 0.964718 -0.066712 -0.222829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2054 2055 0.984921 -0.051171 0.154197 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2055 2056 -0.995745 -0.006755 -2.955629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2056 2057 0.970021 -0.000399 -0.067565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2057 2058 0.994185 -0.011705 0.586327 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2058 2059 1.024610 0.010342 -0.089711 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2059 2060 -1.000170 0.006062 3.091852 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2060 2061 1.016150 0.027556 0.481930 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2061 2062 0.956526 -0.014530 0.317075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2062 2063 0.998966 0.036014 -0.263523 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2063 2064 -0.013323 -0.963384 -1.360896 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2064 2065 1.033180 -0.013257 0.037566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2065 2066 1.035430 -0.021410 0.125253 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2066 2067 1.007860 0.044759 -0.074392 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2067 2068 -1.006030 0.010557 2.881884 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2068 2069 0.970979 0.000906 -0.255404 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2069 2070 0.950895 0.003764 -0.231610 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2070 2071 1.036230 -0.008108 -0.075245 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2071 2072 -0.003183 -1.009030 -1.803528 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2072 2073 1.014710 0.055274 0.210721 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2073 2074 0.984666 0.014239 0.030475 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2074 2075 1.025220 -0.013537 0.056765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2075 2076 -0.012463 0.962999 1.349824 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2076 2077 0.977926 0.018541 0.163185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2077 2078 0.983376 0.016227 -0.502902 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2078 2079 0.997229 -0.035087 -0.110690 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2079 2080 -0.004786 1.032550 1.687141 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2080 2081 1.035130 -0.035468 0.431785 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2081 2082 1.016980 -0.007531 -0.077331 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2082 2083 0.990182 -0.007600 0.130906 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2083 2084 -0.946981 0.034552 -2.653682 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2084 2085 0.964024 0.001002 -0.172604 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2085 2086 1.025500 0.040295 -0.189856 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2086 2087 0.994768 0.044702 0.519279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2087 2088 -0.981345 0.030133 -3.128669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2088 2089 1.041920 -0.003790 0.246165 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2089 2090 1.003090 0.033491 0.056150 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2090 2091 1.018950 0.003695 0.119685 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2091 2092 -1.040340 -0.003694 -3.029350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2092 2093 0.969072 0.008459 0.066946 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2093 2094 0.987400 -0.007668 -0.557790 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2094 2095 1.061040 -0.021750 -0.120723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2095 2096 -0.003128 -1.028580 -1.652240 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2096 2097 0.973587 -0.001105 -0.157184 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2097 2098 1.024040 -0.001938 -0.150337 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2098 2099 1.055990 -0.011525 -0.112042 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2099 2100 1.006990 0.015479 -0.225128 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2100 2101 1.004150 0.003161 -0.006310 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2101 2102 1.017630 -0.020906 -0.161656 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2102 2103 0.982532 -0.021809 -0.163888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2103 2104 -0.950207 -0.026689 2.799227 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2104 2105 1.022850 0.002802 0.192164 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2105 2106 1.031800 0.011439 -0.373818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2106 2107 0.980936 -0.012748 0.142469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2107 2108 -1.022080 0.011772 3.104959 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2108 2109 1.013460 -0.022376 0.141177 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2109 2110 1.007240 0.012771 -0.131005 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2110 2111 1.005640 -0.011419 0.117081 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2111 2112 0.058612 1.000730 1.734638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2112 2113 1.017260 0.023561 0.049479 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2113 2114 1.012340 -0.041780 -0.015065 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2114 2115 1.017850 -0.031193 0.029726 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2115 2116 -0.991812 -0.008336 3.123164 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2116 2117 1.004990 0.009494 0.010459 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2117 2118 1.015760 0.023248 -0.062905 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2118 2119 0.995019 0.023313 -0.347294 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2119 2120 -0.983702 0.027731 -2.920376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2120 2121 1.016620 0.003319 -0.133359 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2121 2122 0.982226 0.049814 0.017549 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2122 2123 1.030120 0.046748 -0.051564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2123 2124 0.008478 -0.988051 -1.431566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2124 2125 1.016960 -0.006730 0.308263 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2125 2126 1.008270 0.042655 -0.425788 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2126 2127 1.012220 -0.034362 0.024100 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2127 2128 0.007788 -0.992941 -1.645087 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2128 2129 0.984670 0.012486 -0.271895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2129 2130 1.006180 -0.001679 -0.025683 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2130 2131 1.026490 -0.008122 -0.034025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2131 2132 -1.057580 0.018758 -2.690534 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2132 2133 1.002110 0.027777 -0.299775 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2133 2134 1.019310 -0.031033 0.414051 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2134 2135 1.026900 0.010415 0.047944 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2135 2136 0.992567 0.022586 0.140908 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2136 2137 0.931255 0.034897 -0.077994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2137 2138 1.047420 0.035540 -0.191467 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2138 2139 1.023330 0.006495 0.132209 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2139 2140 -0.002248 0.965347 1.738699 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2140 2141 1.032770 -0.032427 0.171025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2141 2142 1.052630 -0.011374 -0.052694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2142 2143 1.020480 -0.002243 0.173410 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2143 2144 0.012968 1.013620 1.501126 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2144 2145 0.998701 -0.067667 0.245934 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2145 2146 1.007400 0.007602 0.024876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2146 2147 0.974539 0.011677 -0.034424 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2147 2148 -0.978161 -0.000110 2.770185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2148 2149 1.024440 0.041808 0.433676 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2149 2150 0.995337 -0.020572 0.136977 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2150 2151 1.046590 0.005483 -0.061094 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2151 2152 0.051857 -0.985933 -1.554285 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2152 2153 1.008630 -0.003048 -0.044703 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2153 2154 1.024160 0.025559 -0.097740 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2154 2155 1.018630 -0.025712 -0.144715 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2155 2156 -0.022736 0.993529 1.253939 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2156 2157 0.976283 -0.012937 0.282212 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2157 2158 0.988690 0.013286 0.184172 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2158 2159 0.991272 -0.002147 -0.050098 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2159 2160 -1.056390 0.003847 3.057272 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2160 2161 1.008260 0.029409 0.221323 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2161 2162 0.993312 -0.014657 -0.357906 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2162 2163 0.977908 0.006529 -0.576231 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2163 2164 0.014501 -0.999212 -1.725327 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2164 2165 1.015860 -0.040281 -0.158536 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2165 2166 1.036540 0.007358 0.272728 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2166 2167 0.994539 -0.020159 -0.143308 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2167 2168 -1.030390 0.055379 2.778930 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2168 2169 1.045960 -0.013315 -0.346081 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2169 2170 1.002250 0.049392 -0.057322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2170 2171 1.019940 -0.029276 0.115346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2171 2172 0.029003 1.013020 1.023336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2172 2173 1.017150 -0.008493 -0.105864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2173 2174 0.976563 -0.027183 -0.190948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2174 2175 0.973208 0.002472 0.255341 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2175 2176 0.027730 1.030520 1.972496 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2176 2177 1.021900 -0.022407 0.094654 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2177 2178 0.995381 -0.005357 0.121183 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2178 2179 0.954311 0.026648 -0.131599 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2179 2180 0.011691 0.990744 1.287029 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2180 2181 1.026410 -0.017147 -0.028976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2181 2182 1.003870 0.025273 0.175156 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2182 2183 0.988373 0.027187 0.439996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2183 2184 -1.065960 -0.007610 -3.111241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2184 2185 0.986316 0.021172 -0.161067 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2185 2186 1.006180 -0.002144 -0.215944 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2186 2187 0.968373 -0.004526 -0.218231 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2187 2188 -0.012895 0.966498 1.715570 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2188 2189 1.045970 -0.019880 0.048634 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2189 2190 0.981791 0.004861 0.036638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2190 2191 0.995094 -0.012878 0.122291 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2191 2192 -0.961440 -0.021106 3.131267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2192 2193 1.017660 -0.011176 -0.154845 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2193 2194 0.979011 0.009949 0.073290 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2194 2195 1.011600 0.003916 0.133107 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2195 2196 -0.980475 -0.001557 2.940506 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2196 2197 0.987952 0.004940 -0.475859 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2197 2198 1.018400 -0.044567 -0.161506 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2198 2199 0.953963 -0.005405 -0.211538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2199 2200 -0.021835 1.003330 1.424775 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2200 2201 1.001820 0.025053 0.197853 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2201 2202 1.016470 -0.014723 -0.167765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2202 2203 0.995059 0.002279 0.004373 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2203 2204 -1.068180 0.016054 -2.579829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2204 2205 1.020460 -0.007700 0.024126 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2205 2206 1.026970 -0.025728 -0.201743 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2206 2207 1.032480 0.006334 0.039985 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2207 2208 -0.985657 0.016897 2.835775 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2208 2209 0.972282 -0.080343 -0.142892 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2209 2210 0.993269 0.009632 -0.105417 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2210 2211 0.977844 -0.005234 -0.001487 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2211 2212 -0.006269 -1.021860 -1.298207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2212 2213 1.010870 -0.000066 0.035537 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2213 2214 0.987253 0.032775 -0.041085 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2214 2215 1.030050 -0.004232 0.057059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2215 2216 -0.031279 -1.038320 -1.797438 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2216 2217 0.993079 0.002678 0.108190 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2217 2218 0.999888 -0.006301 -0.120250 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2218 2219 0.985152 -0.027120 0.124749 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2219 2220 -0.998153 0.003666 3.061861 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2220 2221 0.981120 0.001252 -0.202741 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2221 2222 0.996330 -0.015277 -0.378754 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2222 2223 1.016820 0.021726 0.037409 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2223 2224 -0.984286 0.002749 -3.096225 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2224 2225 0.957765 0.015462 -0.057228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2225 2226 0.950216 0.000793 -0.263392 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2226 2227 0.944046 -0.027346 -0.209106 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2227 2228 0.963573 -0.006808 0.220118 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2228 2229 0.983392 -0.030768 -0.142081 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2229 2230 0.966226 -0.013836 0.011856 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2230 2231 1.000010 0.057204 -0.020412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2231 2232 -0.053522 1.005850 1.778378 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2232 2233 1.012220 0.007449 -0.090130 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2233 2234 0.964490 0.004039 0.021175 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2234 2235 1.009480 -0.004538 0.357133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2235 2236 -0.006583 0.985962 1.707923 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2236 2237 0.989409 0.020924 0.457715 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2237 2238 1.022260 0.006167 0.056567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2238 2239 1.006370 -0.005268 0.175489 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2239 2240 -1.016410 -0.007176 -2.910580 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2240 2241 1.002780 -0.022590 0.212469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2241 2242 0.997150 0.011086 -0.179533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2242 2243 1.016960 -0.016305 -0.364588 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2243 2244 -0.010517 1.044900 1.659577 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2244 2245 1.032580 0.006974 -0.189556 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2245 2246 1.013140 0.009063 0.421154 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2246 2247 0.992523 -0.003939 0.121497 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2247 2248 -0.025044 0.989119 1.718356 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2248 2249 0.987705 0.008050 0.087797 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2249 2250 0.991544 -0.004244 0.255297 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2250 2251 0.975652 -0.008011 -0.036241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2251 2252 -0.972110 0.018963 -3.121685 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2252 2253 0.962608 -0.006922 0.059648 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2253 2254 1.031540 -0.023092 0.206756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2254 2255 0.987742 -0.011404 0.087147 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2255 2256 -0.006935 1.003850 1.514032 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2256 2257 0.977098 -0.019237 0.189973 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2257 2258 1.011330 -0.024876 0.023067 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2258 2259 1.004850 0.005802 -0.187046 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2259 2260 -0.961796 0.023777 -3.125192 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2260 2261 0.994603 0.018297 -0.255240 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2261 2262 1.018200 -0.016423 -0.169345 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2262 2263 0.991634 -0.001844 0.146398 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2263 2264 -1.011840 -0.001321 2.772205 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2264 2265 0.959858 -0.013361 -0.305105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2265 2266 0.986769 -0.028462 0.270242 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2266 2267 0.982912 0.004495 0.111580 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2267 2268 -0.013295 1.030650 1.667499 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2268 2269 0.995845 0.008441 0.196519 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2269 2270 0.979924 -0.003410 0.235129 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2270 2271 1.004120 -0.004020 0.054361 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2271 2272 -0.999240 0.017395 -2.976950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2272 2273 0.964108 -0.015085 -0.162640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2273 2274 0.992096 -0.009540 -0.058009 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2274 2275 1.003220 0.009042 -0.060756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2275 2276 -1.038190 -0.022834 3.001544 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2276 2277 1.006270 -0.022595 -0.084005 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2277 2278 0.976669 0.022939 0.130793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2278 2279 0.955196 0.017362 -0.041366 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2279 2280 -0.010561 0.981420 1.591918 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2280 2281 0.982852 0.001389 -0.113463 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2281 2282 0.964336 0.029264 0.004795 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2282 2283 0.999332 0.004466 0.188432 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2283 2284 -0.005578 -0.976189 -1.902162 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2284 2285 1.031070 0.017047 0.265874 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2285 2286 1.030310 -0.005456 0.160922 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2286 2287 1.011550 -0.000339 0.296805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2287 2288 -1.016860 0.010566 -3.006025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2288 2289 0.987727 -0.003115 -0.237054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2289 2290 1.027920 -0.018056 -0.014010 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2290 2291 0.965365 0.002939 -0.041443 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2291 2292 -0.016747 -0.989387 -1.862403 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2292 2293 0.994747 -0.020278 0.326779 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2293 2294 1.021230 0.002379 -0.065890 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2294 2295 1.002380 0.010172 -0.042910 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2295 2296 -0.991975 0.014164 -2.946397 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2296 2297 0.976138 -0.029314 -0.028131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2297 2298 1.011000 0.017132 -0.008336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2298 2299 0.980741 0.029773 0.075883 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2299 2300 -0.030608 0.971841 1.714205 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2300 2301 1.016330 -0.005065 0.142346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2301 2302 0.985007 0.031850 0.086237 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2302 2303 1.031060 -0.007925 0.148138 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2303 2304 -1.000010 0.019526 3.078045 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2304 2305 1.016930 -0.016914 -0.361249 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2305 2306 1.066740 0.014414 0.380415 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2306 2307 0.994896 -0.011799 -0.101169 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2307 2308 -0.003200 -1.000080 -1.245154 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2308 2309 1.039640 0.020246 0.285461 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2309 2310 0.967497 0.006823 0.177051 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2310 2311 0.998602 0.008010 0.140231 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2311 2312 -0.988340 0.010259 3.128119 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2312 2313 1.042150 -0.023355 -0.017626 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2313 2314 1.020660 -0.023487 -0.100069 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2314 2315 1.023930 0.009704 -0.114899 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2315 2316 0.005053 -0.983068 -1.681706 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2316 2317 0.983196 -0.030728 0.320330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2317 2318 1.002940 -0.008084 -0.386224 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2318 2319 1.008540 0.005913 -0.043074 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2319 2320 -0.971417 0.043746 3.101901 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2320 2321 0.962768 -0.027305 0.161927 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2321 2322 1.021310 0.014178 0.061621 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2322 2323 0.988555 0.013838 -0.192319 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2323 2324 0.029702 0.954811 1.811506 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2324 2325 0.996893 0.013360 0.306146 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2325 2326 0.977270 -0.000047 0.055391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2326 2327 0.996274 0.009713 0.013904 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2327 2328 -0.982517 0.007093 -3.041590 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2328 2329 0.996778 -0.012108 -0.104118 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2329 2330 1.031550 -0.041662 0.277501 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2330 2331 1.018110 0.022392 -0.361185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2331 2332 0.017954 -0.973302 -1.265843 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2332 2333 1.003410 0.013334 -0.134758 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2333 2334 0.999358 0.006391 -0.153228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2334 2335 1.019420 -0.013998 0.019522 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2335 2336 -0.990891 -0.006389 -3.072834 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2336 2337 1.047360 0.002590 0.098455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2337 2338 0.993049 0.022603 0.137722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2338 2339 0.984999 0.048005 -0.247314 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2339 2340 -1.004790 0.022038 -3.037526 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2340 2341 0.967929 0.043233 0.152057 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2341 2342 0.979235 -0.013958 0.108749 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2342 2343 1.010190 0.031600 -0.005166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2343 2344 -0.001013 0.944312 1.627327 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2344 2345 1.011730 0.003711 -0.128169 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2345 2346 0.998024 -0.006683 0.014258 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2346 2347 0.965061 0.033705 0.173266 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2347 2348 -0.009474 0.999228 1.273021 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2348 2349 1.029660 0.022253 -0.058814 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2349 2350 1.026370 -0.011676 -0.288675 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2350 2351 1.010560 -0.001785 0.264620 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2351 2352 0.009611 -1.001350 -1.458793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2352 2353 1.003510 -0.033352 0.246521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2353 2354 0.997950 0.009486 -0.069178 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2354 2355 0.979623 -0.024012 0.167458 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2355 2356 -1.014560 -0.040374 -3.065031 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2356 2357 1.005210 0.004018 0.211152 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2357 2358 0.985050 0.002348 0.076256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2358 2359 0.986437 -0.005395 0.093168 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2359 2360 -0.008750 1.007640 1.218555 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2360 2361 0.998504 -0.049802 0.266448 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2361 2362 1.028520 0.013809 -0.077529 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2362 2363 1.010870 0.016496 -0.140872 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2363 2364 -0.009091 -1.027940 -1.881978 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2364 2365 0.999059 0.012991 0.019962 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2365 2366 0.999890 -0.001832 0.034472 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2366 2367 0.981051 0.009825 0.013038 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2367 2368 -0.959298 -0.003647 -3.010576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2368 2369 1.025310 0.000230 -0.080818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2369 2370 0.984088 -0.027691 0.224832 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2370 2371 1.013320 0.028717 -0.069204 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2371 2372 -0.001713 1.020900 1.322181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2372 2373 0.987343 0.040021 -0.078282 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2373 2374 1.016220 -0.011545 -0.165194 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2374 2375 0.953847 0.003734 0.050252 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2375 2376 -0.041983 0.996880 1.698138 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2376 2377 1.001640 0.018179 -0.247500 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2377 2378 1.019150 -0.007505 0.054032 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2378 2379 1.010050 -0.008334 -0.108816 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2379 2380 -0.019521 0.995057 1.740657 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2380 2381 1.019800 -0.017413 -0.146153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2381 2382 0.991347 -0.009080 0.004203 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2382 2383 0.990245 -0.030988 -0.096460 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2383 2384 0.006249 1.035310 1.478318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2384 2385 1.018240 0.015360 0.123355 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2385 2386 0.995664 0.007354 -0.164576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2386 2387 1.010370 -0.002864 -0.277959 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2387 2388 0.042008 0.978582 1.382413 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2388 2389 1.027140 0.001589 -0.266533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2389 2390 1.029630 -0.029960 0.096162 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2390 2391 0.993139 -0.014918 0.048060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2391 2392 -0.005049 -0.999678 -1.427289 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2392 2393 1.024990 0.043257 -0.122389 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2393 2394 1.035110 -0.011643 0.074954 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2394 2395 1.003780 0.000291 -0.119376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2395 2396 -0.991713 0.005119 -3.070355 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2396 2397 0.983909 0.005095 0.085457 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2397 2398 0.988269 0.012374 0.039966 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2398 2399 1.043820 0.014037 0.403245 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2399 2400 0.007640 0.951301 1.370012 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2400 2401 1.007510 -0.019002 -0.015410 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2401 2402 0.990493 -0.035273 0.036306 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2402 2403 0.982867 -0.005261 0.047765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2403 2404 0.004757 -1.021510 -1.449987 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2404 2405 1.016710 0.032368 0.297244 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2405 2406 1.014130 -0.003454 -0.122208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2406 2407 0.986152 -0.009548 0.071344 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2407 2408 0.975126 -0.017190 -0.421589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2408 2409 0.989405 -0.003177 -0.327251 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2409 2410 0.987969 0.003733 0.007913 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2410 2411 0.978191 0.034119 -0.023179 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2411 2412 0.017579 -0.973635 -1.690469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2412 2413 1.027290 -0.049605 -0.328713 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2413 2414 1.007370 -0.005587 -0.067489 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2414 2415 1.004390 0.015882 0.180488 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2415 2416 -0.007043 -1.001090 -1.478214 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2416 2417 0.993548 -0.049697 -0.203759 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2417 2418 1.009430 0.002092 0.000279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2418 2419 0.952458 -0.003079 0.196065 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2419 2420 0.017409 -0.983651 -1.465684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2420 2421 0.998007 -0.004617 -0.442055 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2421 2422 0.973989 -0.020616 0.002835 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2422 2423 0.993037 -0.002264 0.137515 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2423 2424 0.007441 -1.019110 -1.594508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2424 2425 0.997725 0.037861 -0.219684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2425 2426 0.993401 0.001353 0.045203 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2426 2427 0.980140 -0.019804 0.108887 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2427 2428 -1.015490 0.028316 3.045636 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2428 2429 1.013550 -0.016318 -0.044089 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2429 2430 1.000500 0.012007 0.040900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2430 2431 0.980536 0.028899 0.050688 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2431 2432 -0.038224 0.989710 1.672652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2432 2433 0.945463 0.033844 0.138578 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2433 2434 1.037410 0.007813 -0.028926 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2434 2435 1.015130 0.021090 -0.175033 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2435 2436 0.021441 -0.952513 -1.345616 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2436 2437 1.007310 0.007148 0.103101 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2437 2438 0.994819 -0.008296 -0.191589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2438 2439 0.974810 -0.002067 0.088788 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2439 2440 -1.017670 -0.015610 -3.003990 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2440 2441 0.973763 -0.075845 -0.133318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2441 2442 1.004860 0.014404 0.281047 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2442 2443 1.035850 -0.011818 0.034793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2443 2444 -0.997898 0.000558 2.866930 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2444 2445 0.985069 0.035172 0.083716 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2445 2446 0.987248 -0.041376 0.244187 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2446 2447 0.966632 0.011426 -0.153621 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2447 2448 0.015128 1.016740 1.513713 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2448 2449 1.039410 0.020530 0.266970 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2449 2450 1.048360 0.024748 0.215471 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2450 2451 0.975624 0.004843 -0.079277 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2451 2452 -0.030815 -0.980627 -1.646506 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2452 2453 0.994648 -0.005211 0.128693 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2453 2454 1.012790 0.014637 0.107181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2454 2455 1.002430 0.001714 0.064663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2455 2456 -0.009736 -0.945141 -1.745876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2456 2457 0.975689 -0.032321 0.020900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2457 2458 0.985021 0.001410 0.108079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2458 2459 0.998864 -0.020136 0.317098 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2459 2460 -1.019980 -0.044906 -2.880580 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2460 2461 0.976128 0.023313 -0.036295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2461 2462 1.060540 0.004224 -0.071541 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2462 2463 0.985749 -0.009909 -0.057177 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2463 2464 -0.011768 -1.037940 -1.172032 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2464 2465 1.002600 -0.008214 0.260704 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2465 2466 1.010000 0.017268 0.367779 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2466 2467 0.992166 -0.015853 0.033578 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2467 2468 -0.001444 -1.003060 -1.427070 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2468 2469 1.024790 0.006704 -0.341377 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2469 2470 1.017810 0.022026 -0.227243 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2470 2471 1.006580 0.016374 0.034472 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2471 2472 -1.027070 0.029597 2.942277 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2472 2473 0.977193 -0.013784 -0.311233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2473 2474 0.984869 -0.025309 0.000109 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2474 2475 0.976183 0.021947 0.172701 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2475 2476 -0.027204 -1.005580 -1.705223 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2476 2477 0.971386 0.021212 -0.055446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2477 2478 1.000340 0.031375 0.443783 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2478 2479 0.991888 -0.033196 0.369001 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2479 2480 0.019540 1.003840 1.410236 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2480 2481 1.016820 0.005368 0.005768 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2481 2482 1.018170 0.020080 -0.443968 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2482 2483 1.006980 -0.002176 0.110786 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2483 2484 -1.011410 0.022933 -3.112947 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2484 2485 1.008570 0.036605 0.071436 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2485 2486 0.978324 0.008132 0.237062 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2486 2487 0.983592 -0.000733 0.128930 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2487 2488 -1.024440 -0.006391 2.862100 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2488 2489 1.031550 0.038049 -0.337757 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2489 2490 0.971965 -0.009160 0.017666 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2490 2491 0.971884 -0.043031 -0.084299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2491 2492 -0.993596 -0.026781 2.981482 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2492 2493 1.025120 0.009166 -0.448403 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2493 2494 1.021710 0.012302 -0.135459 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2494 2495 0.982030 -0.037073 0.160377 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2495 2496 -0.031874 0.987954 1.435073 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2496 2497 0.996470 -0.008052 0.029741 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2497 2498 0.985049 -0.014365 -0.121029 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2498 2499 0.950240 0.015325 0.054299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2499 2500 0.023296 0.952911 1.525649 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2500 2501 0.972448 -0.008243 0.225468 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2501 2502 1.017900 0.026273 -0.176709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2502 2503 0.996636 0.015367 0.041427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2503 2504 0.003166 -0.974354 -1.540190 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2504 2505 0.997176 0.014390 -0.381233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2505 2506 0.958666 0.001057 0.076220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2506 2507 0.983484 -0.035570 -0.223181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2507 2508 -0.000170 1.015400 1.289824 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2508 2509 0.992670 0.007838 0.024615 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2509 2510 1.005290 0.014293 0.263895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2510 2511 0.992335 0.034436 0.099048 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2511 2512 0.010893 -0.999550 -1.291533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2512 2513 0.956889 0.024955 -0.088600 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2513 2514 1.029510 0.003366 -0.024093 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2514 2515 0.974999 0.036194 0.102871 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2515 2516 0.976525 0.000917 0.183565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2516 2517 0.999981 -0.021517 -0.167744 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2517 2518 1.006320 0.013775 -0.089100 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2518 2519 1.004020 0.031642 -0.039139 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2519 2520 -0.011945 -0.989462 -1.595300 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2520 2521 0.955626 0.012950 0.070168 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2521 2522 0.978242 0.026872 0.087140 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2522 2523 1.020750 -0.002714 -0.069468 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2523 2524 0.040196 -1.041050 -1.515039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2524 2525 1.002630 0.024244 0.146091 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2525 2526 1.007040 0.034427 -0.259712 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2526 2527 1.025940 0.034152 -0.105695 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2527 2528 -0.030556 1.040350 1.096990 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2528 2529 1.016070 -0.015830 0.246122 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2529 2530 1.006910 -0.003724 0.177303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2530 2531 0.952368 -0.023046 -0.103203 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2531 2532 -1.031230 -0.035047 3.033136 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2532 2533 1.013620 -0.019766 0.430881 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2533 2534 0.996564 -0.006384 0.032667 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2534 2535 0.961255 -0.019420 -0.317669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2535 2536 0.034682 1.008480 1.826581 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2536 2537 1.010280 -0.008966 0.085834 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2537 2538 1.004140 0.010468 0.258098 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2538 2539 1.007280 -0.014359 0.271427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2539 2540 0.029741 -0.964773 -2.028721 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2540 2541 1.010850 -0.004408 -0.030880 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2541 2542 0.993392 -0.001499 0.529181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2542 2543 1.003120 0.023077 0.036524 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2543 2544 -0.033161 -1.015750 -1.645946 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2544 2545 1.012330 -0.010048 0.035035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2545 2546 1.005530 0.036239 0.261893 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2546 2547 1.004700 -0.064724 -0.040879 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2547 2548 -0.027022 1.013050 1.899644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2548 2549 0.983559 0.026136 -0.222845 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2549 2550 0.987719 -0.024198 -0.288098 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2550 2551 0.949905 -0.015036 -0.159883 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2551 2552 0.005566 0.996513 1.421451 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2552 2553 1.026370 -0.056116 0.047159 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2553 2554 1.034580 -0.000948 0.338973 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2554 2555 0.993381 -0.000922 -0.276669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2555 2556 0.001726 0.998735 1.222753 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2556 2557 1.000040 -0.021764 -0.189585 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2557 2558 0.977222 0.007958 -0.289053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2558 2559 1.002540 0.000133 0.038540 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2559 2560 0.007190 -1.009730 -1.580766 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2560 2561 1.059860 0.030545 -0.220895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2561 2562 0.996955 0.022855 0.104288 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2562 2563 0.984247 -0.015238 -0.245992 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2563 2564 -0.001139 -1.007550 -1.595578 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2564 2565 1.006370 0.004958 0.017493 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2565 2566 0.983329 0.009109 -0.033245 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2566 2567 1.009350 0.015927 0.000157 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2567 2568 1.013130 -0.014550 -0.108609 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2568 2569 1.001080 0.017426 -0.146339 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2569 2570 1.044920 -0.037594 -0.016515 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2570 2571 0.947230 -0.006970 0.048014 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2571 2572 -0.003718 -1.010410 -1.713245 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2572 2573 0.976291 -0.037995 0.014934 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2573 2574 1.010940 0.007439 0.195697 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2574 2575 1.005770 -0.002686 -0.446542 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2575 2576 -0.002166 1.029400 1.463709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2576 2577 0.969704 0.006178 0.028875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2577 2578 1.020980 -0.013177 -0.024643 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2578 2579 1.000200 0.021216 0.374865 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2579 2580 0.031278 0.999896 1.770903 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2580 2581 0.976488 -0.005630 0.150950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2581 2582 1.020950 0.046721 0.079784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2582 2583 0.973970 -0.019040 -0.218070 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2583 2584 -1.034930 0.026163 -3.031142 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2584 2585 1.023780 0.054077 -0.087949 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2585 2586 0.992401 -0.024982 -0.384432 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2586 2587 0.978174 0.028697 -0.449726 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2587 2588 -0.991739 0.014043 -2.998120 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2588 2589 0.983124 -0.009585 -0.110542 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2589 2590 0.988444 -0.065570 0.000466 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2590 2591 0.988775 0.036641 -0.030564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2591 2592 -1.034040 0.027666 2.960968 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2592 2593 0.996344 -0.007121 -0.104394 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2593 2594 0.980607 0.035752 0.072150 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2594 2595 1.011270 0.008212 -0.437553 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2595 2596 0.005671 1.014980 1.628437 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2596 2597 1.008770 0.031534 -0.189105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2597 2598 1.029190 0.020126 0.221411 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2598 2599 0.987402 -0.014522 -0.006317 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2599 2600 0.024789 1.001140 1.550437 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2600 2601 1.012480 -0.017861 -0.178883 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2601 2602 1.004980 -0.015154 -0.244780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2602 2603 1.000260 -0.008299 0.230764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2603 2604 -1.007440 0.023920 2.929941 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2604 2605 1.014910 -0.023570 0.188300 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2605 2606 1.021180 -0.023134 0.191858 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2606 2607 1.033080 -0.003327 0.031722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2607 2608 0.005722 0.991335 1.693944 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2608 2609 0.999341 -0.032959 -0.132900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2609 2610 0.967245 -0.034448 -0.091372 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2610 2611 0.966967 0.003459 0.247566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2611 2612 0.031466 -0.965304 -2.097985 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2612 2613 0.989573 -0.012328 -0.087878 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2613 2614 0.979051 -0.018950 -0.004003 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2614 2615 0.996498 -0.020305 -0.151044 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2615 2616 0.001744 0.984127 1.841906 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2616 2617 1.017330 -0.015515 0.008001 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2617 2618 0.978097 -0.035117 0.108099 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2618 2619 1.004590 -0.000950 -0.131631 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2619 2620 0.007133 0.989692 1.657505 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2620 2621 0.979856 0.002194 -0.454419 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2621 2622 0.961244 0.026991 0.213437 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2622 2623 0.944850 0.011921 0.154875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2623 2624 0.027901 -1.006550 -1.690203 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2624 2625 0.985052 -0.012908 0.194813 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2625 2626 1.002960 -0.008341 0.188583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2626 2627 1.029470 -0.004407 -0.238968 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2627 2628 -0.015342 1.008470 1.719732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2628 2629 1.030810 0.008563 0.079797 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2629 2630 0.988316 -0.015516 -0.088329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2630 2631 0.987842 -0.043459 -0.027509 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2631 2632 0.007913 0.977768 1.948822 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2632 2633 0.978632 -0.014704 -0.045887 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2633 2634 0.995650 -0.024250 -0.123357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2634 2635 1.018810 -0.019919 0.108088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2635 2636 1.011840 0.023149 0.232855 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2636 2637 0.993604 -0.015338 0.018627 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2637 2638 1.005250 0.052719 -0.091940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2638 2639 0.981526 0.022333 0.210698 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2639 2640 -0.006802 -1.008270 -1.235963 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2640 2641 0.990960 0.008862 -0.042468 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2641 2642 1.004730 -0.002479 0.132570 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2642 2643 0.984184 0.046791 -0.337039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2643 2644 -1.003540 0.005754 2.999200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2644 2645 1.019790 -0.036897 -0.142470 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2645 2646 1.020130 0.033542 -0.257262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2646 2647 1.031890 0.018279 0.018139 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2647 2648 -1.005700 0.003971 2.982353 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2648 2649 1.010270 -0.014716 -0.233079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2649 2650 1.001240 -0.003960 0.229455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2650 2651 1.022820 -0.015281 0.045651 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2651 2652 0.030175 -0.995920 -1.331946 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2652 2653 0.958399 0.012936 0.076487 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2653 2654 0.964300 -0.016478 0.363731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2654 2655 0.977275 -0.028211 0.126054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2655 2656 -0.026504 0.941574 1.686228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2656 2657 1.022060 -0.018502 0.187901 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2657 2658 0.992838 -0.001822 -0.151048 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2658 2659 0.966286 0.006732 -0.224688 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2659 2660 -0.988917 -0.014257 -2.962046 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2660 2661 1.025310 -0.018150 0.173497 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2661 2662 0.997025 -0.004987 -0.146474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2662 2663 0.979572 0.014225 -0.000958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2663 2664 0.036029 -0.997376 -1.580689 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2664 2665 1.029200 -0.002306 0.088020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2665 2666 1.012430 0.011073 0.035387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2666 2667 1.016230 0.011740 -0.274882 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2667 2668 -0.054041 0.991373 1.755873 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2668 2669 0.973294 -0.001801 0.038723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2669 2670 1.000770 -0.008036 0.017589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2670 2671 0.990828 0.000318 0.086262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2671 2672 0.021806 -0.993917 -1.534626 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2672 2673 0.999649 -0.000226 -0.048540 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2673 2674 0.997989 -0.001386 -0.222850 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2674 2675 0.984877 -0.007442 -0.031873 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2675 2676 -0.980244 0.008222 3.068105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2676 2677 0.992981 0.006796 0.186767 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2677 2678 0.967876 0.059901 0.480275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2678 2679 1.054860 0.004162 -0.102961 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2679 2680 0.014926 1.007980 1.740338 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2680 2681 1.016410 0.012327 0.112585 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2681 2682 1.002490 0.023579 -0.032355 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2682 2683 1.006920 -0.002750 0.158167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2683 2684 0.044367 -1.013720 -1.483494 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2684 2685 1.001620 0.000420 0.053202 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2685 2686 1.025410 -0.005786 -0.259849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2686 2687 1.010420 -0.018407 -0.148754 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2687 2688 -1.005980 -0.021517 -2.833018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2688 2689 0.996324 0.001011 0.182649 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2689 2690 1.022460 0.018811 0.087451 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2690 2691 1.012040 -0.035256 0.257173 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2691 2692 -0.971418 0.016225 -3.032755 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2692 2693 1.010460 -0.033049 -0.029894 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2693 2694 0.983227 -0.019670 -0.093906 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2694 2695 1.018560 0.007524 -0.089075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2695 2696 -0.998356 0.004728 3.106183 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2696 2697 1.024270 0.014323 -0.290633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2697 2698 1.022690 -0.043679 -0.055374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2698 2699 0.963962 0.041828 0.215909 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2699 2700 -0.978435 -0.014490 3.093835 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2700 2701 1.037480 0.030260 0.062386 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2701 2702 1.043470 0.003407 0.042565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2702 2703 1.003060 -0.036924 0.461268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2703 2704 0.001601 1.016020 1.329079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2704 2705 0.967411 0.008063 -0.268358 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2705 2706 0.982967 0.014053 0.094534 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2706 2707 0.995136 0.027239 0.183596 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2707 2708 0.025409 -0.994799 -1.674017 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2708 2709 1.016600 0.044091 -0.181980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2709 2710 1.002260 0.027744 -0.058269 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2710 2711 1.009940 0.000674 -0.129351 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2711 2712 0.002035 -0.996933 -1.439375 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2712 2713 1.021500 -0.024379 0.062751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2713 2714 0.990908 -0.027228 -0.692028 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2714 2715 1.012780 0.016911 0.037993 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2715 2716 1.042150 0.024499 -0.175543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2716 2717 0.997581 -0.009229 -0.119249 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2717 2718 1.016870 -0.009794 0.031574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2718 2719 1.030480 -0.045470 -0.127419 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2719 2720 -1.015590 0.011203 2.896747 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2720 2721 0.988251 -0.017214 -0.036809 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2721 2722 1.009910 0.022868 -0.172082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2722 2723 1.036930 0.013254 -0.095385 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2723 2724 -0.992572 0.029954 2.827781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2724 2725 0.990565 -0.029281 0.237692 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2725 2726 1.021910 0.012625 -0.071652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2726 2727 0.971779 -0.034815 0.021508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2727 2728 -0.008257 -1.030740 -1.840427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2728 2729 0.993570 -0.012805 -0.013726 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2729 2730 1.007430 0.039659 0.252548 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2730 2731 1.042410 -0.010424 -0.032513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2731 2732 -1.011330 -0.003181 -2.912016 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2732 2733 0.998816 0.000856 0.157931 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2733 2734 1.017510 0.031519 0.059000 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2734 2735 1.012890 -0.038101 0.008059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2735 2736 -0.996330 -0.001944 3.095833 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2736 2737 1.021830 0.002296 0.196703 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2737 2738 1.010670 0.023052 0.135768 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2738 2739 1.004500 -0.008408 -0.274117 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2739 2740 -0.967826 -0.037447 -2.822213 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2740 2741 1.005580 0.007925 0.036530 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2741 2742 1.026280 0.032292 0.131563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2742 2743 0.995761 0.053594 0.087792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2743 2744 -1.003820 0.005106 -2.895462 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2744 2745 1.004250 -0.000317 -0.181033 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2745 2746 0.957606 0.003090 -0.042729 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2746 2747 1.013090 -0.033207 -0.044041 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2747 2748 0.030165 -0.962281 -1.279275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2748 2749 1.024940 -0.004027 0.460208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2749 2750 0.998841 -0.003074 -0.060446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2750 2751 0.985981 0.010478 0.099610 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2751 2752 -1.011500 -0.024271 2.848560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2752 2753 1.005090 -0.023216 -0.027420 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2753 2754 1.015220 -0.014182 -0.157634 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2754 2755 1.018290 -0.024871 0.067131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2755 2756 -1.003430 -0.032490 -2.686931 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2756 2757 0.987912 0.001077 -0.046680 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2757 2758 0.991127 0.014479 0.091245 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2758 2759 0.976544 -0.007568 0.199188 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2759 2760 -0.977498 0.053981 2.957379 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2760 2761 1.002700 -0.003736 -0.353220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2761 2762 0.997763 0.019986 -0.011283 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2762 2763 0.988149 -0.012439 -0.194014 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2763 2764 -0.009858 -1.020110 -1.270799 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2764 2765 1.037190 -0.006954 0.297750 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2765 2766 1.006130 0.027376 -0.108864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2766 2767 1.020050 -0.026575 0.278870 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2767 2768 -1.012670 -0.024303 2.901234 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2768 2769 1.001200 -0.002950 0.083888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2769 2770 0.993216 0.010407 0.044325 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2770 2771 1.018570 -0.014331 0.009278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2771 2772 -1.031550 -0.001962 3.050822 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2772 2773 1.013360 0.016951 -0.218775 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2773 2774 1.010190 -0.036899 -0.037536 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2774 2775 0.991095 -0.002216 0.070060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2775 2776 -0.011470 -1.000670 -1.464428 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2776 2777 0.976581 0.012713 0.244439 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2777 2778 0.997798 -0.019065 -0.100095 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2778 2779 0.957119 -0.009488 -0.184155 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2779 2780 0.021778 -0.998726 -1.506447 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2780 2781 1.017480 -0.026751 -0.067765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2781 2782 0.972341 -0.013938 0.192039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2782 2783 1.020970 -0.062003 -0.332033 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2783 2784 -0.017293 -0.978436 -1.660086 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2784 2785 1.021560 -0.033653 0.026862 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2785 2786 1.033670 -0.015221 -0.108792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2786 2787 1.037600 -0.013239 -0.202116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2787 2788 -0.020474 -1.006960 -1.325853 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2788 2789 1.047050 0.007804 0.223571 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2789 2790 1.019010 -0.022033 0.336416 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2790 2791 1.031340 -0.020549 0.160984 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2791 2792 -0.977847 0.012812 3.053811 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2792 2793 0.993443 0.014183 -0.063320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2793 2794 0.988130 -0.002052 0.078251 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2794 2795 1.036340 0.015252 0.232991 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2795 2796 0.010786 -0.991349 -1.616032 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2796 2797 1.022840 -0.017920 -0.378770 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2797 2798 1.005120 -0.008266 -0.198047 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2798 2799 1.026410 -0.034886 0.167541 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2799 2800 -0.022505 -1.042210 -1.601841 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2800 2801 1.007870 0.002592 0.162980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2801 2802 0.994095 -0.043818 0.084151 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2802 2803 0.993646 0.008640 -0.320136 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2803 2804 -1.004350 -0.036364 -2.908810 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2804 2805 0.960908 0.018947 -0.277633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2805 2806 0.982568 0.035860 -0.030816 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2806 2807 1.049850 -0.022659 0.037659 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2807 2808 -0.007134 1.031370 1.675935 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2808 2809 1.002810 0.002313 0.291752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2809 2810 1.002990 0.003580 0.228061 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2810 2811 1.027240 0.019647 -0.173574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2811 2812 -0.015855 -0.990595 -1.765013 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2812 2813 0.989364 0.024907 0.203688 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2813 2814 1.025990 -0.046769 0.122272 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2814 2815 0.998673 0.043010 0.030039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2815 2816 0.011728 -1.032740 -1.551186 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2816 2817 0.992600 -0.016130 0.130827 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2817 2818 0.969988 0.010409 -0.091374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2818 2819 0.991991 -0.026672 0.408772 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2819 2820 0.018667 0.962607 1.533050 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2820 2821 1.039520 0.048674 0.020648 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2821 2822 0.983641 -0.017363 -0.106907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2822 2823 0.995486 -0.026001 0.073550 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2823 2824 0.021588 -1.013210 -1.502807 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2824 2825 0.987154 -0.015331 -0.180751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2825 2826 1.020660 -0.008477 0.212918 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2826 2827 0.972462 -0.022776 -0.121381 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2827 2828 0.001002 0.989616 1.386709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2828 2829 0.962607 -0.026840 0.081622 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2829 2830 0.959035 -0.023510 -0.090346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2830 2831 0.992401 0.047971 -0.014320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2831 2832 0.033301 0.998961 1.259834 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2832 2833 0.995844 0.002860 -0.129569 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2833 2834 1.014730 0.031986 0.107051 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2834 2835 1.025720 -0.023854 -0.030234 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2835 2836 0.011713 -0.995912 -1.573440 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2836 2837 1.017240 -0.027877 -0.218547 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2837 2838 0.987415 -0.008038 -0.340274 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2838 2839 1.018770 -0.013337 -0.162932 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2839 2840 -0.018985 0.979113 1.469387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2840 2841 1.040510 -0.004036 -0.056167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2841 2842 1.000060 0.011258 0.185249 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2842 2843 0.978551 -0.014215 -0.084349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2843 2844 1.022220 0.015610 0.225428 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2844 2845 1.012230 -0.021814 0.316323 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2845 2846 1.000540 -0.020415 0.045314 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2846 2847 1.000140 0.013666 -0.065790 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2847 2848 0.006214 0.979603 1.594265 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2848 2849 0.971837 0.011227 -0.089220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2849 2850 0.976713 0.008125 -0.075356 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2850 2851 1.019970 -0.000770 0.183940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2851 2852 0.005544 1.013860 1.697041 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2852 2853 1.019150 0.015311 -0.275731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2853 2854 1.013280 0.024962 -0.240307 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2854 2855 0.958794 0.004062 0.234879 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2855 2856 0.033886 -0.997440 -1.669637 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2856 2857 0.992447 0.029367 -0.136256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2857 2858 1.004650 -0.028164 0.312241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2858 2859 1.021950 0.051418 0.130522 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2859 2860 -1.026920 0.013534 -2.891102 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2860 2861 0.996195 0.020259 -0.103962 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2861 2862 0.995985 0.004240 0.294640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2862 2863 1.032940 -0.022457 -0.105603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2863 2864 -1.003990 0.018355 -3.104980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2864 2865 1.007260 -0.010953 -0.152551 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2865 2866 0.994487 -0.015202 0.050200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2866 2867 0.999958 -0.022810 -0.005589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2867 2868 0.015650 -1.011780 -1.461763 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2868 2869 1.015220 -0.010405 -0.165587 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2869 2870 1.002550 -0.037283 -0.242873 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2870 2871 0.993873 -0.001312 -0.211948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2871 2872 -0.015496 -0.992639 -1.722457 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2872 2873 0.976310 -0.020022 -0.171353 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2873 2874 1.012630 -0.040582 -0.130939 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2874 2875 1.031060 0.007076 -0.333986 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2875 2876 -0.011463 1.003950 1.378007 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2876 2877 0.961769 -0.009444 -0.439436 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2877 2878 1.028950 -0.033197 -0.298411 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2878 2879 0.978274 -0.024873 0.108722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2879 2880 0.015975 -1.013850 -1.799272 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2880 2881 1.000110 0.000539 -0.004016 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2881 2882 1.001900 0.036002 -0.149466 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2882 2883 1.009340 0.011900 0.158551 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2883 2884 0.014164 1.008070 1.132369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2884 2885 1.028990 -0.026388 -0.012061 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2885 2886 1.015090 0.013922 0.320543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2886 2887 1.004450 -0.063117 0.182724 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2887 2888 0.001424 -1.027230 -1.406983 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2888 2889 1.032820 0.014000 -0.095648 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2889 2890 0.987343 -0.021351 0.693662 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2890 2891 1.019140 0.018025 0.000412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2891 2892 -0.008240 1.020320 1.791731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2892 2893 1.001550 -0.022703 0.362526 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2893 2894 1.004270 -0.001822 -0.057299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2894 2895 1.031220 -0.024904 0.012836 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2895 2896 -0.039929 -1.027380 -1.654653 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2896 2897 1.013130 0.017634 -0.219039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2897 2898 0.999414 -0.017737 0.153252 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2898 2899 1.002400 0.035070 -0.002220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2899 2900 -0.999026 0.000149 2.823847 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2900 2901 1.004260 0.003587 -0.038436 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2901 2902 1.010220 -0.003167 0.224484 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2902 2903 1.014940 -0.004698 -0.051752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2903 2904 -0.995950 -0.007645 -3.056054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2904 2905 1.030080 -0.019404 0.000864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2905 2906 1.048390 0.034364 -0.375532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2906 2907 1.011820 -0.023392 -0.121259 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2907 2908 -0.989741 0.007428 3.075531 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2908 2909 1.016590 0.011892 -0.318586 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2909 2910 0.998109 -0.010367 -0.370393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2910 2911 1.011630 0.009822 0.137473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2911 2912 0.013964 1.011420 1.672405 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2912 2913 1.023120 0.024857 -0.130182 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2913 2914 1.024880 -0.018936 -0.382563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2914 2915 0.995993 -0.070615 0.255246 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2915 2916 -0.993659 0.016830 -2.987013 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2916 2917 1.004900 0.056664 -0.148618 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2917 2918 1.028590 0.026529 -0.019115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2918 2919 1.038980 0.006681 0.068210 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2919 2920 0.004279 -0.983973 -1.638395 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2920 2921 1.011970 0.007374 0.033857 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2921 2922 0.979387 0.009373 0.417814 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2922 2923 0.971936 -0.006456 -0.019385 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2923 2924 0.012787 -1.014390 -1.309836 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2924 2925 0.960955 -0.033283 0.220461 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2925 2926 0.987199 0.012755 -0.143590 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2926 2927 0.990042 0.009911 0.223606 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2927 2928 -0.013347 -1.003670 -1.746645 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2928 2929 0.981579 -0.012719 0.093363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2929 2930 1.021570 0.023280 0.009946 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2930 2931 0.994768 0.023812 -0.071335 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2931 2932 -0.982008 -0.005613 -2.880038 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2932 2933 1.024340 0.058055 0.110328 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2933 2934 0.965431 0.032266 -0.077993 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2934 2935 1.017790 -0.016027 0.166305 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2935 2936 -1.020130 -0.008964 -3.051462 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2936 2937 1.001050 0.020630 0.148322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2937 2938 0.970688 0.006325 -0.007130 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2938 2939 1.023280 0.038712 -0.347217 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2939 2940 -1.015010 0.031593 -3.075474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2940 2941 1.011010 -0.037083 0.229052 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2941 2942 1.009430 0.004082 -0.076230 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2942 2943 0.981258 -0.012120 -0.154670 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2943 2944 -0.003730 -1.010510 -1.599214 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2944 2945 1.011480 0.018309 0.066739 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2945 2946 1.019600 0.008857 0.108254 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2946 2947 0.969378 0.022525 -0.056225 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2947 2948 -0.008019 0.987384 1.910843 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2948 2949 0.965265 -0.030686 -0.305501 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2949 2950 0.995730 -0.007599 -0.286869 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2950 2951 0.997706 0.005772 0.532127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2951 2952 -0.032460 0.993589 1.662774 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2952 2953 0.991453 0.016916 0.202619 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2953 2954 1.016920 -0.047744 -0.141334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2954 2955 0.990303 0.019731 -0.007676 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2955 2956 -0.986956 0.002509 -2.811622 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2956 2957 0.985486 -0.001759 -0.047618 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2957 2958 1.023280 0.006678 -0.116042 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2958 2959 1.026570 0.007830 0.013067 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2959 2960 -0.983144 0.023060 2.671584 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2960 2961 0.965787 -0.035311 0.150318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2961 2962 0.975433 -0.007328 -0.098707 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2962 2963 1.006510 0.015197 -0.198482 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2963 2964 0.003901 -1.027740 -1.623223 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2964 2965 1.002810 -0.011664 -0.043094 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2965 2966 0.979011 0.033504 -0.180956 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2966 2967 1.006860 -0.014917 -0.395823 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2967 2968 0.027754 -0.952560 -1.708525 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2968 2969 1.019280 -0.037401 0.055595 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2969 2970 0.995999 0.035825 -0.213513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2970 2971 1.012960 -0.008028 -0.089913 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2971 2972 -0.995114 -0.001823 3.073347 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2972 2973 1.004520 0.014816 0.039292 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2973 2974 1.022860 0.019968 0.000579 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2974 2975 0.974703 -0.014532 -0.085365 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2975 2976 -0.005036 -0.995235 -1.816424 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2976 2977 0.967418 -0.018829 -0.257629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2977 2978 0.999220 0.016715 0.036301 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2978 2979 1.010460 -0.000049 -0.100229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2979 2980 -1.021920 -0.002723 -3.060144 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2980 2981 1.003910 -0.013932 0.093677 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2981 2982 1.029770 -0.024625 0.226538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2982 2983 0.987649 -0.025314 0.109115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2983 2984 -0.982962 0.021382 3.093756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2984 2985 0.977620 0.007767 0.001405 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2985 2986 1.014750 -0.003285 0.082442 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2986 2987 1.049360 0.014825 -0.002058 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2987 2988 0.019128 -1.017890 -1.762832 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2988 2989 0.941729 0.001676 -0.097365 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2989 2990 0.967853 -0.014148 0.407969 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2990 2991 0.975238 0.003791 -0.057211 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2991 2992 0.000965 0.987860 1.317938 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2992 2993 1.025030 0.005818 0.013698 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2993 2994 0.972741 -0.008795 -0.069857 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2994 2995 0.985798 0.032731 0.345782 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2995 2996 -0.015097 1.051280 1.696860 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2996 2997 1.012890 -0.038109 0.087781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2997 2998 0.983235 0.033434 -0.137128 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2998 2999 1.010300 0.054464 0.174914 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2999 3000 0.011849 -1.040770 -1.705526 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3000 3001 0.949358 -0.010485 0.189729 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3001 3002 1.017270 -0.007142 -0.451343 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3002 3003 1.014050 0.011289 0.294295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3003 3004 0.046428 -1.026420 -1.741015 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3004 3005 1.019560 -0.025181 -0.031716 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3005 3006 0.979318 -0.010385 -0.003982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3006 3007 0.997052 0.000346 0.043957 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3007 3008 -0.994087 0.012978 -3.052257 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3008 3009 0.962758 0.025130 0.318126 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3009 3010 1.036380 0.021381 -0.015417 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3010 3011 1.012720 -0.043121 0.028476 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3011 3012 -1.025960 0.010271 -3.037641 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3012 3013 1.014640 -0.045891 0.056987 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3013 3014 1.012950 -0.012644 -0.074942 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3014 3015 0.991530 0.035044 0.135603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3015 3016 0.044520 -1.007300 -1.744562 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3016 3017 0.968854 -0.005499 -0.063794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3017 3018 0.979440 0.007479 0.115817 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3018 3019 0.992653 0.004788 -0.046877 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3019 3020 -0.964126 0.009243 -3.024818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3020 3021 1.031100 -0.030236 -0.112764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3021 3022 0.986029 0.012136 -0.158678 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3022 3023 0.975331 -0.006301 -0.013538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3023 3024 -0.009209 -0.958217 -1.609254 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3024 3025 0.980007 0.023489 -0.287659 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3025 3026 0.985674 0.010867 -0.003785 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3026 3027 0.993694 0.025695 0.035078 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3027 3028 -0.985504 -0.006204 2.817897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3028 3029 0.970024 0.015404 -0.399322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3029 3030 1.005110 0.056684 -0.100793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3030 3031 0.975950 0.002719 0.316316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3031 3032 -0.999902 -0.017723 -3.047142 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3032 3033 0.997774 -0.007012 0.381226 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3033 3034 1.036450 0.044137 -0.212219 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3034 3035 1.008390 0.001449 0.011751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3035 3036 -0.003115 0.970590 1.389557 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3036 3037 0.972989 0.003238 0.317131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3037 3038 1.021430 -0.028384 -0.285911 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3038 3039 0.988378 0.021600 0.019861 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3039 3040 -0.001502 -0.964919 -1.502581 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3040 3041 1.007730 -0.011859 -0.205584 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3041 3042 1.004550 0.012551 0.249274 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3042 3043 0.994287 0.061417 -0.109598 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3043 3044 -0.981177 0.001469 2.903668 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3044 3045 0.995829 0.005394 0.014964 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3045 3046 0.997640 -0.052449 -0.060179 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3046 3047 1.006330 0.019041 -0.018654 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3047 3048 0.011811 1.018760 1.465261 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3048 3049 0.989573 0.021750 0.243196 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3049 3050 0.970189 0.033208 0.017141 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3050 3051 0.986668 -0.007082 0.135344 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3051 3052 0.998260 -0.010288 -0.283933 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3052 3053 1.000550 0.001628 0.190716 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3053 3054 0.970424 0.035196 -0.105779 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3054 3055 0.986476 -0.002696 0.256352 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3055 3056 -0.010733 -0.991533 -1.806267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3056 3057 0.992650 0.003868 -0.008412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3057 3058 0.993704 -0.023899 -0.460489 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3058 3059 0.994726 0.021959 -0.116777 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3059 3060 0.021336 -1.041730 -1.407332 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3060 3061 1.000840 0.019049 0.042822 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3061 3062 1.009340 -0.000537 0.251589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3062 3063 1.019300 0.017017 0.117957 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3063 3064 -0.020555 0.964837 1.340395 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3064 3065 1.047360 0.043389 0.102060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3065 3066 1.005810 -0.021976 -0.022277 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3066 3067 0.986095 -0.026854 0.408024 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3067 3068 -0.007238 -0.982568 -1.458381 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3068 3069 1.016010 -0.021676 0.157003 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3069 3070 0.983223 -0.006413 0.091997 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3070 3071 0.989032 -0.030844 -0.109261 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3071 3072 -0.043996 0.976317 1.733960 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3072 3073 0.979204 0.001265 0.163503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3073 3074 1.015500 -0.020200 0.171328 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3074 3075 0.952273 0.016423 -0.048871 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3075 3076 0.025820 1.037310 1.351495 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3076 3077 0.988828 -0.000432 0.139344 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3077 3078 1.017420 -0.013459 -0.094958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3078 3079 1.011370 0.005204 0.226268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3079 3080 0.009498 0.999466 1.518063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3080 3081 0.977010 0.004558 0.242209 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3081 3082 1.019940 0.035683 0.362123 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3082 3083 0.989132 0.008040 -0.180987 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3083 3084 -0.017071 1.001760 1.673290 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3084 3085 0.979637 0.004384 0.219717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3085 3086 0.996995 -0.060487 0.070551 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3086 3087 1.002230 -0.003233 -0.155523 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3087 3088 0.023995 -1.003350 -1.808778 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3088 3089 0.940594 -0.009397 0.218234 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3089 3090 0.974666 -0.016348 0.032094 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3090 3091 1.003470 -0.035096 -0.302533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3091 3092 -0.005868 -0.982225 -1.791164 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3092 3093 1.002190 -0.012114 0.217326 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3093 3094 1.018750 -0.019448 -0.128276 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3094 3095 0.961561 -0.009705 0.182665 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3095 3096 -0.993661 -0.007216 -2.941434 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3096 3097 1.024260 -0.023602 -0.152601 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3097 3098 1.016880 -0.021928 0.238579 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3098 3099 1.030110 0.005020 0.175006 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3099 3100 0.006887 -0.968244 -1.899214 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3100 3101 0.997617 0.027980 0.072054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3101 3102 1.009040 0.033521 0.183240 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3102 3103 0.996551 -0.024225 0.194358 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3103 3104 -0.974752 -0.017535 3.045483 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3104 3105 0.986964 -0.028888 0.205132 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3105 3106 0.995174 -0.022733 -0.243633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3106 3107 0.982591 -0.030628 -0.209538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3107 3108 -1.026500 0.002373 -3.120672 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3108 3109 1.002680 0.017837 -0.023723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3109 3110 1.016400 0.007271 -0.066617 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3110 3111 0.993447 0.001830 0.169814 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3111 3112 1.002120 -0.015360 0.161521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3112 3113 1.012870 0.057076 -0.131115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3113 3114 0.957435 0.003161 -0.081017 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3114 3115 0.980179 0.017681 0.251885 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3115 3116 -0.017812 -0.988353 -1.438008 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3116 3117 1.023200 0.003165 -0.128676 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3117 3118 0.997735 -0.043241 -0.302590 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3118 3119 1.037020 -0.010359 0.092457 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3119 3120 0.042475 1.030040 1.462919 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3120 3121 0.973994 -0.012334 -0.222936 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3121 3122 1.034400 0.017841 -0.007075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3122 3123 0.972621 0.011912 -0.025220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3123 3124 -0.038270 -1.019300 -1.567539 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3124 3125 1.015080 0.015497 -0.156647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3125 3126 0.990254 0.004846 -0.002649 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3126 3127 0.981534 0.013225 0.004678 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3127 3128 0.970137 -0.001881 -0.036464 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3128 3129 1.019730 -0.010017 -0.007961 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3129 3130 0.941815 0.012729 -0.103814 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3130 3131 1.000640 -0.034665 0.271353 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3131 3132 0.034001 -1.009330 -1.446184 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3132 3133 1.016670 -0.001609 -0.162848 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3133 3134 1.028470 -0.036706 0.293532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3134 3135 1.034290 0.027922 -0.389340 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3135 3136 -0.016567 -1.041300 -1.681858 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3136 3137 1.007340 0.013588 0.121810 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3137 3138 1.010140 0.004208 0.352680 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3138 3139 0.991597 -0.006025 -0.142730 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3139 3140 -0.028050 -1.003610 -1.183960 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3140 3141 1.050590 0.003219 0.306530 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3141 3142 0.991284 0.024321 0.184569 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3142 3143 0.981163 -0.044330 -0.045142 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3143 3144 -0.981978 -0.013366 -3.127445 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3144 3145 1.002050 -0.013540 0.367588 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3145 3146 1.028420 -0.025293 0.291872 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3146 3147 0.995370 0.008368 0.044124 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3147 3148 0.995589 -0.017235 0.048947 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3148 3149 1.055110 0.013693 0.328133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3149 3150 1.023200 0.050185 0.115904 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3150 3151 0.994718 0.009692 0.028241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3151 3152 -0.003944 -0.992141 -1.514730 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3152 3153 0.979559 -0.007663 0.093436 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3153 3154 1.001660 0.008204 -0.111933 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3154 3155 1.027980 -0.004486 -0.095145 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3155 3156 -0.018447 -1.019740 -1.709387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3156 3157 1.020680 0.031927 0.055045 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3157 3158 1.002990 -0.005137 -0.046780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3158 3159 0.985680 0.032260 0.119455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3159 3160 -0.035610 1.022710 1.308642 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3160 3161 1.033370 -0.009778 -0.127013 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3161 3162 1.021200 -0.003259 0.229157 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3162 3163 1.001500 -0.007462 0.032898 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3163 3164 0.030797 0.980349 1.828337 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3164 3165 1.004780 -0.004670 0.195890 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3165 3166 0.960869 0.033826 -0.015149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3166 3167 1.022110 -0.052553 0.112368 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3167 3168 0.031783 0.972155 1.573982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3168 3169 0.976831 0.020663 -0.056160 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3169 3170 1.008120 -0.011115 -0.175626 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3170 3171 1.000490 0.014178 0.155655 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3171 3172 -1.008730 0.033128 -2.980097 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3172 3173 0.998576 -0.015849 -0.029130 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3173 3174 1.011310 0.015076 -0.030352 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3174 3175 1.040810 -0.024728 0.026267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3175 3176 -1.018250 -0.016349 -3.012792 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3176 3177 1.024810 -0.001298 -0.280775 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3177 3178 1.011150 -0.026270 -0.035516 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3178 3179 1.006910 0.013363 0.108616 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3179 3180 -0.994480 -0.050674 -3.064474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3180 3181 0.969415 -0.058401 0.134537 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3181 3182 0.997888 0.016144 0.041151 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3182 3183 0.999458 -0.012818 -0.120937 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3183 3184 -1.058080 0.019248 3.068096 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3184 3185 1.013730 0.008796 0.254260 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3185 3186 1.013490 0.014673 0.121087 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3186 3187 1.021390 0.007879 0.184720 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3187 3188 0.020804 -1.005400 -1.297711 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3188 3189 0.988311 -0.020583 0.025574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3189 3190 1.011590 -0.038495 -0.082633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3190 3191 0.970249 0.015649 0.038910 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3191 3192 0.003827 -0.986228 -1.424703 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3192 3193 0.994494 -0.017611 -0.368581 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3193 3194 0.966012 -0.001789 0.180297 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3194 3195 1.038210 0.010016 -0.336886 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3195 3196 -0.015025 -0.998713 -1.609115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3196 3197 0.989508 0.013562 -0.048244 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3197 3198 1.004170 0.027873 -0.402105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3198 3199 1.031390 -0.000549 -0.057017 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3199 3200 -0.991136 0.026371 -3.020623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3200 3201 1.002790 -0.006385 0.173320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3201 3202 1.022980 0.005412 -0.212338 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3202 3203 0.979212 0.002242 -0.238858 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3203 3204 0.020852 1.011780 1.866343 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3204 3205 0.978291 0.003959 0.030387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3205 3206 1.004260 -0.012109 -0.136743 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3206 3207 1.012720 -0.019548 -0.061528 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3207 3208 -0.007101 0.995182 1.424376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3208 3209 0.997039 -0.022550 0.059504 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3209 3210 1.005140 -0.003952 0.139478 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3210 3211 1.010480 -0.005612 -0.306309 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3211 3212 0.026720 -0.993725 -1.970803 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3212 3213 0.986010 0.047137 -0.050149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3213 3214 1.036350 -0.023911 -0.050849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3214 3215 0.982395 0.008032 0.315271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3215 3216 -0.051563 1.042360 1.548420 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3216 3217 0.981199 0.015844 -0.041325 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3217 3218 0.995275 -0.017249 -0.240608 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3218 3219 0.986383 -0.004405 0.268347 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3219 3220 0.005746 -0.969143 -1.364030 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3220 3221 0.996395 -0.012152 -0.070191 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3221 3222 0.981031 0.008997 -0.102947 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3222 3223 1.018790 0.015672 0.163843 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3223 3224 -1.019030 0.024485 2.895879 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3224 3225 1.022830 -0.005479 -0.098000 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3225 3226 0.989587 -0.020687 -0.114976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3226 3227 0.981982 0.007701 -0.049520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3227 3228 0.010294 1.049480 2.057786 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3228 3229 1.003250 -0.009607 -0.257643 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3229 3230 1.009520 -0.009812 -0.313627 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3230 3231 0.998004 -0.008317 0.326710 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3231 3232 -0.001194 -0.949646 -1.582455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3232 3233 1.007160 0.015357 0.171618 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3233 3234 0.999688 -0.025097 -0.203369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3234 3235 1.028760 0.003181 -0.129499 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3235 3236 0.009509 1.003530 1.608900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3236 3237 0.994913 -0.018592 -0.071783 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3237 3238 0.970273 0.008640 0.345771 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3238 3239 1.014730 -0.008631 0.154590 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3239 3240 0.021401 1.000930 1.457726 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3240 3241 0.994319 0.005962 -0.123279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3241 3242 1.020890 -0.004046 -0.133876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3242 3243 1.011220 0.008063 0.155926 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3243 3244 -0.028569 -0.975524 -1.429891 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3244 3245 1.026990 0.003618 0.160271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3245 3246 0.985678 -0.019764 -0.239320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3246 3247 1.002410 -0.051475 -0.076565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3247 3248 -0.013734 -0.994023 -1.378609 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3248 3249 0.992300 -0.025487 -0.329326 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3249 3250 1.006060 -0.022211 0.081311 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3250 3251 1.023410 -0.007577 -0.185016 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3251 3252 0.027151 1.038630 1.739810 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3252 3253 0.984537 0.048160 -0.062434 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3253 3254 1.029420 0.046449 0.361722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3254 3255 1.063040 0.035190 -0.072630 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3255 3256 -0.003733 1.005170 1.673216 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3256 3257 0.994021 0.018864 -0.206079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3257 3258 0.968680 0.030430 -0.044265 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3258 3259 1.001920 -0.006390 0.058178 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3259 3260 0.005830 1.000990 1.080470 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3260 3261 0.970383 0.012995 0.039514 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3261 3262 1.038540 -0.029174 -0.139512 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3262 3263 0.963657 -0.033835 -0.079432 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3263 3264 -1.002580 0.016192 -3.141570 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3264 3265 0.986377 -0.019337 0.394428 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3265 3266 1.008190 0.004562 -0.219211 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3266 3267 1.012250 -0.008190 -0.095626 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3267 3268 0.047126 0.990677 1.457185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3268 3269 0.999563 -0.004261 -0.122652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3269 3270 0.986505 0.009717 0.054729 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3270 3271 0.994982 -0.004465 0.187125 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3271 3272 -0.001707 0.966667 1.588958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3272 3273 0.995110 -0.017087 -0.180458 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3273 3274 0.989202 0.029826 -0.165758 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3274 3275 1.004410 0.005848 0.134895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3275 3276 0.974643 0.008885 -0.069462 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3276 3277 0.996159 0.006740 0.059446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3277 3278 1.003740 -0.023628 0.238092 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3278 3279 1.046830 0.041642 0.123527 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3279 3280 -0.968141 0.011181 2.827061 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3280 3281 0.984484 -0.013169 0.176186 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3281 3282 1.033630 -0.003531 -0.105861 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3282 3283 1.002540 -0.003289 0.087377 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3283 3284 0.021268 -0.995361 -1.708782 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3284 3285 1.003310 -0.008733 0.101400 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3285 3286 1.004240 -0.056349 -0.268232 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3286 3287 1.009710 0.030077 0.305166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3287 3288 1.033450 -0.053486 -0.309519 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3288 3289 0.999968 -0.006819 -0.272311 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3289 3290 1.008640 -0.001390 -0.252337 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3290 3291 1.013380 0.051969 0.038939 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3291 3292 -0.022554 -0.989052 -1.381845 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3292 3293 1.010690 0.000425 -0.019391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3293 3294 0.998470 -0.008427 0.074623 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3294 3295 0.991732 0.018742 -0.169721 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3295 3296 0.003514 0.999713 1.492191 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3296 3297 1.004030 -0.021252 0.107059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3297 3298 1.038520 -0.002479 0.435853 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3298 3299 0.972708 -0.037792 -0.109815 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3299 3300 -0.994057 0.012695 2.814748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3300 3301 0.982993 0.007534 -0.247984 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3301 3302 0.998298 -0.049185 0.128134 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3302 3303 1.014460 0.025328 0.239771 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3303 3304 -0.020164 -1.004130 -1.325045 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3304 3305 0.946001 -0.020358 0.192766 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3305 3306 1.018220 -0.015113 0.050298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3306 3307 1.006600 0.020458 0.250425 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3307 3308 -0.024853 -0.994147 -1.435924 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3308 3309 0.999317 -0.000938 0.077876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3309 3310 1.004270 0.043327 0.184235 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3310 3311 1.013180 -0.013152 -0.172464 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3311 3312 -0.003540 0.997655 1.424474 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3312 3313 1.007100 -0.016869 0.030170 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3313 3314 1.013660 0.037597 -0.124113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3314 3315 0.957995 -0.019456 -0.119751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3315 3316 -1.003190 -0.019956 -2.908907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3316 3317 0.991117 -0.022885 -0.091023 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3317 3318 0.983881 -0.019107 0.139635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3318 3319 0.978531 0.013999 -0.330376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3319 3320 -0.964764 -0.012120 -2.982020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3320 3321 1.009660 -0.031282 -0.129985 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3321 3322 1.006300 -0.013102 -0.429509 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3322 3323 1.005050 0.022625 -0.128414 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3323 3324 0.974548 0.013733 -0.050715 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3324 3325 0.998534 -0.021741 0.229183 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3325 3326 1.011550 -0.026498 0.060453 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3326 3327 0.978220 0.042031 -0.080024 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3327 3328 -1.001210 0.012815 -3.130824 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3328 3329 0.983243 0.006756 -0.073153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3329 3330 1.035860 0.040321 -0.013821 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3330 3331 0.989548 0.031210 0.151113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3331 3332 -0.990839 0.000080 -3.002056 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3332 3333 0.978668 0.020714 -0.352349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3333 3334 1.030010 0.016320 0.298967 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3334 3335 0.987151 -0.005037 0.197840 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3335 3336 -0.026502 1.016620 1.545220 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3336 3337 0.971185 0.000007 -0.207434 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3337 3338 1.027990 -0.031862 -0.049375 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3338 3339 1.035150 -0.013152 0.034146 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3339 3340 0.987255 -0.046908 -0.291686 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3340 3341 0.987192 -0.007479 -0.069126 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3341 3342 0.968604 -0.036349 0.017803 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3342 3343 0.993689 0.014978 0.113854 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3343 3344 -0.027298 -0.986601 -1.378259 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3344 3345 1.017720 -0.001902 0.268542 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3345 3346 1.002580 0.007977 0.343999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3346 3347 0.992045 -0.002418 -0.170451 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3347 3348 -0.004922 0.993692 1.371851 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3348 3349 0.988995 -0.045466 0.250437 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3349 3350 0.972563 0.066375 -0.315853 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3350 3351 0.975218 0.050133 -0.142777 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3351 3352 0.007082 0.979028 1.774231 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3352 3353 0.972766 0.026418 -0.338464 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3353 3354 1.008460 -0.006790 0.140437 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3354 3355 0.982965 -0.019914 0.166604 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3355 3356 -0.004909 -0.988526 -1.810149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3356 3357 1.015810 -0.010074 0.186432 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3357 3358 1.016810 0.061353 -0.366235 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3358 3359 1.009930 -0.007788 -0.130649 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3359 3360 -0.005262 -1.006070 -1.608005 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3360 3361 1.009710 0.013225 -0.089057 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3361 3362 0.973774 0.005458 0.093326 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3362 3363 0.971329 -0.016467 0.005918 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3363 3364 -0.002464 -1.028500 -1.406398 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3364 3365 1.009260 -0.007530 -0.197821 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3365 3366 1.002070 -0.027895 -0.105893 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3366 3367 1.007310 -0.019784 0.280196 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3367 3368 -0.022902 0.992987 1.623381 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3368 3369 1.010080 -0.010873 0.083400 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3369 3370 0.998794 -0.004979 0.029608 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3370 3371 1.014560 0.015757 0.287672 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3371 3372 -0.995620 0.014788 -2.764137 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3372 3373 0.997254 0.006841 -0.256361 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3373 3374 0.993158 -0.002875 0.069647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3374 3375 0.995964 -0.020818 0.285306 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3375 3376 -0.017825 1.029740 1.668491 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3376 3377 1.002470 0.020044 -0.510008 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3377 3378 0.991185 0.012959 -0.137313 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3378 3379 1.013930 0.006322 -0.127010 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3379 3380 0.002187 1.000940 1.508250 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3380 3381 0.990160 -0.021642 0.280714 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3381 3382 0.988236 -0.009134 0.436105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3382 3383 1.027550 0.038226 0.095588 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3383 3384 -0.013870 -0.956363 -1.383500 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3384 3385 0.976551 0.012302 0.229424 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3385 3386 0.966227 -0.006554 -0.122641 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3386 3387 1.020040 -0.017772 -0.132075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3387 3388 0.006898 0.988254 1.599436 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3388 3389 1.031480 0.015841 0.097266 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3389 3390 0.972092 0.005104 -0.252420 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3390 3391 0.995755 -0.004485 0.167983 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3391 3392 -0.032529 0.984761 1.501686 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3392 3393 0.958792 -0.025786 -0.116059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3393 3394 0.980095 0.010297 0.170087 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3394 3395 0.980601 -0.009935 0.012620 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3395 3396 -1.012890 0.003801 3.087326 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3396 3397 1.021020 0.047047 -0.051390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3397 3398 0.994935 0.005898 -0.101619 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3398 3399 0.985878 0.003018 0.157904 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3399 3400 -0.023994 1.030890 1.218681 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3400 3401 0.979475 -0.030651 -0.130976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3401 3402 1.019020 -0.007660 -0.399896 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3402 3403 1.024270 -0.031548 0.156663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3403 3404 0.026544 0.965432 1.297614 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3404 3405 1.021120 0.019263 -0.172501 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3405 3406 0.967448 0.006502 -0.201125 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3406 3407 0.996180 0.004856 0.213365 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3407 3408 -0.007997 -0.970019 -1.566227 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3408 3409 0.984247 -0.021405 -0.040816 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3409 3410 1.037620 0.019758 -0.082027 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3410 3411 0.993571 0.000352 0.178416 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3411 3412 0.009931 1.015140 1.288683 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3412 3413 0.988289 0.015752 -0.066178 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3413 3414 1.015980 0.036492 0.144631 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3414 3415 0.979782 -0.009294 -0.338849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3415 3416 0.012431 -0.979725 -1.588948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3416 3417 0.992631 0.028246 -0.107510 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3417 3418 1.037560 0.049775 0.330195 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3418 3419 0.990104 0.009483 0.236886 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3419 3420 -0.989299 -0.016609 -3.114299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3420 3421 1.011220 -0.004058 0.094207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3421 3422 0.998743 -0.016821 -0.069030 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3422 3423 1.014890 0.006925 0.627718 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3423 3424 -0.026352 -0.984030 -1.642138 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3424 3425 1.009070 0.014879 -0.415182 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3425 3426 1.019190 -0.035818 -0.096309 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3426 3427 1.001610 -0.040134 -0.108303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3427 3428 -0.983389 -0.030993 2.810034 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3428 3429 1.001200 0.006919 0.071205 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3429 3430 0.992492 -0.026385 0.265127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3430 3431 0.999548 -0.025304 -0.093166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3431 3432 0.000700 0.986876 1.316500 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3432 3433 0.975706 -0.009326 -0.129435 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3433 3434 0.987059 -0.011086 0.071023 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3434 3435 1.021300 0.023348 -0.039611 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3435 3436 -1.011110 -0.025837 2.749944 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3436 3437 1.028840 0.003365 0.088023 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3437 3438 0.993046 0.033949 0.106849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3438 3439 0.988596 0.000471 -0.391533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3439 3440 -0.024381 -0.997496 -1.537376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3440 3441 0.984994 -0.010701 0.100876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3441 3442 1.039270 0.018315 0.107026 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3442 3443 0.994462 -0.019647 -0.110542 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3443 3444 -1.020820 -0.009946 -2.954583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3444 3445 0.997848 0.015083 -0.263302 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3445 3446 0.968514 0.004938 -0.434611 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3446 3447 1.012000 0.012928 0.181455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3447 3448 0.006940 -0.995464 -1.528438 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3448 3449 1.012430 0.017194 0.121746 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3449 3450 1.012860 -0.008696 -0.167362 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3450 3451 0.989045 -0.015434 0.289473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3451 3452 -0.966712 -0.016869 3.028086 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3452 3453 1.019170 0.033303 0.107960 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3453 3454 0.988966 0.045130 -0.237742 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3454 3455 1.005030 0.034708 -0.195572 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3455 3456 -0.967105 0.022517 2.947992 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3456 3457 0.975519 0.050836 -0.054644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3457 3458 0.958945 -0.015681 0.115941 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3458 3459 1.021510 0.034336 0.198297 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3459 3460 -0.010583 -1.032180 -1.368247 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3460 3461 1.015290 -0.015414 0.146719 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3461 3462 1.005570 -0.035481 0.095325 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3462 3463 0.999418 0.033903 -0.035460 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3463 3464 0.027271 -0.989602 -1.712140 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3464 3465 0.966129 -0.026717 -0.025224 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3465 3466 0.984137 -0.030826 0.087585 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3466 3467 0.969697 0.003952 -0.132202 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3467 3468 -1.025200 0.016253 -2.762090 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3468 3469 1.035110 0.001733 0.317892 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3469 3470 0.982730 -0.061135 0.169633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3470 3471 1.007350 0.005581 0.115966 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3471 3472 -1.014860 0.003771 2.966007 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3472 3473 1.022080 0.024371 -0.464717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3473 3474 0.997731 -0.014095 -0.130207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3474 3475 1.006980 -0.000038 -0.292761 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3475 3476 -0.022124 -0.969834 -1.418096 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3476 3477 1.016190 0.012519 0.277919 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3477 3478 1.010640 0.021958 0.055992 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3478 3479 1.026950 -0.000437 -0.060655 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3479 3480 -0.019308 1.011770 1.391649 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3480 3481 1.019790 -0.009418 -0.107052 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3481 3482 0.987497 0.038948 0.151847 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3482 3483 1.012170 -0.020682 -0.024133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3483 3484 -0.019261 -0.983232 -1.192589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3484 3485 1.021570 -0.022117 -0.160161 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3485 3486 0.987755 0.013787 -0.534896 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3486 3487 1.007470 -0.024479 0.049948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3487 3488 0.004909 -0.990065 -1.779713 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3488 3489 1.006360 -0.002062 0.131589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3489 3490 0.996329 0.003036 0.040431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3490 3491 1.028180 0.027322 0.353951 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3491 3492 0.002842 0.974513 1.235091 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3492 3493 1.018600 0.014913 0.108301 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3493 3494 1.030430 -0.013492 0.133722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3494 3495 1.008730 -0.020997 -0.278781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3495 3496 -0.003263 -1.000280 -1.353669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3496 3497 1.014890 0.002422 0.457630 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3497 3498 1.053930 -0.016927 0.036489 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3498 3499 0.951435 0.043095 0.237479 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 5 9 0.033943 0.032439 -3.088931 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3 10 0.044020 0.988477 -1.867035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 8 14 0.015808 0.021059 -3.040603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 7 15 -0.014728 -0.001595 -0.121744 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 21 26 -0.952140 -0.041846 3.067088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 19 27 -0.017616 -0.005218 1.664803 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 40 51 2.977430 0.032654 -3.004420 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 7 55 -0.033505 -0.006809 -1.915278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 14 56 0.003854 0.000186 -2.821930 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 8 56 -0.019655 0.006738 0.151397 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 5 57 -0.048046 -0.007535 3.072580 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 47 64 -0.992098 -0.016459 -2.967428 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 43 68 0.993623 0.039194 -0.193844 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 45 69 -0.006758 -0.006796 0.008695 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 83 101 -1.977040 -0.007033 -2.991161 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 96 102 0.009822 -0.005774 3.083644 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 79 103 -0.018687 -0.003386 1.507787 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 79 105 -0.012583 -2.006310 -1.767299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 108 113 1.003930 0.015909 -3.052691 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 109 113 0.007893 0.022383 3.071838 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 107 115 -0.004603 0.010031 1.535908 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 107 116 -1.006620 0.031373 -2.816938 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 96 118 0.030695 0.001385 -3.052209 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 102 118 0.025841 -0.019824 -0.093019 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 105 118 -0.978873 0.015246 -3.021529 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 81 121 0.021299 0.020454 -0.359793 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 82 122 0.015889 -0.005368 -0.388443 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 105 122 1.027960 0.003379 -0.151041 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 86 126 0.016650 0.001989 -0.170110 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 130 138 1.056950 -0.992927 1.506039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 137 141 -0.015647 -0.033674 -2.968987 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 136 144 0.005887 -0.029549 0.051988 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 134 145 -1.018030 -0.005965 2.750775 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 134 146 -1.991410 0.002261 3.056989 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 139 147 -0.034861 -0.029734 0.382293 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 130 147 0.968708 0.048339 1.416246 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 131 147 -0.009658 -0.049102 0.954693 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 150 159 1.007570 -0.027778 -2.005005 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 156 163 -1.024930 -0.001463 2.916365 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 154 164 -0.021340 -0.013376 -2.987297 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 156 164 0.012303 -0.013795 -0.136816 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 149 169 0.000313 -0.010342 -3.106702 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 133 171 -2.018960 -0.002866 -1.582433 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 92 172 -0.021347 -0.009751 -0.060607 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 130 172 1.001470 1.016060 1.878319 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 94 174 0.009081 0.007000 -0.304239 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 120 174 -0.997476 1.018120 -1.884062 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 78 175 0.995985 0.001641 3.101601 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 79 175 0.047171 -0.036224 -2.980274 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 119 175 -0.011971 0.022700 1.824294 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 94 176 -0.009378 -0.006867 2.704173 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 93 177 0.029985 -0.030254 -3.032907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 94 177 -0.992944 0.024181 2.962041 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 91 178 0.014004 1.027950 -1.625651 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 139 179 -0.009992 -0.001400 2.760434 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 92 179 -0.963062 -0.011130 2.994428 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 130 179 1.018600 -0.003145 -1.762084 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 145 180 1.019300 0.006052 3.028670 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 171 180 0.000054 1.026050 1.874746 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 140 181 1.038800 0.019089 0.236638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 133 181 -0.008724 -0.006345 0.114762 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 135 182 -1.050800 0.019972 -0.218965 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 134 182 -0.056552 0.047107 -0.241907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 136 182 0.011125 -0.015108 -3.084181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 128 189 -0.989149 -1.981080 1.164326 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 127 191 -0.018618 0.033680 3.129984 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 203 211 0.012884 0.002148 -1.729560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 202 212 -0.048296 -0.004273 -3.063847 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 200 213 1.011940 0.041571 2.892049 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 201 213 0.017884 0.000258 3.105819 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 199 215 0.014216 -0.005940 1.242605 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 201 216 -1.975870 -0.981812 -1.522083 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 196 220 -0.024594 -0.061977 -0.000149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 197 221 0.001136 0.023364 -0.301749 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 196 221 1.014520 -0.027180 -0.051709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 199 222 -1.004040 0.039913 0.014192 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 198 222 0.018732 0.024080 -0.140546 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 200 223 -0.988228 0.007260 1.736801 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 216 224 -0.011991 0.000445 -0.141610 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 219 226 -0.995612 -0.011419 -0.349391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 196 226 0.019514 0.031522 3.111175 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 195 227 -0.030778 0.000902 -1.781449 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 193 228 0.996745 -0.026141 2.990210 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 88 231 -1.001120 0.021493 -0.301440 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 190 232 0.019372 0.024358 -2.976434 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 188 233 1.021870 -0.004737 2.867217 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 189 234 -0.990713 -0.015648 3.104135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 188 234 -0.004968 0.012445 3.084227 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 186 236 0.004458 0.021037 -3.040625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 184 237 0.971251 -0.021497 2.959011 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 185 237 0.019916 -0.001845 -3.077720 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 183 238 0.010910 -1.046940 1.705241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 184 238 0.033911 -0.006097 3.113722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 143 239 0.001108 -0.025039 1.477511 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 135 239 0.026544 0.034611 1.660441 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 184 239 -0.981307 -0.018655 3.092620 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 236 241 1.018000 -0.037901 -2.792262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 237 242 -1.016310 -0.027431 -3.136270 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 235 243 -0.006977 -0.042663 -2.016376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 187 244 0.013253 0.976097 1.720806 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 252 258 0.016041 -0.029798 -2.993770 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 250 259 1.022390 0.000242 -0.188724 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 247 265 -0.002655 2.019930 1.456911 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 279 286 0.032356 -0.967609 1.701287 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 280 286 -0.016423 0.022912 2.929482 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 282 286 -1.970550 0.033416 3.022882 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 279 288 -0.985267 0.000961 -3.082980 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 278 289 -1.012494 0.006856 2.894936 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 276 289 0.986065 -0.009846 -3.123625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 276 290 0.022782 -0.020032 -3.127131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 277 290 -0.972550 0.038508 -2.768018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 276 292 -0.984308 1.004310 1.678200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 274 293 -0.998769 0.007025 3.030654 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 272 294 -0.003504 -0.031233 -2.960260 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 271 295 -0.006691 0.000162 -1.707318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 272 295 -1.018730 -0.021203 -2.964861 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 268 295 2.993540 -0.001041 -1.848725 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 270 295 0.995366 -0.001633 -1.349680 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 271 296 0.982399 -0.013394 0.129268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 271 297 1.987350 0.006651 0.150794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 324 328 1.979780 0.001746 3.071243 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 321 333 -0.043955 -0.046636 -2.987426 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 317 337 0.023904 -0.001727 3.130140 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 314 338 0.988263 -0.976109 1.641435 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 316 339 -1.015260 -0.017231 3.008747 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 314 339 0.972186 -0.019480 1.728374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 310 343 1.049060 -0.033210 -1.596635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 308 346 0.033858 0.002921 -2.945755 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 262 349 -0.979658 0.044801 0.092548 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 261 349 -0.028712 0.025899 -0.175318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 263 351 -0.007299 -0.006264 -0.256059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 250 354 0.009201 -0.007471 -0.208319 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 307 355 0.016016 -0.000361 2.976689 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 261 357 0.005463 0.022872 -0.224770 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 257 357 -0.007003 -0.012578 2.947067 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 253 357 0.011823 0.014891 0.002356 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 264 358 -1.003200 -0.990045 1.395743 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 257 358 -1.024290 0.014041 -2.928254 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 249 359 -2.030730 -0.016039 -3.071876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 351 360 -0.985047 -0.017186 -2.930409 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 357 361 0.002213 0.007529 -3.077208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 357 362 -0.959250 -0.019954 -3.036487 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 307 363 0.035379 -0.014909 -3.067277 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 253 365 -0.040970 0.002650 -0.199031 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 357 365 -0.016242 -0.031496 -0.127427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 353 365 -0.005924 -0.003726 3.130559 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 264 367 -1.010970 0.005725 1.518444 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 264 368 -1.975460 -0.041576 -2.987868 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 360 368 -0.978143 -0.991113 -1.637958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 263 369 -0.025636 2.029780 1.380091 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 255 369 0.029259 2.000900 1.647742 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 246 369 1.003270 -1.990310 -1.493601 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 352 370 -1.005020 -3.007530 -1.018989 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 370 374 0.985282 2.993520 1.877462 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 343 376 -0.028924 0.984516 1.677827 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 323 379 0.011167 0.036256 1.828833 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 333 380 -0.987006 -0.016189 -0.075769 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 322 380 -0.018954 0.013547 2.824255 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 333 381 -0.026370 0.047653 0.232004 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 332 381 1.024040 0.017482 0.012629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 318 383 0.993537 0.010490 1.901286 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 334 383 0.983580 -0.013428 0.249629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 337 385 -0.025618 -0.006253 0.198685 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 316 386 -0.000379 0.009521 3.080296 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 343 390 -0.962859 -0.016296 0.175247 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 312 391 -1.041640 -0.027316 3.086986 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 344 392 0.019701 -0.024825 0.048240 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 308 393 0.958961 0.030394 -3.141110 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 307 395 0.026539 -0.003537 -1.504167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 307 396 -0.975571 0.020043 -2.648480 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 301 399 1.982140 0.024036 1.564657 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 303 399 0.014051 -0.014608 1.482217 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 300 399 3.016780 0.026357 1.814637 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 303 400 1.026010 0.017202 -0.155788 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 339 403 -0.003385 -0.008583 -3.124680 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 340 404 -1.002570 -1.024710 -1.832590 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 314 404 1.036810 0.988868 1.675569 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 400 405 0.966934 0.025027 -3.062644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 401 405 -0.002499 0.029736 -3.003284 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 400 406 0.025658 0.027564 3.135032 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 397 406 1.987610 -1.008710 1.660621 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 399 407 -0.014670 -0.024863 1.417996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 302 408 1.010330 0.983108 1.224889 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 303 409 -0.016580 1.966560 1.430408 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 412 417 1.027580 -0.005463 -2.991410 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 413 417 0.006479 0.023387 -2.844923 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 412 418 0.025902 -0.023795 -2.945564 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 413 418 -1.003690 0.001786 3.026951 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 411 420 1.000230 -0.022490 0.043446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 425 429 2.014390 2.017418 1.363109 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 426 430 1.005150 3.017500 1.672639 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 440 446 -0.012974 -0.005554 -3.029060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 440 448 -1.004610 1.018750 1.243222 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 437 453 0.005577 0.050710 -0.065250 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 453 458 -1.005180 -0.011726 -2.800912 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 453 461 0.002225 0.027011 -0.390612 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 454 462 -0.000092 -0.012252 -0.105940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 438 462 -0.022174 -0.029616 -0.274849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 439 463 0.012663 -0.010816 -0.239371 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 455 463 0.052926 0.013356 -0.316275 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 446 464 0.009180 0.003776 -3.071547 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 469 473 -0.019209 -0.024285 2.943969 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 444 475 -1.001237 -0.009345 -1.551036 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 467 475 0.006740 -0.017902 1.697855 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 443 475 -0.017355 -0.013213 1.519883 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 472 478 -0.015134 0.000668 -3.032721 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 472 479 -0.991103 -0.020285 3.085074 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 469 480 0.984987 0.040278 3.075231 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 473 481 -0.001684 -0.010132 0.079648 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 447 486 -1.033630 0.006690 0.201338 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 455 486 0.003331 1.013440 -1.664521 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 446 486 0.012722 0.020413 0.390791 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 440 486 -0.016964 -0.031764 2.891880 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 439 487 0.025696 -0.022539 -2.013465 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 454 488 0.019571 0.013669 3.124955 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 448 488 0.009747 0.017729 -0.446063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 453 489 -0.002706 -0.010749 2.537015 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 450 489 -1.005820 -0.010103 0.003498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 497 501 -0.017784 0.007362 -3.120556 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 495 503 0.019737 0.028227 -0.111054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 498 505 -1.068580 -0.023265 -0.035350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 501 505 0.000798 -0.039487 -3.061854 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 458 506 1.017030 -0.993376 1.498507 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 435 507 0.004803 -0.023700 -3.104748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 451 508 0.982187 0.007212 -0.104663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 509 522 -0.999746 0.019901 3.053841 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 459 523 -0.012661 -0.008120 -2.817401 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 435 523 -0.007475 0.009549 -1.176664 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 450 523 1.023690 -0.006094 -3.099137 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 490 523 0.983135 -0.010985 -2.980984 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 512 526 -1.002210 -0.967603 1.857584 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 521 526 -1.030310 -0.009681 2.877442 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 521 528 -0.999602 0.005438 -0.417757 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 460 531 -1.014030 0.037027 0.245902 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 507 531 -0.019190 0.002844 1.594777 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 494 534 -0.011350 -0.000529 -0.314380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 501 534 1.010030 0.051645 0.148190 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 496 534 -0.030982 0.006331 -2.965504 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 503 535 -0.034482 -0.034340 0.370043 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 496 535 -1.068360 -0.024244 3.056889 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 533 537 -0.010552 -0.040291 3.084380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 494 537 -0.974870 -0.017736 2.884123 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 493 537 -0.010354 0.006845 2.617363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 433 539 1.988640 -0.012859 2.968963 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 497 540 0.976100 0.010146 -2.916767 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 497 541 0.014543 0.005256 2.969559 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 505 541 0.005946 0.016573 -3.087477 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 495 544 -0.000830 0.995550 1.290121 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 515 546 0.009322 -1.044930 1.829386 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 514 546 1.040390 -0.988948 1.802063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 515 547 0.008615 0.005698 1.399252 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 513 549 -0.013627 0.049293 2.924938 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 509 550 2.016980 -0.993588 1.781357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 510 552 0.011643 0.020712 -2.937109 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 529 553 -0.029893 -0.024232 0.204911 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 499 554 0.083845 -0.990241 1.597632 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 508 555 -0.995544 0.019893 -3.084609 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 522 556 0.013572 -0.021629 -3.068255 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 525 557 0.028115 -0.014429 -0.109622 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 522 557 -0.999279 -0.011670 -2.757259 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 528 557 1.013240 0.024794 2.923390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 550 559 1.023750 -0.020812 -1.770875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 519 560 1.002010 -0.005677 0.000999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 511 560 -0.009388 0.968929 1.569153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 552 560 -1.044950 -1.006220 -1.795822 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 566 584 0.009081 -0.009599 2.902493 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 568 584 0.046477 -0.003903 -0.173295 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 564 585 0.984490 0.013077 3.049363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 564 587 -1.012420 -0.041797 -3.137746 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 431 590 1.013850 0.006017 2.897316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 429 591 2.040970 -0.012908 2.971337 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 431 591 0.035967 -0.013266 3.012605 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 428 591 3.014053 -0.004317 2.931853 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 430 592 0.972186 1.015790 1.747640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 567 598 -0.039344 0.975338 -1.482601 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 567 599 0.022545 0.010318 -1.680162 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 563 602 1.001940 0.021335 -2.789318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 572 604 -0.030720 -0.009624 0.249866 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 574 607 0.989025 0.031518 -0.086417 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 579 611 0.025850 -0.020853 0.206620 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 578 612 0.009426 0.017093 3.063669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 577 614 -1.011410 -0.024391 3.054883 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 613 617 -0.026076 0.012648 2.976267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 612 618 -0.001647 -0.009266 -2.857521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 578 619 0.979669 -0.003279 0.316982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 619 627 0.029965 0.043901 1.777223 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 610 628 -0.031897 0.023449 3.087058 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 578 629 -0.995522 -0.025263 -2.852890 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 613 630 0.992401 -0.033880 -0.009267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 615 631 0.063646 -0.030134 0.021327 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 608 640 0.001548 -0.011526 -0.016519 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 616 641 1.016620 -0.014313 -0.048271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 609 641 -0.019654 0.025334 0.084609 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 576 644 2.004280 0.003516 -3.112199 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 627 644 0.008424 1.007994 1.372467 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 578 645 -1.010080 0.031985 -3.110973 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 640 646 0.014239 0.018026 -2.632908 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 614 646 0.036679 -0.022750 0.062128 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 607 647 0.003535 -0.015555 -1.288944 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 607 648 1.017260 0.006009 0.080805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 633 648 -0.997450 -0.016374 -0.061638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 623 655 0.041071 0.021009 1.561208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 623 656 0.014897 -0.993920 -1.867248 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 653 658 -1.034730 0.012850 3.014478 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 651 660 1.032760 0.039650 0.139234 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 665 669 -0.021003 -0.021045 -3.033162 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 664 670 0.067768 -0.013012 3.103120 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 664 671 -0.980827 0.033997 -2.567953 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 666 674 -0.002511 -0.021082 0.386929 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 667 676 -0.989834 0.017117 -3.121238 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 664 678 0.022260 -0.040517 3.104221 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 662 680 -0.026112 -0.042902 -3.010446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 658 684 0.031811 0.029043 3.054410 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 652 684 -0.009614 -0.027053 0.230866 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 656 685 1.033793 -0.014719 -3.083350 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 655 687 0.018253 0.026995 0.099073 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 622 688 0.000251 0.020107 2.938731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 621 689 -0.021750 -0.000838 -3.125051 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 619 691 -0.018373 -0.031887 1.615998 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 694 711 1.001550 0.035919 2.890955 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 693 711 2.012660 -0.024182 2.858845 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 713 717 0.006171 -0.013412 2.916023 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 713 718 -1.013030 -0.004366 -3.004109 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 710 719 0.985909 0.025175 1.716593 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 709 722 -0.981855 -0.031823 2.815942 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 707 722 0.010710 0.998126 -1.556275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 720 726 -0.024670 -0.062485 2.814992 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 710 726 -0.007337 0.045379 -0.044805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 695 727 0.013910 -0.002738 -3.087597 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 711 727 0.006007 0.003899 -0.356061 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 712 727 -0.997679 -0.011281 1.871311 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 697 729 -0.022366 -0.004741 0.019565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 697 730 1.007330 -0.032581 -0.240063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 699 731 0.000384 -0.048050 0.491571 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 700 732 0.020865 -0.011260 0.426818 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 706 738 0.009854 -0.030865 0.060393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 705 738 1.015258 -0.009245 -0.052858 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 737 742 -1.000960 0.001167 -2.973603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 703 743 0.008369 -0.052215 -1.395862 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 735 744 -1.023100 -0.002482 2.655182 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 702 744 0.032898 0.004119 2.936621 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 698 748 0.985564 1.021700 1.306228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 732 748 0.005039 -0.018893 -0.114809 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 698 749 1.036120 2.039310 1.774768 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 745 749 0.029773 0.016234 2.837115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 743 751 -0.006410 -0.008019 1.487521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 742 751 1.014190 0.014098 1.771663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 736 751 -1.006160 -0.042015 -2.199028 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 703 751 0.003096 -0.010105 0.320727 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 701 753 0.002495 0.024841 2.769796 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 733 754 -0.995960 0.017296 3.101393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 731 755 -0.026412 -0.004778 -1.611946 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 700 755 -0.987260 -0.035771 3.132876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 732 755 -1.016050 0.019656 -3.027738 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 734 763 0.978254 -4.010060 -0.288833 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 702 763 1.003810 -4.003500 0.239638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 760 764 1.989240 0.027000 3.126099 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 761 765 0.019375 0.007551 -3.095732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 655 776 1.006900 0.002828 -0.264477 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 624 776 -0.997290 -0.957569 -1.551819 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 731 779 -0.000548 0.010283 1.602937 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 731 780 0.948464 -0.016768 -0.021836 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 757 781 -0.030928 -0.042804 0.107416 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 758 782 0.004330 -0.034946 -0.280644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 760 783 -1.001561 0.012876 -2.036430 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 758 783 1.036680 0.002881 -0.006199 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 769 785 -0.017153 -0.026949 -0.192299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 770 786 -0.032637 0.015194 -0.026401 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 817 821 -0.001648 0.010308 -3.054230 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 816 821 0.994113 0.023868 2.882933 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 810 827 1.003210 -0.031120 -1.924864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 827 835 -0.013356 0.008003 2.987833 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 818 842 1.038220 1.022130 -1.739315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 820 851 -1.071580 -0.016206 -0.047933 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 818 852 0.020190 0.001069 3.094236 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 817 853 0.005550 -0.031309 -2.985904 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 827 858 -1.006050 0.021431 -0.077101 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 827 859 -0.003938 -0.009849 0.146320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 827 860 -0.990392 0.010211 3.093360 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 856 864 0.011766 -0.005502 -0.216888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 836 866 -1.000490 -0.982164 1.709112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 813 868 -1.001710 -0.023440 -0.114892 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 857 868 1.000760 0.022713 2.822950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 858 869 -1.018790 0.000845 -3.072097 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 855 871 -0.014450 0.003213 1.670749 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 881 885 0.012058 0.001203 -3.048912 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 889 904 -1.029490 0.010446 -0.158684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 894 904 0.001117 0.041681 -3.138094 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 907 923 0.027058 0.022846 2.998228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 932 938 0.030597 0.022756 -3.091097 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 921 949 0.001179 -0.034417 3.078307 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 928 951 -1.006520 0.003242 1.723166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 947 963 -0.008250 0.010410 -0.039930 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 959 967 -0.017835 0.007191 -0.130566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 961 968 -0.970797 -0.011331 0.059596 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 959 968 -1.043003 -0.008281 2.880567 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 965 969 0.035436 0.000156 3.081761 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 890 971 1.014600 -0.012291 1.318694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 888 974 0.010655 0.014183 3.023651 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 977 981 -0.051609 0.005606 -2.942981 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 975 983 0.022242 -0.024609 -3.045042 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 981 985 0.044670 -0.014918 3.027161 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 977 985 -0.012591 -0.015957 0.084925 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 842 987 1.008160 0.019159 -3.082945 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 850 988 -0.046996 0.015113 3.064921 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 847 1007 -0.018043 -0.023742 1.319053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1015 1023 0.001868 -0.017617 1.915401 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1020 1024 2.003740 -0.036179 3.098937 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1021 1025 0.015671 -0.006779 -2.814001 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1019 1026 -0.988701 -0.053514 -0.020494 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1018 1026 0.010567 0.021887 0.262654 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1032 1036 3.024490 1.027470 1.686495 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1034 1038 1.033360 3.029640 1.587999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1027 1041 1.981180 0.015541 2.842999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1037 1041 2.015490 1.999190 1.434999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1019 1044 -1.020790 -0.009833 3.044854 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1024 1044 1.996350 -0.039356 -2.915522 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1021 1044 -1.021870 0.009433 0.187685 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1017 1045 0.002770 -0.016487 -3.113820 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1021 1045 -0.017546 0.009544 -0.003284 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1024 1046 -0.027407 -0.025632 -3.051294 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1013 1050 -1.018820 -0.022003 3.017281 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1051 1059 -0.000521 -0.003432 1.783878 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1057 1061 -0.019666 0.009576 2.980279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1056 1062 -0.014273 0.006326 2.869095 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1005 1068 -0.990890 -0.013084 -0.006233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1062 1071 1.034950 -0.051550 -1.876911 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 847 1071 -0.000302 -0.049285 1.641601 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1069 1073 -0.002108 -0.019552 -2.898387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1077 1081 -0.019651 0.015640 2.832379 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 897 1085 0.024026 -0.053944 2.797563 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 880 1087 -1.007860 0.035889 -0.110794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 897 1089 0.010096 -0.031598 0.002883 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 898 1089 -0.956383 -0.061318 0.228131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1084 1093 0.986714 0.008824 0.067681 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 895 1095 0.016448 -0.016240 1.659279 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 982 1095 0.980299 -0.021796 -1.693972 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 974 1095 0.990366 -0.018713 1.614380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1089 1096 -0.983453 0.056343 0.271256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1086 1097 -1.016860 0.020569 -3.022395 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1087 1111 -0.011134 -0.021994 -1.608480 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 944 1119 -1.044400 0.004774 -1.652812 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 876 1122 -0.992887 -0.986627 2.017036 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 882 1123 1.002525 -0.048537 -1.511074 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 877 1125 -0.021025 -0.001406 -0.119342 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1113 1125 -0.027992 -0.009511 3.033615 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 983 1126 -0.004322 -0.971638 1.594096 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1112 1126 -0.031635 0.014086 -2.868268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1111 1127 -0.012630 -0.009780 -1.607057 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 903 1129 -2.007350 -0.072666 3.138763 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 902 1129 -1.020730 0.026359 -2.792403 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1098 1129 -1.028140 0.003695 0.221862 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1093 1132 -1.013910 0.007061 -0.135479 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 902 1133 -0.970775 0.008462 -0.218467 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1096 1134 -0.037481 -0.015975 -2.874941 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 887 1134 0.986744 -0.001420 -3.135876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 902 1135 0.999984 -0.015282 -0.037624 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 880 1135 -0.994727 -0.011560 -0.305765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1111 1135 0.006478 -0.009872 1.076401 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 895 1135 -0.000746 0.002196 1.415547 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 896 1137 1.000440 0.018750 0.283939 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1132 1137 0.986435 0.026172 3.062494 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1084 1139 -0.945698 0.021005 2.957283 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1132 1140 -0.024701 -0.002171 -0.048355 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1089 1141 -0.016079 0.003291 -2.981981 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1128 1142 -0.016134 0.002495 2.990380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 879 1143 -0.028623 -0.021561 2.680315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1095 1143 0.031225 -0.043769 -0.121635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1097 1144 -0.989585 -0.046071 0.234896 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1134 1144 -0.049137 0.016264 -2.892557 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1084 1146 0.023296 -0.021173 -2.921221 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1074 1147 1.025200 0.020439 1.629267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1066 1148 0.004199 0.005393 -3.094314 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1068 1149 0.997411 0.019504 -0.185413 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1069 1149 0.020761 0.063190 0.157004 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 847 1150 -0.008100 -1.007030 1.428892 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 987 1153 0.025725 -2.003190 1.635442 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 990 1153 -0.997761 -0.020090 2.871794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 848 1153 0.980276 0.002024 -0.059652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 852 1155 -1.000810 -0.013364 0.240397 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 844 1156 -0.999494 -0.974175 -1.604599 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 982 1161 -1.010580 0.016439 -3.077560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 987 1163 0.011472 -0.029695 -0.047343 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 843 1163 -0.040108 -0.008086 3.097379 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 987 1164 0.988695 0.007812 0.077618 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1164 1172 0.029985 -0.028736 -0.091262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1154 1172 1.000790 -0.971220 -1.446431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1166 1174 0.011036 -0.046500 -0.121621 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1168 1175 -0.955856 0.016113 2.934490 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 807 1175 0.045491 -0.007390 3.053787 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 811 1179 -0.021739 0.035658 0.187821 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1180 1187 -1.012720 -0.009862 -3.033774 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 836 1188 -0.019854 -0.017590 0.159594 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1178 1189 -0.979921 -0.029894 -2.714574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1192 1197 0.972197 -0.020406 3.069040 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1178 1202 0.021771 -0.011313 -0.129150 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 811 1203 -0.012733 0.011015 0.213688 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 830 1205 -0.978576 -0.021355 -0.072917 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1181 1205 -0.016920 0.006314 0.102102 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1183 1206 -1.015470 0.001259 0.174116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 832 1208 0.029627 -0.021253 0.150660 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1181 1213 -0.004519 -0.014459 -0.028442 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1183 1214 -0.990834 0.053848 0.393952 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 831 1215 -0.010878 -0.027882 -0.036742 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1195 1218 0.007239 0.986384 -1.872694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 796 1226 -0.009047 0.028873 3.023535 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 789 1233 -0.004777 0.019591 -2.959816 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 785 1234 1.986970 1.030670 -1.269431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 772 1236 -0.023354 -0.015028 -0.046335 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 772 1237 0.970780 0.008000 0.341049 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 775 1237 -2.036000 0.031032 -0.000292 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 775 1238 -0.947644 -0.021795 -0.162350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 775 1239 0.010750 0.001075 0.181481 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 774 1239 0.981261 0.029157 -0.057043 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 774 1240 -0.007580 0.016848 2.878043 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1237 1241 0.029213 0.001034 2.559248 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 786 1243 0.966733 -0.014052 1.732759 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 787 1245 2.017360 0.007072 0.142923 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 663 1247 0.037582 -0.015094 -1.779423 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 671 1248 -0.015754 0.974989 1.318842 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 651 1251 -0.014143 -0.030742 2.694825 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 651 1252 1.023290 0.013386 -0.150865 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 661 1253 -0.010955 -0.001937 -0.091323 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1249 1254 -1.050960 -0.000731 -2.896376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 679 1255 -0.006776 0.025865 -1.601513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 680 1255 -0.981029 0.024000 -2.728118 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 669 1257 -0.010148 -0.003697 2.999692 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1261 1265 0.001641 -0.005252 2.907011 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1258 1267 0.971498 0.008290 -1.643241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 667 1268 0.037411 1.004100 1.621679 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1259 1268 -0.016592 1.010240 1.585396 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1264 1270 -0.016178 -0.003134 -3.120468 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1264 1272 -0.978396 1.037390 1.701581 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1215 1279 -0.014723 0.010145 -3.044831 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 1207 1280 -0.027401 -0.956051 -1.624589 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1182 1280 1.002450 -1.032060 -1.802201 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1217 1282 1.008830 -0.006896 0.034842 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1271 1286 1.037390 -0.026758 -2.893622 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1272 1287 -0.964693 -0.006845 -1.123927 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1273 1289 -0.058998 0.021928 -0.018082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1276 1291 -0.991162 0.037842 -1.840085 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1273 1293 -0.032681 -0.005307 2.775249 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1264 1295 -0.990973 0.015239 -1.695477 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1286 1296 -0.016485 -0.004921 2.862292 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1217 1301 -0.005983 -0.032258 3.080590 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1217 1302 -1.038120 -0.039909 2.828232 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1206 1303 1.001670 -0.004560 1.667804 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1278 1303 1.037570 0.009532 -1.230167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1207 1303 -0.024936 0.008567 1.818572 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1182 1304 0.987968 -0.988998 -1.613713 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1281 1305 -0.026439 -0.017545 0.059390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1218 1306 -0.013756 0.021505 -0.082135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1282 1306 0.008403 -0.014387 -0.265242 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 804 1308 0.006634 0.008376 -0.235242 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1196 1308 -0.002412 0.022516 -0.149433 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1196 1309 0.995002 -0.028896 0.050877 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1178 1312 -1.984020 0.004553 -0.358520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 867 1315 -0.008877 -0.018438 1.435167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 836 1316 -0.022507 -0.031761 0.052229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1167 1319 -0.003555 -0.012873 -1.460913 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1309 1321 -0.007702 -0.006171 -3.007583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 805 1322 -0.984224 0.069062 -2.743418 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 803 1323 0.007683 0.028752 -1.694531 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1196 1323 -0.968214 0.019022 -3.062907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1197 1326 1.049530 0.021102 -0.315106 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 806 1327 1.021090 -0.018480 0.159914 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1313 1329 0.024314 -0.003533 -0.101414 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1177 1330 1.004640 -0.010983 0.145663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1316 1330 0.000953 -0.000381 -2.576110 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 835 1331 -0.000224 -0.026200 -1.739050 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1187 1332 1.034440 -0.011640 -0.044298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 868 1332 -0.045684 0.052453 0.364825 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 814 1333 -0.942263 0.008167 0.304925 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 866 1333 -0.978336 -0.018795 -3.068996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 855 1335 -0.011241 -0.024757 1.508303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 871 1335 0.020562 -0.033361 -0.122888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 815 1336 0.011094 1.048210 1.382973 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 854 1337 -0.988367 0.006024 2.963348 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 817 1337 0.026266 0.018801 0.058978 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1155 1338 1.031210 0.029572 -3.070908 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 822 1341 -0.994650 -0.004765 0.306815 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 822 1342 -0.012065 0.022868 -0.007170 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 855 1342 -0.992565 0.015256 0.083432 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 870 1342 1.044060 1.013210 -1.723734 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 856 1343 -0.997516 0.021188 1.589546 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1336 1343 -0.992432 0.009429 2.924406 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 855 1343 0.024004 -0.015934 0.158394 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 874 1345 -0.993089 -0.000893 -0.321724 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 877 1349 0.015274 0.010068 0.014951 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1124 1349 0.988981 0.017990 0.148403 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 905 1353 0.020787 0.016743 0.039427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 908 1354 -1.012360 1.001390 -1.443073 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 913 1356 1.010400 0.008842 3.080370 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1106 1357 -0.993092 -0.061505 2.614828 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1103 1360 -0.970847 -0.021487 3.042361 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1003 1363 0.016821 0.031104 -1.486861 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1147 1363 0.003309 -0.031305 1.256025 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1002 1365 -1.016180 0.001363 2.971613 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1360 1376 0.015137 -0.012542 0.305691 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1146 1379 0.979439 -0.010972 1.541795 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1091 1380 -0.004020 -0.996511 -1.584442 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1363 1380 -1.000390 -0.048948 2.857358 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1362 1380 -0.032901 -0.028841 -3.139168 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 908 1386 -0.001703 -0.014144 -2.950411 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 970 1387 0.985618 0.014480 2.812363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 953 1390 -1.013930 0.010045 3.079714 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 919 1390 -0.972105 0.016994 -0.083811 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 927 1391 -0.003119 0.025694 0.072016 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 971 1395 -0.008339 0.002661 1.857743 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 953 1397 0.002788 -0.023497 -2.825662 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 919 1399 -0.000666 0.014170 0.116958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 928 1400 -0.997991 -0.986739 -1.233241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 954 1403 1.007710 0.025757 -0.113389 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1401 1405 -0.008950 -0.000108 3.096498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 949 1405 -0.034988 0.000712 -0.166840 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 919 1407 -0.015707 0.036337 0.443246 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1409 1413 -0.000067 0.004377 2.790060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1391 1414 0.006505 1.001870 -1.477784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1401 1415 -2.032420 -0.047270 1.506159 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 914 1419 1.020620 0.021201 -1.462659 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1388 1419 -0.971843 0.027535 2.802427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1106 1419 0.993448 0.009715 -1.932757 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1396 1420 -0.009565 -0.018367 0.042552 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1390 1421 -0.962255 -0.038758 0.418795 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 918 1422 -0.017523 0.018860 0.083012 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 926 1424 1.009420 1.019830 1.696730 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1409 1425 0.016458 0.001345 -0.163293 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1409 1426 0.959210 0.007808 0.064263 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1412 1426 -0.041282 0.023866 3.037794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1411 1426 -0.976888 0.001810 -0.152212 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1411 1427 -0.023367 -0.014826 -0.159962 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 919 1437 2.010350 0.000567 3.047149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 928 1439 -1.015530 -0.039143 -1.427796 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 929 1440 -0.957272 -0.000966 0.411431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 938 1444 0.005560 0.007720 -3.091574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 933 1445 -0.027362 -0.009613 0.021255 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 935 1446 -0.998082 -0.038664 -0.276141 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 936 1446 -0.003381 -0.055755 -2.986990 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 935 1447 0.002132 -0.025125 0.136579 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 933 1448 0.985889 -0.005992 2.936722 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1445 1449 -0.002047 -0.006760 -2.968896 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 930 1451 1.032270 0.004585 -1.315083 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 939 1451 0.012372 0.014449 -0.100430 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 967 1455 0.040848 -0.008869 -1.595079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 959 1455 -0.005892 -0.017892 -1.261690 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 961 1457 0.000524 0.025833 -0.061017 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 946 1458 -0.008323 -0.029720 0.151409 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 963 1459 -0.007944 0.038257 -0.077369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 891 1460 -0.975070 0.018133 -3.108780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1109 1462 1.023030 0.000942 -0.441789 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 888 1463 -0.981693 0.010219 2.938235 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 880 1463 -1.009390 0.000979 -1.595627 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1111 1463 0.035318 0.011228 -0.117652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 896 1464 0.000673 0.024919 -0.376089 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 878 1464 2.064300 0.005987 0.107041 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 901 1466 -1.012950 0.003785 3.031655 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1075 1467 -0.016365 -0.014501 1.717985 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1005 1469 -0.040447 0.017503 -0.210059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1152 1470 -1.046610 1.001520 -2.045759 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1054 1470 1.002910 0.978320 -1.287189 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1149 1470 1.001490 0.041992 -0.043056 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1055 1470 -0.047496 1.007400 -1.564363 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 846 1471 1.003500 0.012958 1.469345 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1056 1472 0.027253 0.037000 -0.066201 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1058 1476 0.018769 -0.030901 -2.860648 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1011 1476 -1.006840 0.028142 -2.933320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1473 1478 -0.996022 -0.024361 3.022765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1062 1478 0.025481 0.027570 0.114327 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1470 1479 1.023730 -0.035430 1.481533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 848 1480 -0.985273 1.028870 1.531689 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1470 1480 -0.002240 -0.003483 3.054437 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 992 1481 0.980649 0.020231 -0.083298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1082 1481 -0.977920 -0.019068 0.353365 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1004 1482 -0.020949 -0.044074 3.093447 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1131 1483 0.028117 -0.036847 -1.418938 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1505 1509 0.000155 0.050299 -3.100610 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1505 1510 -0.991965 -0.032306 -3.041446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1500 1514 0.015825 -0.028283 -3.116527 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1507 1515 -0.000540 -0.006703 -0.083161 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1500 1524 -0.011590 -0.051042 -0.037298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1512 1528 0.020936 -0.007061 0.464979 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1500 1532 -0.012099 0.006710 0.022990 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1525 1533 0.041699 -0.010673 -0.113182 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1513 1533 -0.024474 -0.006472 3.086534 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1529 1533 0.005821 -0.011900 -2.795970 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1528 1534 -0.012859 0.002395 2.879439 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1542 1546 -2.032920 -0.028263 3.104672 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1538 1546 -0.001153 -0.021043 -0.180839 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1540 1549 1.008890 -0.024279 -0.318557 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1544 1549 0.965491 0.010821 -3.005640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1535 1550 0.002453 -0.980158 1.712355 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1529 1552 -0.941811 0.021549 0.146315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1507 1555 -0.007828 -0.028038 0.162817 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1508 1555 -1.012000 0.000204 3.035021 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1496 1558 0.025541 0.053880 -3.024039 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1496 1560 0.023704 -0.030335 -0.283864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1523 1562 1.006470 0.011965 2.678646 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1568 1572 1.990730 0.009975 3.112267 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1569 1573 0.025277 0.023721 -2.827533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1568 1574 -0.014423 0.025781 2.859784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1570 1574 -2.026450 -0.007319 3.050930 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1566 1577 -0.971582 -0.004600 -2.807020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1507 1578 1.003880 -0.008241 -2.678755 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1564 1578 -0.029500 -0.008489 -2.813384 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1564 1580 -0.988524 -1.021170 -1.430811 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1495 1581 0.025544 -2.032980 1.497592 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1557 1581 0.027473 -0.000452 0.115180 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1492 1586 0.008252 -0.049927 3.018298 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1489 1589 -0.027788 0.001956 -2.898103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1368 1590 -0.009307 0.018705 -3.110538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1488 1590 -0.053957 -0.030098 2.976111 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1000 1592 -0.011904 0.020104 0.010542 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1379 1594 0.048802 -0.971424 1.742156 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1084 1595 -0.972193 -0.011245 0.288034 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1076 1595 -0.983261 0.030496 1.287018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1378 1597 -0.973792 0.007344 2.989055 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1103 1598 -0.975311 -0.017606 0.054103 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1359 1599 0.046216 0.031624 -1.409676 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1373 1601 -0.012112 -0.067906 -3.092513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1372 1601 1.012910 -0.042876 3.061316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1373 1602 -1.000450 0.017167 -2.371430 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1491 1603 0.023012 -0.022073 1.230462 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1488 1606 0.019637 0.003979 -2.799887 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1592 1608 0.006513 -0.000707 -0.069696 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1466 1611 0.984007 -0.012913 2.902547 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1075 1611 0.010400 0.037361 -1.675301 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1066 1612 1.020390 0.997410 1.767764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 996 1613 0.983380 0.000636 0.153418 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1366 1613 -0.993432 0.000518 -0.385337 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1366 1614 0.020801 0.012757 0.158262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1001 1614 -0.991497 0.024104 -3.021959 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1590 1617 -0.981579 -0.033086 -2.940082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1372 1618 -0.991939 -0.986357 1.893241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1372 1619 -1.025570 -0.030801 1.095975 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1369 1621 0.017493 -0.010244 -2.878798 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1489 1622 -0.994764 -0.007718 3.000751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1615 1623 0.024768 -0.010594 1.423549 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1591 1624 0.001830 -1.014820 -1.764966 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1367 1624 1.013600 0.004116 0.081573 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1559 1631 0.035237 -0.002678 3.100112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1494 1631 1.002450 0.001731 -1.528431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1585 1633 0.011301 -0.025397 -0.135369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1493 1633 -0.005749 -0.062755 -2.785813 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1619 1634 -0.003209 0.979348 -1.559070 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1585 1637 -0.008394 -0.031025 2.938401 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1632 1639 -1.023040 -0.003523 3.137213 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1559 1639 0.026885 -0.023757 -1.599322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1630 1639 1.000750 0.008475 1.772751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1630 1640 0.001940 -0.006708 3.000273 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1627 1642 -0.011797 -0.994328 1.761108 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 1628 1642 0.025183 0.033985 -3.070565 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1629 1642 -0.978706 -0.036212 -2.774829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1626 1643 0.990189 0.000271 1.556074 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1626 1644 -0.015220 0.045989 -2.841179 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1627 1644 -1.012120 0.039252 3.077064 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1625 1645 0.033938 -0.008808 -2.954249 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1603 1651 0.013848 -0.034857 -1.795732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1619 1652 -0.025692 -0.986771 -1.683088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1603 1652 -1.003100 0.010293 -3.113694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1619 1653 0.010397 -2.022490 -1.420632 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1373 1653 0.003640 -0.009320 -0.174357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1600 1654 0.020990 0.029391 3.045638 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1376 1654 -1.021260 -0.995576 1.277374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1358 1656 1.028080 -1.008060 -1.044257 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1428 1660 0.038871 -0.017573 -0.175858 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1430 1661 -0.971578 -0.038387 -0.016446 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1430 1662 -0.005980 0.006865 -0.358215 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1433 1665 -0.009442 0.006266 -0.196859 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1433 1666 0.964713 0.001310 0.010044 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1432 1666 1.991050 -0.039276 -0.180747 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1433 1668 1.020710 0.007327 -3.058303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1432 1670 -0.016755 0.013415 -2.940059 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1433 1670 -1.015960 0.057237 3.031754 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1662 1670 1.005630 -0.973967 1.793982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1663 1671 0.016092 0.022554 1.795010 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1430 1672 -0.052025 0.019916 -2.808070 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1660 1675 -1.028090 -0.014326 -3.043845 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1567 1679 0.052961 -0.026592 -3.106531 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1491 1683 -0.030614 -0.012981 -2.755875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1602 1684 0.016507 0.045327 -2.968454 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1653 1685 0.006870 -0.000258 0.015081 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1374 1686 -0.026712 0.042490 -0.091204 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1600 1686 0.011316 -0.025326 3.097992 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1654 1687 1.006180 -0.044340 -0.063167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1131 1690 -0.050590 -1.035890 1.444850 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1484 1691 -0.964523 -0.009951 1.551112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1075 1692 -0.002661 -0.989145 -1.430212 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1094 1694 -0.010222 0.021632 -0.019191 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 887 1694 1.013720 -0.005597 2.956796 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1351 1695 0.048750 0.008216 2.981813 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1160 1696 -0.015311 -0.020983 0.062520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1351 1696 -0.015105 0.980480 1.732723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 977 1697 -0.019061 0.018685 0.152424 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 978 1698 0.015530 -0.012458 -0.159043 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1156 1698 -0.042967 -0.016443 3.069228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 851 1699 -0.011377 0.021452 -1.747619 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1170 1699 1.014280 -0.006191 3.086892 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1164 1700 -0.037099 -0.007389 -0.160636 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1169 1701 -0.012276 0.029744 2.862228 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1170 1701 -0.990542 -0.018269 -2.999544 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1173 1701 0.034222 -0.012592 -0.420579 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1168 1702 -0.028731 0.030607 3.111646 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 807 1702 1.025570 -0.012849 -2.999520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1175 1702 -0.994863 -0.000769 0.004135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 840 1703 -0.983693 -0.004853 -3.098594 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 837 1704 0.996581 0.006810 -3.064828 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1189 1705 0.042166 0.030493 -2.534735 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 860 1707 -0.996410 -0.005489 -1.704262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 837 1710 1.017470 0.025187 0.135924 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1190 1710 0.006273 0.046213 -0.230566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1703 1711 -0.019539 -0.033384 -1.381284 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1327 1711 -0.029829 0.029701 1.708082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1056 1718 -1.041180 0.988114 -1.372950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1006 1720 1.975410 0.003716 -0.090568 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1716 1722 -0.001014 0.027962 3.034717 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1024 1727 -1.040230 0.009598 0.162592 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1013 1729 -0.029393 0.000872 -3.012573 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1011 1731 0.043012 0.007343 -1.253381 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1049 1734 -1.035850 -0.028953 -2.951521 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1725 1737 -0.020751 -0.028487 -2.618635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1715 1738 1.003940 -0.007055 -2.838663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1712 1741 1.035500 -0.021092 2.834457 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 839 1742 1.014310 0.020438 2.869338 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1198 1742 0.973530 1.000020 -1.650412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1310 1743 1.017990 -0.016884 -1.838406 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 839 1743 -0.008209 -0.005486 3.071841 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1198 1744 -0.007360 0.002631 -2.905976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 805 1745 0.004753 -0.028478 -2.867714 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 804 1746 -0.004237 0.007754 3.028551 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1307 1747 0.017794 -0.000484 1.847420 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1272 1749 -1.033530 1.980510 -1.442092 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1272 1750 -1.008850 1.012100 -1.573836 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1229 1755 -2.007400 0.004205 0.167940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1226 1756 0.028176 -0.009283 -2.891276 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 803 1762 -1.014130 -0.002741 -0.069616 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 802 1762 -0.011211 0.037664 0.023420 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1299 1763 0.048254 -0.012087 -1.547526 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1284 1763 -0.950508 -0.018423 1.493535 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1325 1765 -0.039045 0.051128 0.085617 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1192 1766 0.026850 0.017396 -3.028282 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1705 1770 0.992140 0.005837 0.237299 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 810 1772 -0.019509 0.014623 -3.126002 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1709 1773 -0.032537 -0.009563 -0.107543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 840 1776 -0.008991 -0.014097 -0.049376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1742 1776 0.984850 1.019260 1.540416 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1701 1777 -0.018148 -0.007704 3.028138 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 838 1777 0.951290 -1.988010 -1.073869 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 842 1778 0.007338 0.047519 -0.073644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 818 1778 0.998344 0.985718 -1.571859 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1162 1779 1.033060 -0.012264 3.029233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 820 1779 -0.983202 -0.000695 1.603391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1339 1779 -0.014567 0.020591 -1.656701 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 848 1782 -0.020090 -0.002671 -3.118461 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1471 1784 -0.989961 0.007265 -3.085286 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1362 1786 2.009140 0.032795 3.075949 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1139 1787 0.001433 -0.008966 -1.524864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1612 1788 0.022436 0.018928 0.571163 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1001 1789 -0.019302 0.015147 -2.922147 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1484 1789 0.981733 -0.005129 0.150756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1486 1790 0.028622 -0.005728 -0.167395 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1591 1791 0.006511 0.018026 -1.461062 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1614 1792 -0.024319 0.021626 -2.927048 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1488 1792 -0.992041 -0.998031 -2.024344 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1000 1792 0.006321 -0.014589 0.091091 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 997 1793 -0.016293 0.008335 -2.658786 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1003 1795 0.009315 0.016001 -0.175629 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1129 1795 2.019390 -0.027331 2.937017 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1365 1798 0.977791 -0.017884 0.271119 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1614 1798 -0.018596 -0.005947 -0.342599 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1487 1799 -0.013280 -0.007277 -0.086856 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1591 1799 -0.000208 -0.001249 -1.520402 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1052 1804 -0.000634 0.007816 -0.068406 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1053 1804 -1.015010 0.015333 -0.016009 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1008 1805 0.969076 0.025093 -3.111315 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1716 1810 0.018755 -0.030424 3.042718 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1722 1810 -0.016904 0.023981 -0.002907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1737 1810 1.996260 1.030150 -1.806696 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1716 1811 -0.977985 0.017115 2.687920 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 799 1814 1.002580 0.011122 -2.912717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1759 1815 0.038160 0.001372 2.729761 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1816 1821 0.996607 -0.019422 3.017499 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1758 1822 1.039130 1.023740 -1.249996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1758 1824 -0.001172 -0.039715 2.685083 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1224 1825 0.971458 0.035688 0.032625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1822 1830 1.026290 -0.998340 1.727250 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1223 1831 -0.043423 -0.017946 -1.242744 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1819 1834 -0.999430 -0.033842 -0.003715 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1819 1835 -0.010830 -0.004956 -0.012113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1264 1846 -0.980093 -1.029550 1.154472 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1271 1846 0.006543 0.984734 -1.522313 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1296 1847 -0.980522 0.047472 -1.734997 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1265 1849 0.007324 0.030923 -0.169971 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 667 1851 0.017787 0.033783 -1.520658 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 666 1851 1.000860 -0.006682 -1.613819 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1267 1851 0.014751 -0.031739 -0.133310 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1259 1852 0.987388 0.032652 -0.216412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1856 1861 1.001410 0.018451 -3.101241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1857 1861 0.007755 0.019954 2.953910 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1855 1864 -0.980483 -0.000596 3.005600 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1859 1864 -4.047550 -0.989426 -1.458430 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1853 1864 1.009570 -0.050749 -2.939799 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1854 1864 0.017373 -0.000680 3.048692 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1856 1864 -0.982531 -0.985014 -1.566778 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1852 1865 0.998403 0.000784 3.092807 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1853 1866 -1.012020 -0.054794 3.121047 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1851 1868 1.010120 -0.041438 -0.208496 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1259 1869 0.025435 -2.001580 -1.754617 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 659 1873 2.006550 0.051249 3.050871 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 634 1874 0.976091 -0.987714 1.881122 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 683 1875 -0.003206 0.009378 -1.719856 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 637 1877 -0.015217 -0.005695 -0.091387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 633 1877 0.004996 0.008405 -2.876661 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 608 1878 -0.991811 -1.001350 1.400317 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 615 1879 0.023068 0.034923 -1.439725 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 648 1886 -1.014960 -1.045130 1.404233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 615 1888 -0.043234 -0.979152 -1.584862 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 563 1891 -0.018918 0.002471 1.621303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 562 1891 0.976611 -0.000758 1.659359 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 603 1892 -1.005620 0.007646 3.119605 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 567 1894 -1.029700 -0.026622 -0.124872 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 599 1895 -0.009313 0.022371 1.479171 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 581 1896 0.995625 -0.045115 -2.929635 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 580 1897 1.037560 0.042838 -2.793288 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 581 1897 0.011062 0.012406 -3.120889 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 580 1898 0.007767 -0.000761 -2.904538 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 693 1899 -2.027630 0.019342 -1.778129 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 567 1904 1.006840 0.027757 0.045595 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 582 1904 0.992929 -1.006630 -1.328929 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 714 1907 0.983808 -0.015309 -1.550754 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 716 1907 -1.015100 -0.004468 1.564110 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 714 1908 2.021470 -0.021243 -0.246146 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 415 1909 2.037950 -0.014452 -2.752443 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 414 1911 1.009303 0.021084 -3.123446 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 1909 1913 2.001330 2.000770 1.697945 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 595 1914 0.986292 0.004597 -2.777341 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 594 1914 2.011210 0.039655 3.105619 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 594 1915 0.956900 -0.009452 3.044766 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 595 1916 0.027690 -0.969365 -1.595956 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 596 1916 -0.026175 -0.039141 -0.341886 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 597 1917 -0.013807 0.001487 -0.213473 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 598 1917 -1.013653 0.012266 0.047593 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 600 1919 -0.981792 0.016899 1.544339 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 598 1919 0.988610 0.028227 0.318119 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 563 1923 -0.021495 -0.018837 -2.995303 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 572 1924 -0.030230 0.020506 0.025444 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1889 1926 -1.001200 0.022634 -3.028805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 607 1926 -0.994124 -0.052017 0.063675 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 638 1927 0.973856 0.037990 -3.089009 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1887 1927 0.007841 0.015599 -1.574018 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 639 1927 -0.004398 0.009334 2.942292 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 647 1928 0.986739 0.013357 0.143867 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1886 1928 0.003117 -0.016517 -3.063957 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1881 1928 -0.956658 -0.001364 -0.234112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1884 1929 0.976082 -0.019474 3.026240 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1881 1929 0.027876 -0.006818 -0.066393 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1884 1931 -0.997972 -0.004057 2.942486 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 519 1935 -0.020380 0.008875 1.624585 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 550 1935 1.060820 0.030482 1.585342 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 559 1936 1.005770 -0.029426 -0.215056 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1933 1937 0.037638 -0.000073 -2.796192 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1932 1937 1.001354 0.007475 2.750356 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1932 1938 -0.020102 -0.022084 3.026463 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1883 1938 -0.001480 -0.999908 1.727107 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1883 1939 0.050230 0.005542 1.924135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1871 1941 1.974070 0.002981 2.944819 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1872 1942 -1.009578 1.014417 -1.687703 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1871 1942 1.031780 -0.002135 -2.773280 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1868 1946 -0.035733 -0.018844 -3.061054 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 675 1947 0.048119 0.008014 1.381821 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1267 1947 0.046182 -0.020423 -3.083454 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1259 1948 -1.021840 -0.011865 2.832670 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 674 1948 -0.002307 0.008866 2.848049 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1867 1948 0.960130 -0.004011 0.096441 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 662 1949 1.040570 -1.989190 1.171999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 664 1950 -0.017560 0.000987 3.082596 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 673 1950 -1.007390 -0.008573 -2.871133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 680 1951 -1.015550 -0.012440 -1.756751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 671 1952 -0.040135 -1.067720 -1.111398 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1754 1953 1.023490 1.960920 -1.336933 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1227 1953 2.038220 -0.006269 2.828178 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 679 1953 -0.044334 -2.003110 -1.378113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1828 1955 -1.008380 0.026738 -0.331549 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1838 1959 0.960079 0.022115 -1.798646 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 789 1961 -0.028623 0.026898 -2.784547 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 790 1966 -0.040145 0.007678 0.196528 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 789 1966 1.010740 0.048194 0.351460 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1839 1967 -0.003271 -0.018728 -2.882932 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1827 1971 -0.037287 0.048466 1.630545 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1756 1971 -1.000480 0.001534 -1.267820 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1955 1971 0.020325 0.005030 -1.954811 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1226 1971 0.940694 0.002506 1.650511 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1228 1972 -0.011532 -0.032134 -0.094865 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1841 1973 -0.027176 0.000495 -3.116181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 793 1973 0.010023 -0.024635 -2.914319 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1838 1975 0.993292 0.007587 -1.207692 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1232 1976 -0.051791 -0.021885 -0.097744 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 771 1978 -0.019719 1.014230 -1.764843 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 771 1979 -0.000206 0.003485 -1.602629 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 770 1980 0.058967 0.020185 -3.006845 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1242 1980 1.013800 0.969365 1.525785 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 767 1983 0.006000 0.036361 -3.104451 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 766 1984 1.024110 0.980924 1.736314 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1835 1991 3.956960 -0.096783 -1.687319 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1988 1993 0.993837 0.011343 -2.963335 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1988 1994 0.035207 0.015742 -3.016082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1989 1994 -0.970148 -0.027328 3.069607 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1990 1994 -1.996790 -0.020248 -2.839786 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1987 1994 1.001550 0.000319 2.778374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1988 1996 -0.971028 -0.973356 -1.280513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1987 1996 -0.052882 -1.019580 -1.560994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1230 2000 0.021388 0.014343 2.891810 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1755 2002 0.992975 0.000187 -3.069583 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1955 2003 0.004573 -0.006991 -1.815159 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1968 2005 1.010680 -0.026294 -2.673915 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1230 2005 -0.991952 0.017318 0.144125 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2000 2006 -0.020501 0.003673 2.740731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1840 2006 0.020880 0.017827 3.051711 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 790 2007 1.021550 -0.033538 1.550217 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 794 2010 -0.017384 0.031931 -0.078010 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1970 2010 0.001508 -0.005808 0.088076 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1968 2014 -0.008449 0.004160 2.770941 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 792 2014 -0.019009 -0.027456 -2.948268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1837 2016 1.001800 -0.001429 2.844492 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1820 2020 -0.009990 -0.012339 -0.216556 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1224 2022 -0.980207 -1.027030 1.428351 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 799 2022 -0.016362 1.025830 -1.046064 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1831 2023 -0.035729 0.006079 -1.677085 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1759 2023 0.032661 0.003237 -1.309894 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1739 2026 -0.030014 -1.007380 1.369317 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1715 2028 -0.019867 1.040800 1.554116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1714 2028 0.977572 0.963022 1.833422 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1812 2028 -0.007393 0.000826 -0.103647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1823 2030 0.026544 1.000880 -1.425695 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2023 2030 0.015186 1.021570 -1.432177 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1759 2030 0.988558 -0.010733 -3.005177 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1832 2030 -0.982808 -0.995503 1.366884 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1831 2031 0.029073 -0.031806 3.075670 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1820 2035 -1.000310 -0.018609 -2.995164 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2016 2038 0.020347 0.010353 3.045652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2007 2039 -0.010566 0.004154 1.479201 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1974 2039 1.022580 0.010607 1.998684 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 792 2039 -0.975663 0.006343 -1.406875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1958 2039 1.056480 0.025081 1.644088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1837 2040 0.998594 -0.004310 2.922515 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1838 2041 -1.021900 -0.018406 -3.034380 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1837 2041 -0.035368 -0.037845 2.816644 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1836 2043 -1.005910 0.010857 -2.993642 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1836 2044 -1.013990 0.961023 1.694505 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2034 2044 -0.012104 -0.011055 -3.104571 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1818 2045 -0.984814 0.026111 -2.845615 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2022 2047 0.992697 0.005019 -0.169536 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2024 2047 -0.999838 -0.008773 -1.209316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1815 2048 -0.015699 -0.993257 -1.574370 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2023 2048 -0.971493 0.005466 3.024461 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2044 2050 0.005256 0.015895 -2.827232 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2020 2050 -0.047345 -0.015828 2.753616 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1736 2054 -0.980549 -1.007630 1.356121 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1727 2055 -0.024263 -0.005480 -1.599161 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2053 2057 0.006377 -0.025482 -2.897136 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1835 2058 -0.035744 -1.004010 1.690216 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2035 2059 0.014187 0.011625 1.631971 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1819 2060 -0.013295 -0.979249 -1.659320 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2053 2060 -1.022890 0.000143 -0.607526 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2057 2061 -0.010022 -0.013338 2.823272 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1016 2063 -0.999666 -0.001139 -1.532431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1726 2064 0.021772 0.002078 -3.124770 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1737 2069 0.005663 0.015579 -2.873498 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2055 2070 0.019740 -0.989890 1.620888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1736 2070 0.010108 0.003604 -2.948514 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1022 2070 1.980760 -0.004949 -3.077434 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1733 2072 0.990775 -0.040962 3.074717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1734 2072 -0.021742 0.028397 -3.012322 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1729 2073 -0.000967 0.003654 0.128782 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1049 2073 -0.000849 -0.004014 -0.074321 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1050 2074 0.002857 0.032945 0.039646 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1475 2075 0.029878 -0.001665 -1.374594 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1031 2079 0.009979 0.055373 1.527238 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1030 2079 1.009520 0.011356 1.397345 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1044 2082 -1.031240 0.982312 -1.173972 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1041 2083 1.982530 -0.017594 -1.703284 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1028 2083 -0.982685 -0.032865 -2.566192 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1028 2084 0.003589 0.008715 0.027941 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1030 2085 -0.999539 -0.051019 -0.134205 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2081 2085 -0.017713 -0.039781 3.017820 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1031 2086 -1.003710 0.017349 -0.167425 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2084 2089 0.984522 0.004276 -3.085259 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2082 2089 -1.010130 0.022459 -0.027708 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2084 2090 0.006448 -0.001773 3.048594 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1043 2091 -0.027313 0.016901 -1.588582 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2082 2092 -0.019945 0.021159 -2.985751 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1028 2093 0.988746 0.000541 0.195618 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1029 2093 -0.025729 0.011497 -0.114663 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2077 2097 0.032377 0.014382 2.842556 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2076 2099 -1.009020 0.035205 2.918502 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1011 2099 0.002509 -0.027450 2.933931 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 991 2101 1.990100 -0.009981 3.005115 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1055 2102 -0.991817 -0.000657 -0.236480 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1720 2102 -1.006060 -0.974040 1.449922 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1805 2102 1.008920 -0.000563 0.043822 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1054 2102 -0.014287 -0.013171 0.134187 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 847 2103 0.014441 -0.012173 3.135599 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1720 2103 -0.985811 0.015028 1.684914 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1061 2104 1.015760 0.038414 3.004829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1472 2104 0.035089 0.020479 -0.130670 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1057 2105 0.001901 -0.003796 -0.095531 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1802 2106 1.023310 1.008000 -1.766131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1011 2107 -0.014657 -0.002713 -0.103971 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2075 2107 0.020549 0.005609 1.495596 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1051 2108 -0.016169 -1.029060 -1.829036 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1805 2108 -0.978229 -0.036966 0.371581 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1476 2109 0.978129 0.016480 0.306399 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1719 2110 0.003659 0.998157 -1.283941 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1079 2110 -0.020833 -1.029280 1.765505 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1783 2111 -0.004405 0.023406 2.983748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 993 2112 -1.035330 0.010768 0.116732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1074 2115 1.020840 0.016380 0.003032 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1098 2116 0.963011 0.982869 1.698859 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1481 2117 -0.014016 0.009582 -2.835247 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1471 2119 -0.025314 -0.023398 -0.092060 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1719 2119 -0.030692 -0.007780 -2.887699 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1152 2119 -0.978982 -0.038834 -1.475391 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2110 2120 1.023630 1.001610 1.652894 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1807 2120 0.019001 1.017700 1.583245 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2112 2120 0.009490 0.001713 -0.291600 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1719 2120 0.934411 0.023671 -0.075098 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1076 2121 1.013940 -0.018180 2.985849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1469 2121 -0.008287 -0.024472 3.104876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1787 2123 -0.014550 -0.005549 0.344532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1794 2124 2.013550 -0.028764 -0.311680 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1134 2126 0.027625 -0.006017 -0.125306 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 984 2127 -0.976713 -0.000776 1.778348 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1135 2127 0.036444 0.012461 -0.252190 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 984 2129 1.061020 -0.025876 0.244825 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1171 2131 0.013997 -0.010630 -3.052732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1780 2131 -1.002270 -0.029974 1.792479 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 982 2133 -1.022520 -0.024845 0.374551 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 979 2133 -2.021720 -0.013441 -2.903974 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1135 2134 -0.012957 -1.006890 1.207482 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1463 2136 -0.976299 -0.032576 -3.089343 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1352 2136 0.021523 -0.026127 0.157978 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 972 2137 0.999856 -0.008931 2.551289 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1135 2137 -0.008707 1.993330 1.438967 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 962 2139 1.027850 0.042713 -1.619943 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 969 2139 1.964380 -0.026309 -1.580683 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 947 2140 0.989420 0.010341 0.203999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1382 2142 1.012110 -1.011040 1.482968 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1068 2147 -1.000130 -0.023585 -0.081342 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1103 2151 0.018169 0.041636 -0.055887 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1103 2152 0.025865 -1.017300 -1.630140 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 910 2152 -0.000712 0.005106 2.740019 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 913 2152 -0.985691 0.037168 0.125387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1107 2155 0.002214 0.007085 0.024901 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1402 2156 -0.018542 -0.012004 -3.094689 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 926 2158 0.013886 0.018801 -0.008444 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1407 2159 -0.000160 -0.008063 -0.111485 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1422 2159 0.977617 -0.026915 -0.011783 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 926 2160 0.010085 -0.007144 -2.803200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1416 2160 0.017048 0.015657 0.098330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 907 2162 1.006900 0.035542 3.137149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 923 2163 0.004838 0.039354 0.286812 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1356 2163 -0.981788 0.009758 1.731459 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1403 2164 -0.000509 -0.998203 -1.640102 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1459 2164 1.007720 -0.002926 -0.116855 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2142 2166 0.018794 0.020632 -0.110145 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1360 2167 -1.015490 -0.004566 -1.720703 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 908 2169 1.045560 0.030195 2.800149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2140 2169 0.977292 0.017856 3.052278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2153 2169 -0.040625 -0.004112 -0.173730 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1104 2169 1.003690 -0.015631 0.773238 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 973 2170 -1.975850 -1.020320 1.236532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1396 2171 -0.964162 0.001359 -1.657932 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1397 2172 -0.979919 -0.039979 -0.142261 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 953 2173 -0.013592 0.002680 -3.136897 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 926 2174 -0.006753 -0.015450 0.027974 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1398 2174 -0.046016 0.009202 0.239593 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2160 2174 0.013099 0.019870 -2.854397 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1414 2175 0.975627 0.012608 1.388950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1409 2177 -0.038026 -0.032824 0.054669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1425 2177 -0.029678 0.028579 -0.141023 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1426 2178 -0.009886 -0.006080 -0.145979 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1410 2179 0.996063 0.014155 0.228156 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1658 2180 0.027671 0.037775 -2.891329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2168 2181 -0.971254 2.032890 -1.587886 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2143 2182 0.008460 -1.001880 1.562141 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1103 2184 1.011250 -0.000019 -0.137190 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1657 2185 -0.010289 0.004310 0.078603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1657 2186 1.031004 0.021220 -0.211455 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1658 2186 0.029193 0.012672 -0.367633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1412 2187 -0.948609 -0.013068 1.752986 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1676 2188 -0.033829 0.008398 0.098148 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1577 2191 -1.973630 -0.005125 -0.007088 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1427 2194 1.023550 0.005585 -2.925005 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2179 2194 0.990726 0.025666 3.139192 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1659 2196 -0.042989 0.997669 1.359412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2189 2196 -0.989769 0.034106 0.250717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1677 2197 0.009754 -0.008643 -0.427692 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2192 2197 1.029620 0.009140 -3.135467 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1574 2198 1.011470 1.013850 -1.151747 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1679 2199 0.010308 0.014343 -0.301350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2190 2199 0.970799 -0.026625 -0.026432 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2191 2200 0.012586 1.022400 1.440288 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1619 2202 0.931324 0.035435 -2.824911 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1491 2202 1.012270 0.009587 3.139143 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1619 2203 -0.000030 -0.035834 -3.033106 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2200 2209 0.989312 0.035075 -0.035254 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1587 2209 0.009050 2.006930 -1.609250 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1683 2209 -2.031510 0.017061 0.413379 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2202 2210 0.012884 0.024700 0.150953 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 1682 2210 0.027735 -0.031366 0.351104 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1634 2211 1.003710 -0.023912 -1.576292 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1587 2211 0.028232 -0.002171 -1.506211 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1634 2212 -0.017978 -0.003685 -2.927408 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1584 2214 0.013162 -0.024312 -3.033997 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1495 2215 -0.010388 -0.031735 -0.158504 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1581 2217 -0.012550 -0.001886 2.966405 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1556 2217 1.005350 0.022388 -3.141266 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1562 2220 0.003903 -0.018329 -3.103804 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1580 2220 0.003655 -0.019290 0.045200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1560 2222 0.047554 0.042555 -3.076297 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1558 2224 -0.007749 -0.001892 -2.776844 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1497 2224 -1.000120 -0.021023 0.312829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1556 2226 -0.011844 0.008880 -2.895079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1565 2227 -2.025470 -0.019142 1.523317 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1555 2228 -0.048534 0.974767 1.737301 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1523 2228 -0.982840 -0.007711 2.767529 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1506 2228 1.017910 0.970605 1.670334 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1517 2229 -0.007094 -0.037405 -0.194753 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1518 2230 -0.018791 -0.029518 -0.292492 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1520 2230 0.018172 0.018870 3.049224 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1518 2231 0.998833 -0.021549 0.373165 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1545 2236 0.983896 -0.011568 -2.807345 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1503 2238 -0.020836 -1.032650 1.665188 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1535 2238 -0.014972 -0.994486 1.947185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1503 2240 0.020343 -0.996215 -1.430357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1548 2241 1.006060 0.003447 3.044147 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1549 2242 -1.004340 0.028839 3.141158 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2233 2242 1.985180 1.047830 -1.615794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1546 2242 0.032804 -0.018494 -0.008586 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2257 2262 -0.993769 -0.018196 -2.882968 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2255 2263 -0.031108 -0.002930 -1.556686 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2248 2264 -1.000100 -0.955228 -1.927515 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2269 2277 0.019649 -0.007483 0.384375 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2272 2278 -0.011904 -0.026386 -3.015405 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2239 2294 0.019513 -0.991739 1.855002 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1544 2294 -1.005630 0.962090 -1.486063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1527 2296 0.970664 -0.013707 0.045876 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2292 2300 -1.003850 -1.003860 -1.213286 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2285 2300 -0.973684 0.024589 -0.331153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2285 2301 -0.022549 0.015346 -0.140957 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2288 2301 0.987109 -0.023667 -3.111958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2288 2302 -0.007702 0.006190 2.814708 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2286 2302 -0.001593 -0.013773 0.037394 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2299 2306 -0.049568 1.031460 -1.475698 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2283 2307 -0.024297 -0.005727 1.561804 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1544 2310 -0.998762 0.965888 -1.663364 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2296 2312 0.059217 -0.009303 -0.190310 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2253 2318 0.967605 0.001211 -0.097415 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2316 2321 0.997886 0.048669 -3.028614 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2292 2322 -1.024360 1.008290 -1.483416 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2307 2324 0.007075 -1.006220 -1.780567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2313 2325 0.003355 -0.001986 -3.031064 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2310 2328 0.034545 -0.018214 3.049407 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2308 2330 -0.022677 0.016248 2.863254 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2319 2335 -0.009006 0.007122 -0.282747 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2317 2341 0.007285 -0.035176 -0.167217 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2262 2344 -0.017692 -0.019994 -3.031451 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2277 2349 0.007411 -0.058181 -0.332576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2272 2349 0.945937 -0.039742 2.906898 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2272 2360 0.023198 0.001580 -0.033708 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2269 2361 -0.011365 0.007823 2.653645 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2348 2362 -0.029691 -0.014325 3.017429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2342 2366 1.010620 1.039450 -1.171781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2264 2366 0.015381 0.009471 -3.126041 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2267 2370 -0.985855 -0.015940 0.104958 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2345 2370 1.007100 0.034338 0.302894 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2258 2370 0.014666 -0.006927 -0.629239 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2267 2372 0.037180 1.018180 1.350481 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2277 2374 1.035730 0.008995 -0.386900 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2331 2377 1.998730 -0.023033 -2.789206 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2253 2382 1.007920 0.004715 -0.167042 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2319 2383 -0.011465 -0.013864 -0.024264 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2368 2384 0.035131 -0.032908 -0.082456 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2262 2385 -0.994153 -0.006190 3.024525 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2276 2388 0.008173 -0.007236 -0.006625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2350 2388 -1.987120 -0.021922 0.184364 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2372 2388 0.020272 0.020533 -0.028531 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2271 2392 -0.038646 -0.986068 -1.615681 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2353 2392 -0.973547 0.017002 -0.289805 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2354 2393 -0.981385 0.001947 0.161337 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2270 2399 0.977351 0.029741 2.129972 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2373 2401 -0.019967 0.020324 2.997089 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2348 2401 0.974662 -0.061033 -3.019569 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2274 2401 -0.970137 -0.014660 0.418364 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2362 2402 -0.028185 0.020642 0.304399 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2347 2403 -0.021427 0.053936 -1.458112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2346 2403 1.026270 -0.030791 -1.282970 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2259 2404 -1.030870 -0.021878 2.760784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2258 2404 -0.038098 0.061333 -3.139276 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2264 2406 -0.003616 0.017462 -2.853709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2247 2407 -0.034864 -0.030890 2.998188 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2246 2408 -0.059554 0.031885 2.922574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2245 2409 -0.006982 0.029146 2.964885 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2246 2409 -0.992440 -0.028750 2.962092 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1548 2412 0.004777 0.006783 0.033108 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1537 2412 1.008250 -0.021024 3.043992 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2242 2412 0.010623 0.008900 -3.029454 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2328 2413 -0.945884 -2.011440 1.463523 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1542 2413 -1.027850 -0.000290 0.110685 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1537 2414 -0.982370 -0.046381 3.027999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1502 2415 0.985726 0.021640 1.635882 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2311 2415 0.008478 0.037638 -1.462507 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2327 2415 -0.005447 0.037649 -1.849108 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2312 2416 0.020205 0.026861 0.143079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2294 2416 0.013503 -0.020792 -3.090127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2308 2418 0.017066 0.003126 -2.882795 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2249 2420 1.008860 0.027893 -2.952899 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2333 2420 -1.025130 0.005412 0.410016 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2322 2421 -0.989078 -0.012109 -3.014885 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2244 2425 1.005390 0.005812 -2.949652 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2410 2426 -0.027883 0.015478 -0.167823 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1539 2427 -0.010268 -0.012567 -1.308349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2234 2427 1.026870 0.032934 -2.776494 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2411 2427 0.012937 -0.031155 -0.114061 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 1547 2427 -0.004009 -0.043558 -1.613702 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2425 2430 -0.998652 -0.000349 -2.746153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2342 2430 0.978792 -0.986586 1.612252 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2336 2432 -0.010458 -0.021764 -0.445418 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2249 2433 0.006886 -0.033196 0.505907 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2308 2434 -0.973665 0.986734 -1.702780 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2283 2434 0.006233 0.969780 -1.190737 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2379 2435 0.007437 -0.010446 -1.612026 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2376 2438 -0.005506 0.051156 -3.049057 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2281 2441 0.022229 0.005175 0.342312 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2290 2442 0.940809 1.026170 -1.598411 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2291 2443 -0.003096 -0.019362 -2.050252 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2306 2443 0.985249 -0.001649 -1.571890 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2280 2446 -0.005263 0.032251 3.139107 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2439 2447 -0.026211 -0.018977 0.022657 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2359 2447 0.018295 0.018307 -3.088053 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2352 2448 -1.039080 0.984967 1.595158 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2351 2449 1.996050 0.012225 -0.063163 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2396 2457 -0.951855 -2.038320 1.420075 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2396 2458 -0.975535 -0.972501 1.257544 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2396 2459 -1.016160 0.005521 1.849390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2452 2462 2.987290 -0.996176 1.409454 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2456 2462 -0.000904 0.006725 2.880734 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2455 2464 1.030430 0.010110 0.192817 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2456 2464 -0.988902 1.025800 1.464654 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2455 2465 1.968482 0.008408 0.219486 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2466 2471 0.972918 -3.985940 -1.291904 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2468 2472 1.991260 0.005399 2.731171 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2469 2474 -1.055920 -0.011992 -3.000439 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2470 2474 -1.941870 0.000239 -2.894990 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2465 2475 2.002280 -0.006063 1.757767 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2467 2475 0.011592 0.006777 1.753605 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2468 2476 -1.005610 1.022500 1.701737 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2467 2476 0.982608 -0.007486 0.099181 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2479 2488 -0.022910 0.983625 1.802275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2484 2492 -0.014958 -0.045523 0.049831 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2489 2493 -0.005237 0.056786 -2.826888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2485 2494 0.973658 -0.039629 -0.186892 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2487 2495 0.001047 -0.007909 0.255070 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2479 2495 -0.035051 -0.008055 -1.585970 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2487 2497 -0.027184 2.013390 1.879596 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2500 2504 3.009090 -0.965979 -1.831378 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2516 2520 3.000580 -1.012790 -1.631264 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2520 2524 2.978920 -1.034160 -1.754167 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2521 2525 1.999010 -2.007880 -1.525133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2529 2533 -0.039996 0.010843 -3.011105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2530 2534 -2.007140 -0.009642 2.769281 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2526 2534 1.012790 0.979168 -1.818875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2531 2535 -3.986020 0.007247 -2.716996 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2526 2535 1.001100 0.012974 -1.387901 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2504 2539 3.003588 0.011036 -3.070559 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2506 2539 1.035750 0.017372 2.792020 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2506 2540 1.001980 1.018410 1.343131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2510 2541 -0.995278 -0.018138 0.118027 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2510 2543 0.988730 -0.029204 -0.034846 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2511 2544 -0.015186 -0.974871 -1.392466 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2512 2545 0.993470 0.038661 0.371662 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2516 2547 -1.002859 0.017899 -0.498667 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 2515 2547 0.000394 -0.015932 0.221895 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2518 2547 -3.002030 -0.010979 -0.037628 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2515 2548 0.008380 0.975292 1.405482 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2514 2549 1.039730 1.988770 1.779416 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2515 2550 0.016093 2.970360 1.338544 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2516 2551 -1.011130 4.051510 1.576694 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2512 2556 -1.017470 2.998060 -1.410819 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2543 2556 3.017890 -0.020124 -2.780777 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2551 2556 -1.012550 3.976070 -2.997440 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2543 2557 2.004630 0.006942 3.020441 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2512 2558 -1.021740 0.985076 -1.222476 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2543 2558 1.018410 -0.026155 -3.056186 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2511 2559 0.010839 0.048574 -3.042408 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2543 2559 0.045857 0.027237 -3.034520 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2560 2564 2.968080 -0.990440 -1.846396 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2562 2566 0.993102 -3.002250 -1.485469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2568 2572 2.991823 -0.985963 -1.186367 134.164079 0.000000 134.164079 23.151158 0.000000 0.000000
-EDGE2 2585 2590 -0.977317 -0.014275 -2.892553 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2583 2590 -1.003600 -0.021668 -0.058545 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2583 2591 -0.029731 -0.005500 -0.135986 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2582 2593 -1.025910 0.015274 2.742496 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2588 2595 -0.988076 0.000827 -2.959909 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2579 2596 0.982613 -0.008008 -0.152669 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2586 2597 0.991298 1.987910 1.389791 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2600 2606 0.018908 0.017495 -2.976528 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2598 2606 0.993412 1.006380 -1.472588 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2598 2607 0.996163 0.014400 -1.595591 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 19 2609 1.985500 0.020477 -2.966937 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 28 2612 -0.955087 -0.975877 -1.420601 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 19 2612 0.002627 0.971752 1.342047 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 29 2612 -2.001310 -1.013120 -1.817387 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2612 2616 2.983670 1.001490 1.511868 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 34 2616 0.985608 -2.984890 1.865325 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 35 2618 -0.001673 -1.017620 1.657965 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 36 2619 -1.064800 -0.024841 -0.204576 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 38 2619 -3.005700 0.012878 -0.071871 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 37 2620 -2.001733 1.023115 1.099913 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 34 2620 -0.011170 0.015413 3.059484 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 54 2625 -0.964070 0.002367 2.870659 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 51 2626 -0.016045 -1.036370 1.545579 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 52 2626 0.006116 -0.010217 2.929449 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 52 2627 -1.029400 0.047217 3.076374 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 66 2627 1.025560 0.037857 1.278487 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 50 2628 0.012420 -0.028549 3.138328 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 65 2630 -1.035540 -0.010688 3.128685 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 0 2631 -1.023620 0.055117 -1.608286 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 63 2631 -0.005630 -0.024850 1.368613 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 22 2638 1.006150 -1.019240 1.410664 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 23 2639 -0.036951 -0.005825 1.706373 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2641 2645 0.015156 -0.014290 -3.081285 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2652 2665 0.977377 -0.028795 2.896699 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2642 2668 -0.010844 -0.019415 3.015532 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2643 2668 -0.981452 0.006692 -3.068790 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2639 2672 0.970295 0.004720 0.022195 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2602 2674 1.024000 -1.023260 2.037166 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2602 2675 0.998882 0.037707 1.318163 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2604 2675 -1.024860 0.004259 -1.346764 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2603 2676 0.004919 -1.035390 -1.752737 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2673 2677 0.015662 0.001403 -2.911654 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2642 2681 -1.014570 0.000117 0.282133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2648 2681 1.019310 -0.002031 0.000545 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2684 2689 1.014710 -0.003670 3.119980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2653 2689 -0.024594 0.023903 -3.105755 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2683 2690 -0.007961 -0.991156 1.912781 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2666 2691 0.998141 0.025122 -0.206221 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2685 2692 -0.984455 0.006881 0.147264 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2688 2694 -0.035816 -0.006383 3.033180 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2685 2694 1.000280 0.027036 0.068029 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2687 2694 -1.013140 0.032766 -0.116081 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2661 2695 1.996640 -0.013585 1.476503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2653 2701 0.036189 -0.007503 -0.352384 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2653 2702 1.033080 0.014830 0.298229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2655 2703 -0.042886 -0.015423 -0.205503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2656 2704 0.020698 0.028289 0.178029 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2660 2705 1.032410 -0.020183 3.073691 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2661 2706 -0.998126 0.015574 -3.046161 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2658 2706 -0.008115 0.049036 0.059376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2660 2707 -1.025680 -0.003443 2.869873 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 151 2710 1.000440 0.038068 3.106716 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 159 2710 0.024018 1.052870 -1.797552 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 160 2710 -0.938596 -1.012570 1.424378 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 158 2711 1.023730 0.000867 -1.492208 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 151 2711 -0.024405 -0.011444 2.993819 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 150 2711 0.990101 -0.007391 2.936429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 158 2712 -0.005726 0.019596 2.764063 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 160 2712 -0.005374 0.024230 -0.087156 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 158 2713 -1.025920 0.013928 -2.875829 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 154 2714 -0.017657 0.009680 0.265288 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 162 2714 0.010498 -0.029195 -0.017513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 163 2715 -0.005393 0.023272 0.132634 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 155 2715 -0.010767 -0.008778 -0.059123 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 47 2718 1.004480 -0.005500 3.014509 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 47 2721 2.003970 -0.041706 0.051546 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 163 2722 1.000360 -0.012054 3.085097 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2716 2722 0.001778 0.002305 -3.016094 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2717 2722 -0.993591 0.017435 -2.973316 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2718 2724 -1.979610 0.050766 -0.069696 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2717 2725 -0.030326 0.006037 -0.142024 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2720 2726 0.008922 -0.017198 2.922300 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 72 2726 -1.004900 0.983664 -1.478454 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 71 2726 1.051330 -0.040903 -3.069650 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 60 2730 0.026860 0.025300 -3.047119 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 61 2737 0.001779 -0.015699 -2.849888 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 72 2743 -0.994278 0.018359 0.201338 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 71 2744 -0.007907 1.008150 1.414749 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2730 2746 0.005192 0.001416 0.112336 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2634 2747 0.985043 -0.004575 0.108266 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2635 2747 -0.031844 0.007623 0.194948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2635 2748 0.012230 -0.989152 -1.606095 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2688 2751 -1.042030 0.016455 -1.385962 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2739 2755 0.007495 -0.007468 1.578711 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2731 2755 0.008286 -0.003978 1.519318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 12 2756 -1.979840 -0.012390 -2.945089 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2636 2757 -0.982653 -2.047880 -1.777614 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2750 2757 -0.976303 -0.002137 -0.263688 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2695 2759 0.022554 -0.011292 1.674100 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 11 2762 1.028020 0.025775 3.068910 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 4 2763 -1.013940 0.014668 0.066188 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2755 2763 0.053661 0.011556 0.002612 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2637 2765 0.016991 0.009387 0.419109 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2764 2769 1.000680 0.003185 2.857390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3 2771 -0.023625 0.020536 2.911268 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2765 2774 0.996924 0.025603 0.362229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2638 2774 -0.017385 0.033209 -0.085553 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2769 2774 -1.005960 -0.008297 3.077619 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2649 2777 -0.003220 0.022014 0.014206 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2645 2778 -0.994625 0.004813 -3.004765 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2653 2780 -0.950820 0.003170 -0.158979 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2665 2782 -0.972502 -0.034088 3.134919 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2688 2782 0.008418 0.009993 2.846642 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2702 2782 0.022374 -0.001457 0.037918 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2694 2782 0.007018 0.019373 -0.126733 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2759 2782 -0.016564 1.008890 -1.570324 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2687 2784 -0.006609 -0.973832 -1.643190 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2752 2784 0.021073 0.017642 0.178933 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 12 2787 -1.029160 0.009293 0.357508 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3 2787 -0.033435 -0.015255 1.798632 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3 2788 0.999565 0.005766 -0.117241 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2765 2789 0.033230 -0.004835 0.154932 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2773 2790 1.028670 0.010425 -0.088509 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 24 2791 -0.998019 -0.028209 -1.573568 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2640 2791 -0.975382 0.020548 1.229603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2766 2792 -0.005532 0.027139 3.090012 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2768 2792 0.007618 0.053857 -0.111049 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2638 2793 -1.010960 -0.002319 -2.994759 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2772 2794 0.015166 -0.007661 3.057099 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 6 2798 0.020489 -0.034535 0.108999 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 15 2800 -0.024931 -0.974073 -1.354777 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 30 2801 -0.991598 -0.020991 -2.966026 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 17 2801 -0.002192 -0.003856 0.011123 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2612 2802 -0.984440 0.952441 -1.423961 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2611 2803 -0.014764 -0.004957 2.978993 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2615 2804 -4.016970 0.992687 1.399080 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 17 2805 -0.019245 0.032116 2.855567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2798 2808 -0.014094 0.020384 -3.059797 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 14 2808 -0.031563 0.015498 3.120752 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 4 2811 -0.980723 0.036967 3.029022 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3 2811 -0.016954 -0.017683 -1.532227 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2636 2811 -1.022940 0.002168 -1.708901 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2728 2813 1.035700 -0.012188 2.855571 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2736 2813 1.023240 0.012659 -2.953660 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 0 2815 -0.997051 0.025189 -3.063788 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2631 2815 0.039468 0.029969 -1.642366 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 48 2816 0.032458 0.024609 0.092553 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2628 2818 0.004813 0.002606 2.727617 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 43 2818 0.983167 0.015971 -3.051647 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2626 2819 0.994178 0.017362 -1.544411 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 43 2820 -0.008621 -1.053610 -1.592732 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 41 2820 2.005000 -1.015590 -1.483511 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 43 2821 0.003047 -2.010120 -1.222771 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 67 2822 -0.012804 2.976370 1.705874 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2818 2822 0.971832 3.034160 1.777832 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2821 2825 2.017730 -1.970210 -1.836035 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2822 2826 0.968501 -2.990720 -1.492507 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2823 2827 -0.023077 -4.028450 -1.551202 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2825 2829 1.997700 2.002120 1.557723 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2830 2834 1.044180 3.015800 1.511366 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2831 2835 -0.001309 3.999202 1.688170 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2832 2836 2.996463 -0.992767 -1.443087 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2833 2837 1.999938 -1.983000 -1.466636 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2837 2841 1.981910 2.032150 1.661555 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 99 2848 3.048030 -0.022301 -3.123111 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 114 2848 1.000860 -3.023930 1.496469 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2845 2849 1.990910 2.035330 1.511289 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 106 2851 0.983462 0.009762 3.136688 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 107 2851 0.009497 -0.002513 2.949725 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 108 2852 -0.001669 -0.010660 -0.057944 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2847 2852 -1.008280 3.978440 -3.072404 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 77 2859 -1.985890 -0.034384 1.583839 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 75 2860 1.049770 0.003571 -0.072509 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2856 2862 0.026496 -0.009913 -3.113645 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 112 2863 -0.985411 0.001288 -1.645599 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2861 2865 0.010684 -0.010867 3.060570 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2858 2866 -0.014129 0.015423 0.014917 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2858 2867 0.997951 -0.028687 0.028431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2859 2867 0.006440 0.008744 -0.065473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2859 2868 -0.018174 -0.960596 -1.559229 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 76 2868 -0.019123 -0.004137 -0.070667 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2858 2868 0.951775 -0.959815 -1.880794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 76 2869 0.977922 -0.008595 0.386611 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 94 2871 1.036630 -0.026187 3.022802 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 96 2871 -0.988599 0.010926 1.626196 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 100 2875 -1.022290 -0.004121 -3.094867 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2850 2875 0.976647 0.007622 -3.038185 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 124 2875 -1.029640 0.014627 -1.481664 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 86 2877 -1.020690 -0.023583 0.071094 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 127 2878 -1.029470 0.018112 0.075108 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 232 2879 -0.987914 0.051397 -0.034567 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 191 2880 -0.004494 0.959396 1.315429 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 196 2882 -0.996133 1.018940 -1.757802 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 194 2882 0.012853 -0.033866 -0.057603 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 193 2882 0.965776 0.007158 0.315653 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 195 2883 0.018329 -0.021106 0.144384 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 225 2884 1.046110 -0.014796 -3.018149 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 220 2885 1.015620 0.019858 0.007866 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 222 2885 -1.007960 0.039564 0.081200 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 216 2886 -0.002182 0.020490 -3.091640 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 215 2887 -0.017937 -0.011827 -1.429418 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 221 2887 2.008520 0.029331 -0.432842 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 213 2889 -0.015981 0.006586 3.017944 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 202 2889 -0.998542 0.004665 0.088072 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 202 2890 -0.015374 -0.015257 -0.098074 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 203 2891 -0.018732 -0.014097 0.069491 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 208 2894 -0.020680 -0.040739 -2.908124 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2897 2901 0.037486 -0.048527 2.951135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2895 2902 -0.007534 -1.024890 1.838178 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 327 2907 -0.028773 -3.973670 0.084811 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2900 2908 -0.000355 -0.034415 -0.073309 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2905 2909 -0.021088 -0.020521 3.129205 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2902 2911 0.991959 -0.003331 -0.459824 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 205 2913 -0.003229 0.044966 -3.002110 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 209 2913 -0.009344 0.005251 0.298910 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 210 2914 -0.001018 -0.006911 0.163735 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 206 2918 -0.005201 0.007980 -0.241661 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2902 2919 1.025320 -0.052034 -1.659176 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2902 2920 0.015617 0.028334 2.930994 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2911 2920 -0.979327 0.013309 2.861948 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2897 2921 -0.012342 -0.041760 -0.107468 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2908 2922 0.007399 -0.010688 -2.918916 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2897 2922 1.025950 -0.023184 0.035051 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2900 2922 0.033673 0.009457 3.066881 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2899 2924 0.035850 -1.022695 -1.337086 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 203 2930 1.032060 0.006277 2.973431 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 212 2931 -0.997630 -0.003064 0.152864 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2892 2931 -1.021700 0.050204 1.421415 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 211 2931 0.029068 0.008406 -1.514303 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2929 2933 0.040753 -0.039378 -2.951988 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2929 2937 0.024617 -0.037270 -0.151154 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2933 2937 0.004048 0.008653 -2.913611 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2928 2938 2.015830 -0.029276 0.256653 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2933 2938 -0.999881 -0.025362 -3.037984 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2930 2940 0.047561 -0.007023 3.085578 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2936 2942 -0.013130 0.019250 -3.093755 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2934 2942 -0.026766 0.025339 0.312369 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2926 2943 0.986387 0.002755 1.559361 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2927 2943 0.007629 -0.007321 1.297813 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2935 2944 -0.002043 -1.025690 -1.606762 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2942 2946 1.015600 -3.012090 -1.769709 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2927 2946 3.005270 0.034072 0.158891 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2951 2958 -0.003929 0.960500 -1.547708 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2950 2958 0.979305 1.022770 -1.853207 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2949 2959 1.983420 -0.002985 -1.636357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2952 2960 0.002539 -0.029643 0.286912 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2951 2960 0.004653 0.996667 1.731807 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2953 2961 0.012524 0.000884 0.101916 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2954 2962 0.011271 0.001231 -0.347112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2955 2963 0.008787 -0.033430 -0.152326 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2955 2964 -0.004873 -1.030220 -1.325503 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2968 2972 1.997310 -0.011277 -3.093849 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2969 2973 -0.012960 -0.022137 3.141213 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2968 2974 0.025458 -0.019177 -3.047969 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2970 2974 -2.009320 -0.027604 -3.106957 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2967 2975 -0.015049 -0.020031 1.367162 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2977 2981 0.025804 -0.000320 3.045452 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2967 2983 -0.031991 0.004434 2.742650 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2966 2983 1.002040 0.008790 -2.807349 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2965 2983 1.991640 0.021597 2.896783 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2975 2984 -0.020864 -1.008890 -1.684311 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2977 2984 -0.945491 -0.018789 0.071234 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2978 2986 0.013580 0.005269 -0.178837 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3002 3010 1.018070 -1.025680 1.599825 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3005 3013 -0.027617 0.046750 -0.153525 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3008 3014 -0.011092 0.007399 -3.083542 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3007 3015 -0.012668 0.006546 -0.062215 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3015 3032 1.001980 -0.010183 0.257950 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3025 3033 0.023798 0.004753 -0.166398 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3026 3035 0.938790 0.010191 0.265034 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3040 3047 -0.988460 0.033880 3.133384 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3036 3050 -0.000018 -0.007460 -2.868249 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3018 3060 -0.022324 -0.021271 3.077717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3016 3061 0.983050 0.020778 -2.790133 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3032 3062 -0.970129 -0.987825 1.606127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3031 3063 0.029287 -0.011266 -1.584745 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3006 3064 0.010322 0.006418 2.828728 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3005 3065 -0.028855 0.009832 -3.114079 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3010 3066 0.001369 -0.003149 -0.176928 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3023 3095 0.017423 0.004221 -3.016982 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3092 3097 0.977131 0.008164 -3.122321 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3093 3097 -0.028782 -0.006193 3.038082 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3091 3106 0.987192 0.025244 -2.814580 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3090 3107 0.990834 -0.009693 3.078537 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3044 3114 0.025139 -0.006542 2.956425 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3044 3115 -1.007030 -0.031591 3.128323 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3117 3123 1.986340 3.988200 1.616301 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3122 3126 0.969149 -3.002360 -1.749706 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3130 3134 0.978883 -3.026130 -1.801409 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3127 3142 0.013162 -0.991279 1.264265 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3128 3143 -1.013270 -0.027119 1.547810 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3127 3143 -0.011349 0.000407 1.391234 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3125 3143 1.989940 -0.029407 1.495275 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3124 3143 2.996740 0.026599 1.611642 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3140 3146 -0.049366 -0.001484 -2.930658 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3141 3146 -1.024050 0.001738 -2.871832 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3138 3148 1.015440 1.001230 1.529953 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3055 3152 -0.990230 0.007642 -3.022026 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3053 3152 1.013920 0.000788 2.780345 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3052 3153 0.963892 -0.009629 -3.059525 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3050 3154 1.951050 -0.031665 -3.049244 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3120 3159 -1.029803 -0.009906 -0.099791 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3119 3159 0.015164 -0.021727 1.758827 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3122 3160 -3.006710 0.996972 1.394237 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3120 3160 -0.972106 1.031470 1.681311 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3118 3160 -0.010999 -0.016407 3.078918 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3117 3161 0.030089 -0.039532 2.780843 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3116 3161 1.017680 0.025563 -2.990917 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3116 3162 0.018731 0.005646 -3.082028 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3114 3162 1.002550 -1.015140 1.294065 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3116 3163 -0.987805 -0.010547 3.109503 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3042 3163 0.993314 0.014586 1.568120 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3114 3164 0.034235 -0.007980 3.095830 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3041 3164 1.004670 0.006800 -2.975887 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3041 3166 -0.986861 0.004214 2.995757 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3048 3168 -0.013818 -0.012328 0.247361 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3051 3171 -0.007414 -0.023891 0.146611 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3048 3173 1.003590 -0.005407 2.954339 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3047 3174 -0.024316 1.002040 -1.565055 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3167 3174 0.038953 0.998947 -1.581196 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3048 3175 -1.050800 0.016191 2.996524 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3171 3178 -1.007370 0.017753 0.012184 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3037 3181 0.022479 0.010981 -0.051427 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3177 3181 -0.010849 0.053531 2.852903 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3169 3181 -0.000961 -0.039884 -2.692873 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3047 3182 -0.052512 1.010490 -1.519717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3174 3182 -0.022968 -0.008221 0.001272 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3173 3185 -0.018943 0.006783 -2.983703 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3050 3186 -0.001257 -0.017975 -0.392203 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3035 3186 0.017816 1.008570 -1.194778 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3157 3187 -2.009850 -0.010512 -2.172116 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3050 3187 1.015650 0.010763 -0.210219 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3155 3187 0.032368 -0.012964 -3.102872 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3028 3188 -0.038930 0.019512 -0.367443 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3178 3189 0.995008 -2.010420 -1.700012 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3007 3191 -0.021118 -0.001473 -2.869633 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3096 3192 -0.004636 -0.014010 0.026169 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3098 3194 -0.004196 0.000375 0.074649 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3106 3196 0.003169 -0.024971 3.130717 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3101 3197 -0.031735 0.023065 0.169472 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3038 3198 1.020660 0.992148 -1.629021 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3103 3198 -1.008600 -0.016656 -0.257300 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3102 3199 1.004770 -0.011218 0.170335 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3038 3200 0.983218 0.995358 1.400554 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3110 3200 -0.027919 0.018069 2.965330 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3167 3200 0.963062 0.006110 -0.327889 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3183 3200 0.011140 0.972561 1.482477 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3175 3200 -0.028225 0.999141 1.977612 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3110 3202 -1.983320 0.058264 -3.093269 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3196 3203 -1.035560 -0.017198 2.901814 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3097 3205 -0.020896 0.005524 3.020545 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3093 3206 1.000560 0.015970 0.001131 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3063 3207 0.007170 0.029412 -2.993921 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3029 3209 -0.009251 0.009925 2.888671 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3172 3210 -1.022710 1.019190 -1.485847 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3034 3210 -0.014326 -0.035931 0.571378 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3179 3211 -0.003624 0.017208 1.544705 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3187 3212 0.995337 0.010824 0.028710 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3052 3212 0.017350 -0.024213 -0.043170 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3055 3214 -1.004390 0.011944 -0.186046 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3151 3215 0.022296 0.026646 1.673036 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3152 3215 -0.990852 -0.000816 -3.086187 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3055 3216 -0.001585 0.994104 1.489809 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3150 3217 -1.025740 -0.009229 -3.081794 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3139 3219 0.023256 -0.010427 -1.686318 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3137 3221 -0.008971 -0.003792 3.033987 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3136 3223 -1.006180 0.000036 -3.103973 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3136 3224 0.065080 -0.006809 -0.340788 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3137 3224 -1.015730 0.013580 0.286329 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3137 3225 -0.000822 -0.022151 0.149513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3138 3227 0.992078 -0.019653 -0.051861 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3220 3227 -1.005880 0.038713 2.986439 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3140 3227 -0.994390 -0.011129 1.645853 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3149 3229 -0.019940 -0.017109 0.420007 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3216 3230 -0.009552 -0.014342 -3.060278 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3214 3231 1.005710 -0.027593 -1.869210 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3152 3232 0.031452 -0.026554 0.361092 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3054 3232 0.005306 0.002510 3.017875 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3053 3232 0.997945 -0.001094 2.980744 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3154 3233 -0.975543 0.012951 -0.078588 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3190 3237 -0.966039 0.006221 -0.099365 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3023 3239 -0.010565 -0.039264 1.503137 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3062 3239 1.015790 0.021934 1.462698 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3063 3239 -0.032374 0.021041 1.300461 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3207 3239 0.032676 -0.002245 -1.449243 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3017 3241 0.012777 0.004542 0.113324 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3059 3243 0.020475 0.005166 1.477731 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2994 3243 1.013420 -0.037151 -2.940682 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2994 3244 1.033330 0.991423 2.084494 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2998 3245 -1.003968 0.003192 -0.009624 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 2997 3246 0.986778 0.004026 -0.238113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3000 3248 0.009733 -0.006027 0.028346 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3001 3248 -0.972750 0.030673 0.287345 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3002 3249 -0.966120 -0.019777 0.321264 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3011 3251 -0.013521 -0.015243 -1.500412 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3010 3251 0.967277 0.000558 -1.596493 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3081 3253 -0.018064 0.049643 2.722659 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3081 3254 -0.978560 0.041142 -2.768412 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3080 3254 -0.053913 0.031810 -3.108262 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3079 3255 0.039905 0.006101 -1.654642 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3078 3255 1.001045 0.009543 -1.530510 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3080 3255 -0.985000 0.018498 -2.929834 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2997 3263 1.992830 0.030991 -3.129488 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3260 3265 0.975766 0.017836 -3.032113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3261 3265 0.006395 -0.012064 -2.900087 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3259 3266 0.027183 0.998334 -1.781980 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3258 3266 0.981972 0.960656 -1.668656 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3261 3267 -2.004950 0.006142 -3.124566 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3259 3268 1.018110 -0.007162 0.181383 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3260 3268 -1.009220 -1.014120 -1.615533 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3260 3269 -0.982024 -1.994270 -1.830974 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3266 3270 0.998586 2.985210 1.833706 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3259 3270 3.020730 -0.006657 -0.172574 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2987 3273 -0.014544 2.005040 -1.690493 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2988 3277 0.996535 0.006323 0.265822 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2989 3277 -0.006471 -0.002537 0.172728 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2990 3280 0.014523 -0.003085 -3.093346 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3277 3281 -0.023799 -0.013400 3.129150 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3276 3283 -0.957795 -0.008174 2.783136 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3273 3283 2.017770 0.017267 2.960570 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2987 3284 0.962216 -0.012651 0.007637 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3274 3284 1.012750 1.002840 1.567797 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3273 3284 1.993440 1.046530 1.616854 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2979 3284 0.982560 0.015196 -0.036715 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3263 3287 -0.029520 0.046339 1.501470 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 2998 3287 0.961422 -0.021989 -1.543932 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3262 3288 0.988331 0.980778 1.367543 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3249 3288 -0.963944 -0.018753 -0.028854 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3012 3289 -0.996686 -2.024750 1.379963 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3248 3289 0.987118 0.056675 0.088202 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3000 3289 1.020160 0.027430 0.020784 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3067 3290 -0.007801 1.023480 -1.844350 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3003 3291 -0.023773 -0.025123 -0.083756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3083 3291 -0.000526 0.049317 1.827459 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3004 3292 0.038275 0.027318 -0.163599 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3010 3292 0.016038 -0.008845 -2.811255 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3032 3294 -2.003470 -0.019289 0.142181 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3191 3295 0.047578 0.008740 2.911160 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3205 3297 0.002818 0.007242 -2.786390 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3092 3300 -0.012357 -0.018684 -0.391121 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3297 3301 -0.008960 0.000573 -2.933366 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3014 3303 1.020680 0.024237 -1.444901 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3030 3303 1.028920 0.002771 1.679586 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3006 3304 -0.009727 -0.028630 -2.787440 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3238 3305 3.020690 -0.009136 -0.174030 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3006 3305 -0.976324 -0.047975 -3.044642 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3292 3306 0.002791 0.010694 3.087120 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3084 3308 0.013598 -0.018127 -0.133321 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3069 3309 -0.008321 -0.001467 -0.327944 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3071 3311 0.019599 -0.004642 0.229645 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3076 3316 -0.994708 1.011810 1.673833 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3074 3317 -1.005920 -0.002685 -3.071026 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3087 3319 -0.007770 0.035686 -1.395444 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3311 3320 0.027145 1.000458 1.540250 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3073 3321 -0.018838 0.003350 0.051003 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3317 3321 0.010735 0.000628 -2.979242 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3313 3321 -0.010312 -0.003634 0.047306 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3312 3321 1.016800 0.018472 0.294912 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3074 3322 -0.040914 0.011727 -0.276612 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3075 3323 0.007302 -0.006454 0.137974 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3076 3323 -1.020910 0.016340 -1.792562 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3325 3329 0.000408 -0.026002 2.885658 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3075 3330 0.965454 0.003569 2.973159 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3315 3330 0.978579 0.024351 -3.008190 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3325 3330 -0.990014 0.027186 2.922572 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3323 3331 0.032932 0.022665 -2.910281 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3324 3332 0.001837 0.005469 0.052228 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3323 3332 0.979045 0.016864 -0.138884 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3329 3333 0.018889 0.005700 -2.903031 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3328 3334 -0.022111 0.026929 2.983513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3326 3334 0.006067 0.052464 -0.067833 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3327 3335 0.040524 0.010327 0.051579 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3327 3339 -0.028995 3.988970 1.593036 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3271 3355 0.011072 -4.006300 1.900256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3350 3355 1.005680 4.021690 1.441250 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3354 3358 1.016220 -3.039670 -1.305113 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3356 3360 3.007000 -1.035120 -1.492455 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3353 3361 0.003598 -3.991810 -3.067271 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3351 3364 3.008030 -0.006582 3.055560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3363 3367 0.038822 -3.989280 -1.363153 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3352 3367 -1.027100 -0.006491 1.626817 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3353 3367 -1.991122 0.023337 1.592163 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3365 3369 2.010080 2.024810 1.482178 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3369 3373 0.003175 0.029224 -2.956340 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3368 3374 0.013354 -0.004092 2.830090 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3369 3375 -2.018380 0.024854 2.963117 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3365 3375 1.977720 0.006191 -1.476287 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3355 3376 -3.951720 0.975272 1.679976 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3350 3377 -0.996170 -0.033531 3.108421 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3349 3377 0.004836 0.013466 2.993560 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3345 3378 2.001810 0.975390 -1.870317 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3346 3378 1.039450 1.011150 -0.846462 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3347 3379 -0.011195 -0.010712 -1.731256 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3348 3379 -1.017020 -0.007679 3.134798 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3348 3380 -1.018335 -1.013008 -1.722364 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3347 3380 1.024860 0.001338 0.050522 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3347 3381 2.008740 0.005463 -0.037821 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3378 3382 0.978103 3.012880 1.283611 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3371 3383 -0.025519 -4.014915 -0.330941 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3387 3391 0.031890 3.981140 1.429867 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3388 3392 3.022650 1.026030 1.896932 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3392 3396 2.023940 0.007535 3.014493 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3393 3397 0.001861 0.030085 -3.019711 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3392 3398 0.008989 -0.002512 -3.008608 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3388 3399 2.982340 -0.013834 -1.323184 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3390 3399 1.011153 -0.005544 -1.402955 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3391 3399 0.026188 0.001226 -1.432971 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3391 3400 1.016930 0.032600 0.115186 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3414 3423 1.011640 -0.006550 1.620970 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3425 3430 -0.990564 0.006053 -2.666628 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3418 3434 0.033170 0.015964 0.119233 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3420 3434 -0.000364 -0.006055 3.099049 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3414 3437 0.952499 -1.991030 1.486112 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3415 3438 -0.014616 -0.980767 1.880422 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3423 3438 -1.027110 -0.019608 0.163837 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3431 3438 -0.002667 0.955677 -1.681410 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3415 3439 -0.023824 0.006743 1.538493 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3429 3441 0.009141 0.024752 2.966199 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3426 3442 -0.004190 0.032259 -0.086940 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3429 3442 -0.988224 -0.036270 -3.117034 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3429 3445 0.030241 0.015364 -0.171756 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3425 3445 -0.014296 -0.008301 3.060119 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3440 3446 0.020401 -0.009372 2.950129 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3439 3454 1.000290 0.012275 -3.056232 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3448 3454 0.014237 0.019249 3.108066 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3432 3455 -0.978312 0.032874 -0.303342 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3439 3455 0.023475 -0.002436 -3.028625 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3423 3456 1.006900 0.008129 0.153734 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3448 3457 1.014220 0.019537 -0.309286 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3464 3470 -0.006609 0.018518 2.944143 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3463 3471 -0.008586 0.024989 1.429319 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3464 3471 -1.033940 -0.023548 2.573105 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3462 3471 1.006910 -0.008297 1.638130 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3464 3472 0.021041 -0.005002 0.147376 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3443 3476 -1.024310 -0.018902 -2.870127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3444 3476 0.013877 -0.002619 0.365290 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3442 3476 -0.012528 -0.053588 2.996099 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3445 3477 -0.001225 0.016374 -0.269513 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3431 3478 -1.022450 0.018633 -0.099748 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3415 3479 0.036866 0.014549 -2.865191 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3416 3480 0.007039 -0.039743 0.098655 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3437 3481 -0.010448 0.001084 3.010846 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3433 3481 0.004163 -0.020544 -0.283537 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3420 3482 0.003153 0.032379 -2.948294 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3419 3483 0.064131 -0.020825 -0.048135 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3435 3484 0.017613 -1.016570 -1.783357 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3436 3485 -1.032430 2.011510 1.629042 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3411 3487 3.993780 0.013439 -1.665790 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3409 3491 1.966110 0.010055 -3.059549 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3412 3491 -0.999288 -0.006317 1.374544 89.442719 0.000000 89.442719 21.073235 0.000000 0.000000
-EDGE2 3404 3496 -1.022640 -3.001550 1.778102 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3403 3497 2.001320 0.041642 3.005657 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3404 3498 -1.010340 -1.019740 1.545056 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3405 3498 -2.018110 -1.032670 1.509127 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3403 3499 -0.031191 0.017659 -2.944473 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
-EDGE2 3402 3499 1.022250 0.034739 3.081548 44.721360 0.000000 44.721360 16.035746 0.000000 0.000000
diff --git a/demos/map_apriltag_1.yaml b/demos/map_apriltag_1.yaml
deleted file mode 100644
index d0c6a90707ddd2d15a2f921f244f085ecb337e6f..0000000000000000000000000000000000000000
--- a/demos/map_apriltag_1.yaml
+++ /dev/null
@@ -1,42 +0,0 @@
-map name: "Example of map of Apriltag landmarks"
-
-nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error.
-
-landmarks:
-
- - id : 1 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 1
- tag width: 0.1
- position: [0, 0, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
- - id : 3 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 3
- tag width: 0.1
- position: [1, 1, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
- - id : 5 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 5
- tag width: 0.1
- position: [1, 0, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
- - id : 2 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 2
- tag width: 0.1
- position: [0, 1, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
diff --git a/demos/map_polyline_example.yaml b/demos/map_polyline_example.yaml
deleted file mode 100644
index d1cde10c538cfa7e6f5b6a4b117309e36426528d..0000000000000000000000000000000000000000
--- a/demos/map_polyline_example.yaml
+++ /dev/null
@@ -1,57 +0,0 @@
-map name: "Example of map of Polyline2D landmarks"
-
-nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error.
-
-landmarks:
-
- - id: 1
- type: "POLYLINE 2D" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- position: [1, 1]
- orientation: [1]
- position fixed: true
- orientation fixed: true
- classification: 0
- first_id: 0
- first_defined: false
- last_defined: false
- points:
- - [1, 1]
- - [1, 2]
- - [1, 3]
-
- - id: 4
- type: "POLYLINE 2D" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- position: [4, 4]
- orientation: [4]
- position fixed: true
- orientation fixed: true
- classification: 0
- first_id: -2
- first_defined: false
- last_defined: true
- points:
- - [4, 1]
- - [4, 2]
- - [4, 3]
- - [4, 4]
-
-
- - id: 6
- type: "POLYLINE 2D" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- position: [6, 6]
- orientation: [6]
- position fixed: true
- orientation fixed: true
- classification: 0
- first_id: 0
- first_defined: true
- last_defined: false
- points:
- - [6, 1]
- - [6, 2]
-
- - id: 7
- type: "AHP"
- position: [1, 2, 3, 4]
- descriptor: [1, 0, 1, 0, 1, 1, 0, 0]
-
\ No newline at end of file
diff --git a/demos/maps/map_apriltag_logitech_1234.yaml b/demos/maps/map_apriltag_logitech_1234.yaml
deleted file mode 100644
index f313d1a11a3f2b4fa239f710cbbea7372747677d..0000000000000000000000000000000000000000
--- a/demos/maps/map_apriltag_logitech_1234.yaml
+++ /dev/null
@@ -1,46 +0,0 @@
-map name: "4 tags printed on a A4 sheet vertical recorded at IRI with logitech webcam."
-
-nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error.
-
-######################
-# World frame is considered to be the top left corner of tag id 0. Reference frame is corherent with a camera
-# looking straight at the sheet with RBF convention.
-######################
-landmarks:
-
- - id : 0 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 0
- tag width: 0.055
- position: [0.0225, 0.0225, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
- - id : 1 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 1
- tag width: 0.055
- position: [0.1525, 0.0225, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
- - id : 2 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 2
- tag width: 0.055
- position: [0.0225, 0.2125, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
- - id : 3 # use same as tag id
- type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
- tag id: 3
- tag width: 0.055
- position: [0.1525, 0.2125, 0]
- orientation: [0, 0, 0] # roll pitch yaw in degrees
- position fixed: true
- orientation fixed: true
-
diff --git a/demos/processor_image_feature.yaml b/demos/processor_image_feature.yaml
deleted file mode 100644
index a48c9e4277b483a0f67e4f856f72c0076a2cc2be..0000000000000000000000000000000000000000
--- a/demos/processor_image_feature.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-processor type: "IMAGE ORB"
-processor name: "ORB feature tracker"
-
-vision_utils:
- YAML file params: processor_image_vision_utils.yaml
-
-algorithm:
- maximum new features: 40
- minimum features for new keyframe: 40
- minimum response for new features: 80 #0.0005
- time tolerance: 0.01
- distance: 20
-
-noise:
- pixel noise std: 1 # pixels
-
-draw: # Used to control drawing options
- primary draw: true
- secondary draw: true
- detection roi: true
- tracking roi: false
-
diff --git a/demos/processor_image_vision_utils.yaml b/demos/processor_image_vision_utils.yaml
deleted file mode 100644
index 028f69ba6321802009765d346f2e735c6a018990..0000000000000000000000000000000000000000
--- a/demos/processor_image_vision_utils.yaml
+++ /dev/null
@@ -1,42 +0,0 @@
-sensor:
- type: "USB_CAM"
-
-detector:
- type: "ORB"
- nfeatures: 100
- scale factor: 1.2
- nlevels: 8
- edge threshold: 8 # 16
- first level: 0
- WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
- score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
-descriptor:
- type: "ORB"
- nfeatures: 100
- scale factor: 1.2
- nlevels: 8
- edge threshold: 8 # 16
- first level: 0
- WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
- score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
-matcher:
- type: "FLANNBASED"
- match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
- roi:
- width: 20
- height: 20
- min normalized score: 0.85
-
-algorithm:
- type: "ACTIVESEARCH"
- draw results: false
- grid horiz cells: 12
- grid vert cells: 8
- separation: 10
- min features to track: 5
- max new features: 40
- min response new features: 80
\ No newline at end of file
diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml
deleted file mode 100644
index 8e8a6c8cd0b75c0366a7aa83db4f3d54e4687fa6..0000000000000000000000000000000000000000
--- a/demos/processor_imu.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-unmeasured perturbation std: 0.00001
-time tolerance: 0.0025 # Time tolerance for joining KFs
-keyframe vote:
- max time span: 2.0 # seconds
- max buffer length: 20000 # motion deltas
- dist traveled: 2.0 # meters
- angle turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: false
-
\ No newline at end of file
diff --git a/demos/processor_imu_no_vote.yaml b/demos/processor_imu_no_vote.yaml
deleted file mode 100644
index 678867b719b191b6ba980a5c712f5164a9f83e28..0000000000000000000000000000000000000000
--- a/demos/processor_imu_no_vote.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.0025 # Time tolerance for joining KFs
-unmeasured perturbation std: 0.00001
-keyframe vote:
- max time span: 999999.0 # seconds
- max buffer length: 999999 # motion deltas
- dist traveled: 999999.0 # meters
- angle turned: 999999 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: false
-
\ No newline at end of file
diff --git a/demos/processor_imu_t1.yaml b/demos/processor_imu_t1.yaml
deleted file mode 100644
index b68740d245b4922a4a095b2a0ac1b2ce5becbd52..0000000000000000000000000000000000000000
--- a/demos/processor_imu_t1.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-unmeasured perturbation std: 0.00001
-time tolerance: 0.0025 # Time tolerance for joining KFs
-keyframe vote:
- max time span: 0.9999 # seconds
- max buffer length: 10000 # motion deltas
- dist traveled: 10000 # meters
- angle turned: 10000 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: true
-
\ No newline at end of file
diff --git a/demos/processor_imu_t6.yaml b/demos/processor_imu_t6.yaml
deleted file mode 100644
index 511a917c7500abbb445c7bfb016737c881dc400a..0000000000000000000000000000000000000000
--- a/demos/processor_imu_t6.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-unmeasured perturbation std: 0.00001
-time tolerance: 0.0025 # Time tolerance for joining KFs
-keyframe vote:
- max time span: 5.9999 # seconds
- max buffer length: 10000 # motion deltas
- dist traveled: 10000 # meters
- angle turned: 10000 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: true
-
\ No newline at end of file
diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3D.yaml
deleted file mode 100644
index f501e333800ec1bbb4b7c751a32aa67a99bec74c..0000000000000000000000000000000000000000
--- a/demos/processor_odom_3D.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.01 # seconds
-keyframe vote:
- max time span: 0.2 # seconds
- max buffer length: 10 # motion deltas
- dist traveled: 0.5 # meters
- angle turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
diff --git a/demos/processor_odom_3d.yaml b/demos/processor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..db3a961a1c3619ff44daab6b963d576a8595e880
--- /dev/null
+++ b/demos/processor_odom_3d.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.01 # seconds
+unmeasured_perturbation_std: 0.001
+
+keyframe_vote:
+ voting_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
diff --git a/demos/processor_tracker_feature_trifocal.yaml b/demos/processor_tracker_feature_trifocal.yaml
deleted file mode 100644
index 67d403058fa6637a15309cb724524c1f9106778d..0000000000000000000000000000000000000000
--- a/demos/processor_tracker_feature_trifocal.yaml
+++ /dev/null
@@ -1,19 +0,0 @@
-processor type: "TRACKER FEATURE TRIFOCAL"
-processor name: "tracker feature trifocal example"
-
-vision_utils:
- YAML file params: ACTIVESEARCH.yaml
-
-algorithm:
- time tolerance: 0.01
- voting active: true
- minimum features for keyframe: 40
- maximum new features: 40
- grid horiz cells: 10
- grid vert cells: 10
- min response new features: 50
- max euclidean distance: 20
-
-noise:
- pixel noise std: 1 # pixels
-
diff --git a/demos/processor_tracker_landmark_apriltag.yaml b/demos/processor_tracker_landmark_apriltag.yaml
deleted file mode 100644
index e8b1fd02dc80ffedefafc44ae3af9898a873cb3b..0000000000000000000000000000000000000000
--- a/demos/processor_tracker_landmark_apriltag.yaml
+++ /dev/null
@@ -1,57 +0,0 @@
-processor type: "TRACKER LANDMARK APRILTAG"
-processor name: "tracker landmark apriltag example"
-
-detector parameters:
- quad_decimate: 1.5 # doing quad detection at lower resolution to speed things up (see end of file)
- quad_sigma: 0.8 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file)
- nthreads: 8 # how many thread during tag detection (does not change much?)
- debug: false # write some debugging images
- refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff)
- ippe_min_ratio: 3.0 # quite arbitrary, always > 1 (to deactive, set at 0 for example)
- ippe_max_rep_error: 2.0 # to deactivate, set at something big (100)
-
-tag widths:
- 0: 0.055
- 1: 0.055
- 2: 0.055
- 3: 0.055
-
-tag parameters:
- tag_family: "tag36h11"
- # tag_black_border: 1
- tag_width_default: 0.165 # used if tag width not specified
-
-
-noise:
- std_xy : 0.1 # m
- std_z : 0.1 # m
- std_rpy_degrees : 5 # degrees
- std_pix: 20 # pixel error
-
-vote:
- voting active: true
- min_time_vote: 0 # s
- max_time_vote: 0 # s
- min_features_for_keyframe: 12
- max_features_diff: 17
- nb_vote_for_every_first: 50
- enough_info_necessary: true # create kf if enough information to uniquely determine the KF position (necessary for apriltag_only slam)
-
-reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor
-add_3D_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only
-
-
-# Annexes:
-### Quad decimate: the higher, the lower the resolution
-# Does nothing if <= 1.0
-# Only values taken into account:
-# 1.5, 2, 3, 4
-# 1.5 -> ~*2 speed up
-
-# Higher values use a "bad" code according to commentaries in the library, smaller do nothing
-### Gaussian blur table:
-# max sigma kernel size
-# 0.499 1 (disabled)
-# 0.999 3
-# 1.499 5
-# 1.999 7
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
deleted file mode 100644
index 66b81a5288877362753612f7aa4b9222da009e8d..0000000000000000000000000000000000000000
--- a/demos/sensor_imu.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-sensor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main IMU" # This is ignored. The name provided to the SensorFactory prevails
-motion variances:
- a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
- w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
- ab_initial_stdev: 0.800 # m/s2 - initial bias
- wb_initial_stdev: 0.350 # rad/sec - initial bias
- ab_rate_stdev: 0.1 # m/s2/sqrt(s)
- wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
\ No newline at end of file
diff --git a/demos/sensor_odom_3D.yaml b/demos/sensor_odom_3D.yaml
deleted file mode 100644
index 9ea77803548cfd9d033f54e40b6d92a72710afb8..0000000000000000000000000000000000000000
--- a/demos/sensor_odom_3D.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-motion variances:
- disp_to_disp: 0.02 # m^2 / m
- disp_to_rot: 0.02 # rad^2 / m
- rot_to_rot: 0.01 # rad^2 / rad
- min_disp_var: 0.01 # m^2
- min_rot_var: 0.01 # rad^2
diff --git a/demos/sensor_odom_3D_HQ.yaml b/demos/sensor_odom_3D_HQ.yaml
deleted file mode 100644
index 08945ef842e46f28642f1be63ca95850b618a35e..0000000000000000000000000000000000000000
--- a/demos/sensor_odom_3D_HQ.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-motion variances:
- disp_to_disp: 0.000001 # m^2 / m
- disp_to_rot: 0.000001 # rad^2 / m
- rot_to_rot: 0.000001 # rad^2 / rad
- min_disp_var: 0.00000001 # m^2
- min_rot_var: 0.00000001 # rad^2
\ No newline at end of file
diff --git a/demos/sensor_odom_3d.yaml b/demos/sensor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..58db1c088fbc80339a78ba815fddbaf69674d3b6
--- /dev/null
+++ b/demos/sensor_odom_3d.yaml
@@ -0,0 +1,7 @@
+type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+k_disp_to_disp: 0.02 # m^2 / m
+k_disp_to_rot: 0.02 # rad^2 / m
+k_rot_to_rot: 0.01 # rad^2 / rad
+min_disp_var: 0.01 # m^2
+min_rot_var: 0.01 # rad^2
diff --git a/demos/sensor_odom_3d_HQ.yaml b/demos/sensor_odom_3d_HQ.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8515eb0cb24f711b29b88196a7f764895c0b6ff6
--- /dev/null
+++ b/demos/sensor_odom_3d_HQ.yaml
@@ -0,0 +1,7 @@
+type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+k_disp_to_disp: 0.000001 # m^2 / m
+k_disp_to_rot: 0.000001 # rad^2 / m
+k_rot_to_rot: 0.000001 # rad^2 / rad
+min_disp_var: 0.00000001 # m^2
+min_rot_var: 0.00000001 # rad^2
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 9ad57098e8e3671bcf18cd54dc458fc6d776dab4..82aab6219874c3575dc14fdb24af6d2cc0e2d07a 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -35,9 +35,8 @@
//Ceres includes
#include "glog/logging.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-//laser_scan_utils
+#include "../../include/core/ceres_wrapper/solver_ceres.h"
#include "iri-algorithms/laser_scan_utils/corner_detector.h"
#include "iri-algorithms/laser_scan_utils/entities.h"
@@ -116,10 +115,10 @@ int main(int argc, char *argv[])
// INITIALIZATION ============================================================================================
//init random generators
- Scalar odom_std_factor = 0.1;
- Scalar gps_std = 10;
+ double odom_std_factor = 0.1;
+ double gps_std = 10;
std::default_random_engine generator(1);
- std::normal_distribution<Scalar> gaussian_distribution(0.0, 1);
+ std::normal_distribution<double> gaussian_distribution(0.0, 1);
// Faramotics stuff
Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
@@ -147,18 +146,18 @@ int main(int argc, char *argv[])
myScanner->loadAssimpModel(modelFileName);
//variables
- Eigen::Vector3s odom_reading;
- Eigen::Vector2s gps_fix_reading;
- Eigen::VectorXs pose_odom(3); //current odometry integred pose
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+ Eigen::Vector3d odom_reading;
+ Eigen::Vector2d gps_fix_reading;
+ Eigen::VectorXd pose_odom(3); //current odometry integred pose
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
clock_t t1, t2;
// Wolf manager initialization
- Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
- Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
- Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
+ Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero();
+ Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero();
+ Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
@@ -171,8 +170,8 @@ int main(int argc, char *argv[])
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
- WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
// Ceres initialization
ceres::Solver::Options ceres_options;
@@ -181,7 +180,7 @@ int main(int argc, char *argv[])
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
- CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblem(), ceres_options);
+ SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options);
std::ofstream log_file, landmark_file; //output file
// Own solver
@@ -242,10 +241,10 @@ int main(int argc, char *argv[])
// ADD CAPTURES ---------------------------
//std::cout << "ADD CAPTURES..." << std::endl;
// adding new sensor captures
- wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
- wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
- wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
- wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
+ wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
+ wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
+ wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
+ wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
// updating problem
@@ -291,7 +290,7 @@ int main(int argc, char *argv[])
std::vector<double> landmark_vector;
for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Scalar* position_ptr = (*landmark_it)->getP()->get();
+ double* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -346,7 +345,7 @@ int main(int argc, char *argv[])
std::vector<double> landmark_vector;
for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Scalar* position_ptr = (*landmark_it)->getP()->get();
+ double* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -364,8 +363,8 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXs state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
- for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
+ Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().size() * 3);
+ for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().end(); frame_it++)
{
if (complex_angle)
state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
@@ -377,10 +376,10 @@ int main(int argc, char *argv[])
// Landmarks
std::cout << "Landmarks..." << std::endl;
i = 0;
- Eigen::VectorXs landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
+ Eigen::VectorXd landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
+ Eigen::Map<Eigen::Vector2d> landmark((*landmark_it)->getP()->get());
landmarks.segment(i, 2) = landmark;
i += 2;
}
diff --git a/demos/vision_utils_active_search.yaml b/demos/vision_utils_active_search.yaml
deleted file mode 100644
index 4ff33b353b3921c223665c78f6d9bb8029377c78..0000000000000000000000000000000000000000
--- a/demos/vision_utils_active_search.yaml
+++ /dev/null
@@ -1,42 +0,0 @@
-sensor:
- type: "USB_CAM"
-
-detector:
- type: "ORB"
- nfeatures: 100
- scale factor: 1.2
- nlevels: 8
- edge threshold: 8 # 16
- first level: 0
- WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
- score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
-descriptor:
- type: "ORB"
- nfeatures: 100
- scale factor: 1.2
- nlevels: 8
- edge threshold: 8 # 16
- first level: 0
- WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
- score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
-matcher:
- type: "FLANNBASED"
- match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
- roi:
- width: 20
- height: 20
- min normalized score: 0.85
-
-algorithm:
- type: "ACTIVESEARCH"
- draw results: true
- grid horiz cells: 12
- grid vert cells: 8
- separation: 10
- min features to track: 5
- max new features: 40
- min response new features: 80
\ No newline at end of file
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
index dc3608694f177138711f769ceadb7ef004665ad4..f0009526c6f9fcb8a61ae84916690a62190e32c6 100644
--- a/doc/doxygen.conf
+++ b/doc/doxygen.conf
@@ -429,7 +429,7 @@ EXTRACT_PACKAGE = NO
# included in the documentation.
# The default value is: NO.
-EXTRACT_STATIC = NO
+EXTRACT_STATIC = YES
# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
# locally in source files will be included in the documentation. If set to NO
@@ -756,7 +756,8 @@ WARN_LOGFILE =
# Note: If this tag is empty the current directory is searched.
INPUT = ../doc/main.dox \
- ../src
+ ../src \
+ ../include/core
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
diff --git a/doc/interpolate.rtf b/doc/interpolate.rtf
index f64c7d77940675ec18b62d6428fe42eaaa376c58..4aea11adf1deb3f7a4e1093555ff799a6c1eb929 100644
--- a/doc/interpolate.rtf
+++ b/doc/interpolate.rtf
@@ -179,12 +179,12 @@
\cf2 *\cf0 \
\cf2 * For simple pose increments, we can use a local implementation:\cf0 \
\cf2 *\cf0 \
-\cf2 * - for 2D\cf0 \
+\cf2 * - for 2d\cf0 \
\cf2 *\cf0 \
\cf2 * dp_S = dR_I.tr * (1-tau)*dp_F // dR is the rotation matrix of the angle delta '\ul da\ulnone '; '\ul tr\ulnone ' is transposed\cf0 \
\cf2 * da_S = dR_I.tr * (1-tau)*da_F\cf0 \
\cf2 *\cf0 \
-\cf2 * - for 3D\cf0 \
+\cf2 * - for 3d\cf0 \
\cf2 *\cf0 \
\cf2 * dp_S = dq_I.conj * (1-tau)*dp_F // \ul dq\ulnone is a \ul quaternion\ulnone ; '\ul conj\ulnone ' is the \ul conjugate\ulnone \ul quaternion\ulnone .\cf0 \
\cf2 * dq_S = dq_I.conj * dq_F\cf0 \
@@ -206,14 +206,14 @@
\cf2 *\cf0 \
\cf2 * ### Examples\cf0 \
\cf2 *\cf0 \
-\cf2 * #### Example 1: For 2D poses\cf0 \
+\cf2 * #### Example 1: For 2d poses\cf0 \
\cf2 *\cf0 \
\cf2 *\cf0 \
\cf2 * t_I = _ts // time stamp of the interpolated motion\cf0 \
\cf2 * tau = (t_I - t_R) / (t_F - t_R) // interpolation factor\cf0 \
\cf2 *\cf0 \
\cf2 * dp_I = tau*dp_F // \ul dp\ulnone is a 2-vector\cf0 \
-\cf2 * da_I = tau*da_F // \ul da\ulnone is an angle, for 2D poses\cf0 \
+\cf2 * da_I = tau*da_F // \ul da\ulnone is an angle, for 2d poses\cf0 \
\cf2 *\cf0 \
\cf2 * D_I = deltaPlusDelta(D_R, d_I)\cf0 \
\cf2 *\cf0 \
@@ -222,7 +222,7 @@
\cf2 *\cf0 \
\cf2 * D_S = D_F\cf0 \
\cf2 *\cf0 \
-\cf2 * #### Example 2: For 3D poses\cf0 \
+\cf2 * #### Example 2: For 3d poses\cf0 \
\cf2 *\cf0 \
\cf2 * Orientation is in \ul quaternion\ulnone form, which is the best for interpolation using `\ul slerp\ulnone ()` :\cf0 \
\cf2 *\cf0 \
@@ -230,7 +230,7 @@
\cf2 * tau = (t_I - t_R) / (t_F - t_R) // interpolation factor\cf0 \
\cf2 *\cf0 \
\cf2 * dp_I = tau*dp_F // \ul dp\ulnone is a 3-vector\cf0 \
-\cf2 * dq_I = 1.\ul slerp\ulnone (tau, dq_F) // '1' is the identity \ul quaternion\ulnone ; \ul slerp\ulnone () interpolates 3D rotation.\cf0 \
+\cf2 * dq_I = 1.\ul slerp\ulnone (tau, dq_F) // '1' is the identity \ul quaternion\ulnone ; \ul slerp\ulnone () interpolates 3d rotation.\cf0 \
\cf2 *\cf0 \
\cf2 * D_I = deltaPlusDelta(D_R, d_I)\cf0 \
\cf2 *\cf0 \
@@ -260,4 +260,4 @@
\cf2 * DC_S = DC_F\cf0 \
\cf2 *\cf0 \
\cf2 */\cf0 \
-}
\ No newline at end of file
+}
diff --git a/hello_plugin/CMakeLists.txt b/hello_plugin/CMakeLists.txt
deleted file mode 100644
index 36a22f7a7d78cb2fbf6ed7a2bf26b29a188d7f3c..0000000000000000000000000000000000000000
--- a/hello_plugin/CMakeLists.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-# INCLUDE_DIRECTORIES(/home/jcasals/workspace/wolf/src)
-# link_directories(/home/jcasals/workspace/wolf/lib)
-
-ADD_EXECUTABLE(hello_plugin hello_plugin.cpp)
-ADD_EXECUTABLE(params_autoconf params_autoconf.cpp)
-# target_link_libraries(hello_plugin class_loader boost_system console_bridge wolf yaml-cpp ${CERES_LIBRARIES})
-# target_link_libraries(params_autoconf class_loader boost_system console_bridge wolf yaml-cpp )
-target_link_libraries(hello_plugin wolf hellowolf yaml-cpp ${CERES_LIBRARIES} dl)
-target_link_libraries(params_autoconf wolf yaml-cpp dl)
-
-# These lines always at the end
-SET(HDRS_PLUGIN ${HDRS_PLUGIN} PARENT_SCOPE )
-SET(SRCS_PLUGIN ${SRCS_PLUGIN} PARENT_SCOPE )
-
-
diff --git a/hello_plugin/hello_plugin.cpp b/hello_plugin/hello_plugin.cpp
deleted file mode 100644
index cc837fa4395dc660a4711817d4b71d4cd23687f7..0000000000000000000000000000000000000000
--- a/hello_plugin/hello_plugin.cpp
+++ /dev/null
@@ -1,203 +0,0 @@
-/*
- * hello_plugin.cpp
- *
- * Created on: Nov 12, 2018
- * Author: jcasals
- */
-#include "core/sensor/sensor_base.h"
-#include "core/common/wolf.h"
-// #include "sensor_odom_2D.cpp"
-#include <yaml-cpp/yaml.h>
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
-
-#include "../hello_wolf/capture_range_bearing.h"
-#include "../hello_wolf/feature_range_bearing.h"
-#include "../hello_wolf/factor_range_bearing.h"
-#include "../hello_wolf/landmark_point_2D.h"
-#include "core/utils/loader.hpp"
-#include "core/processor/processor_odom_2D.h"
-
-#include "core/solver/solver_factory.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-using namespace std;
-using namespace wolf;
-using namespace Eigen;
-
-int main(int argc, char** argv) {
- string file = "";
- if (argc > 1)
- file = argv[1];
- parserYAML parser = parserYAML(file);
- parser.parse();
- paramsServer server = paramsServer(parser.getParams(),
- parser.sensorsSerialization(), parser.processorsSerialization());
- cout << "PRINTING SERVER MAP" << endl;
- server.print();
- cout << "-----------------------------------" << endl;
- /**
- It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
- a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
- the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
- **/
- // vector<ClassLoader*> class_loaders = vector<ClassLoader*>();
- // for(auto it : parser.getFiles()) {
- // auto c = new ClassLoader(it);
- // class_loaders.push_back(c);
- // }
- auto loaders = vector<Loader*>();
- for (auto it : parser.getFiles()) {
- auto l = new LoaderRaw(it);
- loaders.push_back(l);
- }
- ProblemPtr problem = Problem::create("PO", 2);
- auto sensorMap = map<string, SensorBasePtr>();
- auto procesorMap = map<string, ProcessorBasePtr>();
- for (auto s : server.getSensors()) {
- cout << s._name << " " << s._type << endl;
- sensorMap.insert(
- pair<string, SensorBasePtr>(s._name,
- problem->installSensor(s._type, s._name, server)));
- }
- for (auto s : server.getProcessors()) {
- cout << s._name << " " << s._type << " " << s._name_assoc_sensor
- << endl;
- procesorMap.insert(
- pair<string, ProcessorBasePtr>(s._name,
- problem->installProcessor(s._type, s._name,
- s._name_assoc_sensor, server)));
- }
-
- problem->print(4, 1, 1, 1);
- Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
- Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
-
- // landmark observations data
- VectorXi ids;
- VectorXs ranges, bearings;
-
- // SET OF EVENTS =======================================================
- std::cout << std::endl;
- WOLF_TRACE("======== BUILD PROBLEM =======");
-
- // ceres::Solver::Options options;
- // options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
- // CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
- auto ceres = SolverFactory::get().create("Solver", problem, server);
- // We'll do 3 steps of motion and landmark observations.
-
- // STEP 1 --------------------------------------------------------------
-
- // initialize
- TimeStamp t(0.0); // t : 0.0
- Vector3s x(0, 0, 0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
- problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
- auto sensor_rb = sensorMap.find("rb")->second;
- // observe lmks
- ids.resize(1);
- ranges.resize(1);
- bearings.resize(1);
- ids << 1; // will observe Lmk 1
- ranges << 1.0; // see drawing
- bearings << M_PI / 2;
- CaptureRangeBearingPtr cap_rb = std::make_shared < CaptureRangeBearing
- > (t, sensor_rb, ids, ranges, bearings);
- sensor_rb->process(cap_rb); // L1 : (1,2)
-
- // STEP 2 --------------------------------------------------------------
- t += 1.0; // t : 1.0
-
- // motion
- auto sensor_odometry = sensorMap.find("odom")->second;
- CaptureOdom2DPtr cap_motion = std::make_shared < CaptureOdom2D
- > (t, sensor_odometry, motion_data, motion_cov);
- sensor_odometry->process(cap_motion); // KF2 : (1,0,0)
-
- // observe lmks
- ids.resize(2);
- ranges.resize(2);
- bearings.resize(2);
- ids << 1, 2; // will observe Lmks 1 and 2
- ranges << sqrt(2.0), 1.0; // see drawing
- bearings << 3 * M_PI / 4, M_PI / 2;
- cap_rb = std::make_shared < CaptureRangeBearing
- > (t, sensor_rb, ids, ranges, bearings);
- sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2)
-
- // STEP 3 --------------------------------------------------------------
- t += 1.0; // t : 2.0
-
- // motion
- cap_motion->setTimeStamp(t);
- sensor_odometry->process(cap_motion); // KF3 : (2,0,0)
- // observe lmks
- ids.resize(2);
- ranges.resize(2);
- bearings.resize(2);
- ids << 2, 3; // will observe Lmks 2 and 3
- ranges << sqrt(2.0), 1.0; // see drawing
- bearings << 3 * M_PI / 4, M_PI / 2;
- cap_rb = std::make_shared < CaptureRangeBearing
- > (t, sensor_rb, ids, ranges, bearings);
- sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
- problem->print(1, 0, 1, 0);
-
- // SOLVE ================================================================
-
- // SOLVE with exact initial guess
- WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
- std::string report = ceres->solve(
- wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
- problem->print(1, 0, 1, 0);
-
- // PERTURB initial guess
- WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto sen : problem->getHardware()->getSensorList())
- for (auto sb : sen->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(
- sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto kf : problem->getTrajectory()->getFrameList())
- if (kf->isKey())
- for (auto sb : kf->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(
- sb->getState()
- + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto sb : lmk->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(
- sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- problem->print(1, 0, 1, 0);
-
- // SOLVE again
- WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
- report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
- problem->print(1, 0, 1, 0);
-
- // GET COVARIANCES of all states
- WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
- ceres->computeCovariances(
- SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectory()->getFrameList()) {
- if (kf->isKey()) {
- Eigen::MatrixXs cov;
- WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
- }
- for (auto lmk : problem->getMap()->getLandmarkList()) {
- Eigen::MatrixXs cov;
- WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
- }
- }
- std::cout << std::endl;
-
- WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
- problem->print(4, 1, 1, 1);
-
- return 0;
-}
diff --git a/hello_plugin/params.yaml b/hello_plugin/params.yaml
deleted file mode 100644
index 14c4a1fa4753080997625511d5aa092611ce0101..0000000000000000000000000000000000000000
--- a/hello_plugin/params.yaml
+++ /dev/null
@@ -1,30 +0,0 @@
-config:
- sensors:
- -
- type: "ODOM 2D"
- name: "odom"
- intrinsic:
- k_disp_to_disp: 0.1
- k_rot_to_rot: 0.1
- extrinsic:
- pos: [1,2,3]
- -
- type: "RANGE BEARING"
- name: "rb"
- processors:
- -
- type: "ODOM 2D"
- name: "processor1"
- sensor name: "odom"
- -
- type: "RANGE BEARING"
- name: "rb_processor"
- sensor name: "rb"
- -
- type: "ODOM 2D"
- name: "my_proc_test"
- sensor name: "odom"
- follow: "../hello_plugin/params_conf.yaml" #Config continues in this file
-files:
- - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odom.so"
- - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
diff --git a/hello_plugin/params_autoconf.cpp b/hello_plugin/params_autoconf.cpp
deleted file mode 100644
index 9f21c611d63849fa95ab701010dbe8c975fc3b91..0000000000000000000000000000000000000000
--- a/hello_plugin/params_autoconf.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * params_autoconf.cpp
- *
- * Created on: Feb 15, 2019
- * Author: jcasals
- */
-#include "core/sensor/sensor_base.h"
-#include "core/common/wolf.h"
-// #include "sensor_odom_2D.cpp"
-#include <yaml-cpp/yaml.h>
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
-
-#include "../hello_wolf/capture_range_bearing.h"
-#include "../hello_wolf/feature_range_bearing.h"
-#include "../hello_wolf/factor_range_bearing.h"
-#include "../hello_wolf/landmark_point_2D.h"
-#include "core/utils/loader.hpp"
-#include "core/processor/processor_odom_2D.h"
-
-#include "core/solver/solver_factory.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-using namespace std;
-using namespace wolf;
-using namespace Eigen;
-
-int main(int argc, char** argv) {
- string file = "";
- if(argc > 1) file = argv[1];
- parserYAML parser = parserYAML(file);
- parser.parse();
- paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
- cout << "PRINTING SERVER MAP" << endl;
- server.print();
- cout << "-----------------------------------" << endl;
- /**
- It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
- a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
- the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
- **/
- auto loaders = vector<Loader*>();
- for(auto it : parser.getFiles()) {
- auto l = new LoaderRaw(it);
- l->load();
- loaders.push_back(l);
- }
- ProblemPtr problem = Problem::create("PO", 2);
- auto sensorMap = map<string, SensorBasePtr>();
- auto procesorMap = map<string, ProcessorBasePtr>();
- for(auto s : server.getSensors()){
- cout << s._name << " " << s._type << endl;
- sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server)));
- }
- for(auto s : server.getProcessors()){
- cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
- procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
- }
- auto prc = ProcessorParamsOdom2D("my_proc_test", server);
-
- std::cout << "prc.cov_det " << prc.cov_det << std::endl;
- std::cout << "prc.unmeasured_perturbation_std " << prc.unmeasured_perturbation_std << std::endl;
- std::cout << "prc.angle_turned " << prc.angle_turned << std::endl;
- std::cout << "prc.dist_traveled " << prc.dist_traveled << std::endl;
- std::cout << "prc.max_buff_length " << prc.max_buff_length << std::endl;
- std::cout << "prc.max_time_span " << prc.max_time_span << std::endl;
- std::cout << "prc.time_tolerance " << prc.time_tolerance << std::endl;
- std::cout << "prc.voting_active " << prc.voting_active << std::endl;
-
- return 0;
-}
diff --git a/hello_plugin/params_conf.yaml b/hello_plugin/params_conf.yaml
deleted file mode 100644
index 0d80a5802f5da772f59c29adf12257c8f79663d3..0000000000000000000000000000000000000000
--- a/hello_plugin/params_conf.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-cov_det: 100
-unmeasured_perturbation_std: 100
-angle_turned: 100
-dist_traveled: 100
-max_buff_length: 100
-max_time_span: 100
-time_tolerance: 100
-voting_active: false
\ No newline at end of file
diff --git a/hello_wolf/feature_range_bearing.cpp b/hello_wolf/feature_range_bearing.cpp
deleted file mode 100644
index 3be9365676824e14689e63a6f969f33123736fb6..0000000000000000000000000000000000000000
--- a/hello_wolf/feature_range_bearing.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * FeatureRangeBearing2D.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
-
-#include "feature_range_bearing.h"
-
-namespace wolf
-{
-
-FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("RANGE BEARING", _measurement, _meas_covariance)
-{
- //
-}
-
-FeatureRangeBearing::~FeatureRangeBearing()
-{
- //
-}
-
-} // namespace wolf
-
diff --git a/hello_wolf/landmark_point_2D.cpp b/hello_wolf/landmark_point_2D.cpp
deleted file mode 100644
index 2b5fac1b49d674fa7fd3ba8c8883939e4035880a..0000000000000000000000000000000000000000
--- a/hello_wolf/landmark_point_2D.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/**
- * \file landmark_point_2D.cpp
- *
- * Created on: Dec 4, 2017
- * \author: jsola
- */
-
-#include "landmark_point_2D.h"
-
-namespace wolf
-{
-
-LandmarkPoint2D::LandmarkPoint2D(int _id, const Eigen::Vector2s& _xy) :
- LandmarkBase("POINT 2D", std::make_shared<StateBlock>(_xy))
-{
- setId(_id);
-}
-
-LandmarkPoint2D::~LandmarkPoint2D()
-{
- //
-}
-
-} /* namespace wolf */
diff --git a/hello_wolf/landmark_point_2D.h b/hello_wolf/landmark_point_2D.h
deleted file mode 100644
index 0d7fe13a00f5112f00b0161350d872a6f59895b2..0000000000000000000000000000000000000000
--- a/hello_wolf/landmark_point_2D.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/**
- * \file landmark_point_2D.h
- *
- * Created on: Dec 4, 2017
- * \author: jsola
- */
-
-#ifndef HELLO_WOLF_LANDMARK_POINT_2D_H_
-#define HELLO_WOLF_LANDMARK_POINT_2D_H_
-
-#include "core/landmark/landmark_base.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(LandmarkPoint2D);
-
-class LandmarkPoint2D : public LandmarkBase
-{
- public:
- LandmarkPoint2D(int _id, const Eigen::Vector2s& _xy);
- virtual ~LandmarkPoint2D();
-};
-
-} /* namespace wolf */
-
-#endif /* HELLO_WOLF_LANDMARK_POINT_2D_H_ */
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
deleted file mode 100644
index b75801bde13cff3a003e0ba746bec5fb864bb813..0000000000000000000000000000000000000000
--- a/hello_wolf/processor_range_bearing.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-/*
- * processor_range_bearing.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
-
-#include "processor_range_bearing.h"
-#include "capture_range_bearing.h"
-#include "landmark_point_2D.h"
-#include "feature_range_bearing.h"
-#include "factor_range_bearing.h"
-
-namespace wolf
-{
-
-ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) :
- ProcessorBase("RANGE BEARING", _params)
-{
- H_r_s = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState());
-}
-
-void ProcessorRangeBearing::process(CaptureBasePtr _capture)
-{
-
- // 1. get KF
- FrameBasePtr kf(nullptr);
- if ( !kf_pack_buffer_.empty() )
- {
- // KeyFrame Callback received
- PackKeyFramePtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
-
- if (pack!=nullptr)
- kf = pack->key_frame;
-
- kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() );
-
- assert( kf && "Callback KF is not close enough to _capture!");
- }
-
- if (!kf)
- {
- // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
- kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp());
- assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
- }
-
- // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
- CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
- // kf->addCapture(capture_rb);
- capture_rb->link(kf);
-
- // 3. explore all observations in the capture
- for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
- {
- // extract info
- int id = capture_rb->getId (i);
- Scalar range = capture_rb->getRange (i);
- Scalar bearing = capture_rb->getBearing(i);
-
- // 4. create or recover landmark
- LandmarkPoint2DPtr lmk;
- auto lmk_it = known_lmks.find(id);
- if ( lmk_it != known_lmks.end() )
- {
- // known landmarks : recover landmark
- lmk = std::static_pointer_cast<LandmarkPoint2D>(lmk_it->second);
- WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
- }
- else
- {
- // new landmark:
- // - create landmark
- // lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
- lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)));
- WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
- // getProblem()->getMap()->addLandmark(lmk);
- // - add to known landmarks
- known_lmks.emplace(id, lmk);
- }
-
- // 5. create feature
- Vector2s rb(range,bearing);
- auto ftr = std::make_shared<FeatureRangeBearing>(rb,
- getSensor()->getNoiseCov());
- // capture_rb->addFeature(ftr);
- FeatureBase::emplace<FeatureRangeBearing>(capture_rb, rb, getSensor()->getNoiseCov());
-
- // 6. create factor
- auto prc = shared_from_this();
- // auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
- // lmk,
- // prc,
- // false,
- // FAC_ACTIVE);
- auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb,
- lmk,
- prc,
- false,
- FAC_ACTIVE);
- // ftr->addFactor(ctr);
- // lmk->addConstrainedBy(ctr);
- }
-
-}
-
-ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
-{
- SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr);
- ProcessorParamsRangeBearingPtr params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
-
- // construct processor
- ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
-
- // setup processor
- prc->setName(_unique_name);
-
- return prc;
-}
-
-ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _unique_name,
- const paramsServer& _server,
- const SensorBasePtr _sensor_ptr)
-{
- SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr);
- ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>();
- params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
- params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.01");
-
- // construct processor
- ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
-
- // setup processor
- prc->setName(_unique_name);
-
- return prc;
-}
-
-Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const
-{
- return polar(toSensor(lmk_w));
-}
-
-Eigen::Vector2s ProcessorRangeBearing::invObserve(Scalar r, Scalar b) const
-{
- return fromSensor(rect(r, b));
-}
-
-ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3s& _pose) const
-{
- Trf H = Eigen::Translation<Scalar,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<Scalar>(_pose(2)) ;
- return H;
-}
-
-ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2s& _position,
- const Eigen::Vector1s& _orientation) const
-{
- Trf H = Eigen::Translation<Scalar,2>(_position(0), _position(1)) * Eigen::Rotation2D<Scalar>(_orientation(0)) ;
- return H;
-}
-
-Eigen::Vector2s ProcessorRangeBearing::fromSensor(const Eigen::Vector2s& lmk_s) const
-{
- Trf H_w_r = transform(getProblem()->getCurrentState());
- return H_w_r * H_r_s * lmk_s;
-}
-
-Eigen::Vector2s ProcessorRangeBearing::toSensor(const Eigen::Vector2s& lmk_w) const
-{
-// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
- Trf H_w_r = transform(getProblem()->getCurrentState());
- return (H_w_r * H_r_s).inverse() * lmk_w;
-}
-
-Eigen::Vector2s ProcessorRangeBearing::polar(const Eigen::Vector2s& rect) const
-{
- Eigen::Vector2s polar;
- polar(0) = rect.norm();
- polar(1) = atan2(rect(1), rect(0));
- return polar;
-}
-
-Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const
-{
- return range * (Vector2s() << cos(bearing), sin(bearing)).finished();
-}
-
-} /* namespace wolf */
-
-// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing)
-} // namespace wolf
-#include "core/processor/autoconf_processor_factory.h"
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR_AUTO("RANGE BEARING", ProcessorRangeBearing)
-} // namespace wolf
-
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
deleted file mode 100644
index 30b2bd8654a8bc8117f761e492f889fb44918a4a..0000000000000000000000000000000000000000
--- a/hello_wolf/processor_range_bearing.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
- * ProcessorRangeBearing.h
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
-
-#ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
-#define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
-
-#include "core/processor/processor_base.h"
-#include "sensor_range_bearing.h"
-#include "Eigen/Geometry"
-
-#include <map>
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing);
-
-struct ProcessorParamsRangeBearing : public ProcessorParamsBase
-{
- // We do not need special parameters, but in case you need they should be defined here.
-};
-
-using namespace Eigen;
-WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
-
-class ProcessorRangeBearing : public ProcessorBase
-{
- public:
- typedef Eigen::Transform<Scalar, 2, Eigen::Affine> Trf;
-
- ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params);
- virtual ~ProcessorRangeBearing() {/* empty */}
- virtual void configure(SensorBasePtr _sensor) override { }
-
- // Factory method for high level API
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr sensor_ptr = nullptr);
- static ProcessorBasePtr createAutoConf(const std::string& _unique_name,
- const paramsServer& _server,
- const SensorBasePtr _sensor_ptr = nullptr);
-
- protected:
- // Implementation of pure virtuals from ProcessorBase
- virtual void process (CaptureBasePtr _capture) override;
- virtual bool voteForKeyFrame () override {return false;}
-
- private:
- // control variables
- Trf H_r_s; // transformation matrix, robot to sensor
- std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
-
- protected:
- // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
- Eigen::Vector2s observe (const Eigen::Vector2s& lmk_w) const;
- Eigen::Vector2s invObserve (Scalar r, Scalar b) const;
- private:
- // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
- Trf transform (const Eigen::Vector3s& _pose) const;
- Trf transform (const Eigen::Vector2s& _position, const Eigen::Vector1s& _orientation) const;
- Eigen::Vector2s fromSensor (const Eigen::Vector2s& lmk_s) const;
- Eigen::Vector2s toSensor (const Eigen::Vector2s& lmk_w) const;
- Eigen::Vector2s polar (const Eigen::Vector2s& rect) const;
- Eigen::Vector2s rect (Scalar range, Scalar bearing) const;
-};
-
-} /* namespace wolf */
-
-#endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
deleted file mode 100644
index 74f2c0e46cec59d1be7ba297547f927223d80c94..0000000000000000000000000000000000000000
--- a/hello_wolf/sensor_range_bearing.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * SensorRangeBearing.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
-
-#include "sensor_range_bearing.h"
-#include "core/state_block/state_angle.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(SensorRangeBearing);
-
-SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std) :
- SensorBase("RANGE BEARING",
- std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
- std::make_shared<StateAngle>(_extrinsics(2), true),
- nullptr,
- _noise_std)
-{
- assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D");
-}
-
-SensorRangeBearing::~SensorRangeBearing()
-{
- //
-}
-
-SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
- const Eigen::VectorXs& _extrinsics, //
- const IntrinsicsBasePtr _intrinsics)
-{
-
- IntrinsicsRangeBearingPtr _intrinsics_rb = std::static_pointer_cast<IntrinsicsRangeBearing>(_intrinsics);
-
- Eigen::Vector2s noise_std(_intrinsics_rb->noise_range_metres_std, toRad(_intrinsics_rb->noise_bearing_degrees_std));
-
- SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(_extrinsics, noise_std);
-
- return sensor;
-}
-
-SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name, //
- const paramsServer& _server)
-{
- Eigen::Vector2s noise_std(0.1,1.0);
- SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(Eigen::Vector3s(1,1,0), noise_std);
- sensor->setName(_unique_name);
- // sensor->node_name_ = _unique_name;
- return sensor;
-}
-
-} /* namespace wolf */
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf
-{
-WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing)
-} // namespace wolf
-#include "core/sensor/autoconf_sensor_factory.h"
-namespace wolf
-{
-WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing)
-} // namespace wolf
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
deleted file mode 100644
index 5b80ff6b8f03ab7b9151252f29b99341212975b6..0000000000000000000000000000000000000000
--- a/hello_wolf/sensor_range_bearing.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * SensorRangeBearing.h
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
-
-#ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_
-#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
-
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
-
-namespace wolf
-{
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsRangeBearing);
-
-struct IntrinsicsRangeBearing : public IntrinsicsBase
-{
- Scalar noise_range_metres_std = 0.05;
- Scalar noise_bearing_degrees_std = 0.5;
- IntrinsicsRangeBearing()
- {
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
- }
- IntrinsicsRangeBearing(std::string _unique_name, const paramsServer& _server):
- IntrinsicsBase(_unique_name, _server)
- {
- noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std", "0.05");
- noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std", "0.5");
- }
-};
-
-WOLF_PTR_TYPEDEFS(SensorRangeBearing)
-
-class SensorRangeBearing : public SensorBase
-{
- public:
- SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std);
- virtual ~SensorRangeBearing();
-
- // Factory method for high level API
- static SensorBasePtr create(const std::string& _unique_name, //
- const Eigen::VectorXs& _extrinsics, //
- const IntrinsicsBasePtr _intrinsics);
- static SensorBasePtr createAutoConf(const std::string& _unique_name, //
- const paramsServer& _server);
-};
-
-} /* namespace wolf */
-
-#endif /* HELLO_WOLF_SENSOR_RANGE_BEARING_H_ */
diff --git a/include/core/association/association_nnls.h b/include/core/association/association_nnls.h
deleted file mode 100644
index c3030f7493cc3e9117a6433bf8e0521fc6fb8ecf..0000000000000000000000000000000000000000
--- a/include/core/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 "core/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/core/association/association_node.h b/include/core/association/association_node.h
deleted file mode 100644
index 50e2f67d07ba42a5fe08d7178ac9746934753bdd..0000000000000000000000000000000000000000
--- a/include/core/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 "core/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/core/association/association_solver.h b/include/core/association/association_solver.h
deleted file mode 100644
index 7a1ff70ef3876eb75d0b469f581784118aac2f43..0000000000000000000000000000000000000000
--- a/include/core/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 "core/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/core/association/association_tree.h b/include/core/association/association_tree.h
deleted file mode 100644
index e910183b011d485f03368c77578a8d0cdc53f8d5..0000000000000000000000000000000000000000
--- a/include/core/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 "core/association/matrix.h"
-#include "core/association/association_solver.h"
-#include "core/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/core/association/matrix.h b/include/core/association/matrix.h
deleted file mode 100644
index c76bba51e4de2db56a380e48e7002a16bba78bcc..0000000000000000000000000000000000000000
--- a/include/core/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/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 7864c46b9e03eb2b3609ca588d39e4ef1a06d975..68d34c20bfd4e2abe3ca76d331f696d86423effe 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -11,28 +11,31 @@ class FeatureBase;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
namespace wolf{
//class CaptureBase
-class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
+class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<CaptureBase>
{
+ friend FeatureBase;
+ friend FactorBase;
+ friend FrameBase;
+
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
+ void setProblem(ProblemPtr _problem) override final;
public:
@@ -43,71 +46,76 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
- virtual ~CaptureBase();
- virtual void remove();
+ ~CaptureBase() override;
+ virtual void remove(bool viral_remove_empty_parent=false);
// Type
virtual bool isMotion() const { return false; }
bool process();
- unsigned int id();
+ unsigned int id() const;
TimeStamp getTimeStamp() const;
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
FrameBasePtr getFrame() const;
+ private:
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);
+ public:
+ const FeatureBasePtrList& getFeatureList() const;
- void getFactorList(FactorBasePtrList& _fac_list);
+ void getFactorList(FactorBasePtrList& _fac_list) const;
+ FactorBasePtrList getFactorList() const;
SensorBasePtr getSensor() const;
virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
+ private:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+ public:
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
- // 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);
+ // State blocks
+ StateBlockPtr getStateBlock(const char& _key) const;
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);
+
+ void fix() override;
+ void unfix() override;
+
+ void move(FrameBasePtr);
void link(FrameBasePtr);
template<typename classType, typename... T>
- static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all);
+ static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
- protected:
- SizeEigen computeCalibSize() const;
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
- void updateCalibSize();
+ FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
+ void removeFeature(FeatureBasePtr _ft_ptr);
};
}
@@ -120,68 +128,37 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
namespace wolf{
template<typename classType, typename... T>
-std::shared_ptr<CaptureBase> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
+std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
{
- CaptureBasePtr cpt = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> cpt = std::make_shared<classType>(std::forward<T>(all)...);
cpt->link(_frm_ptr);
return cpt;
}
-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);
+ return getStateBlock('P');
}
inline StateBlockPtr CaptureBase::getSensorO() const
{
- return getStateBlock(1);
+ return getStateBlock('O');
}
inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
{
- return getStateBlock(2);
+ return getStateBlock('I');
}
-inline unsigned int CaptureBase::id()
+inline unsigned int CaptureBase::id() const
{
return capture_id_;
}
inline FrameBasePtr CaptureBase::getFrame() const
{
- return frame_ptr_.lock();
+ if(frame_ptr_.expired()) return nullptr;
+ else return frame_ptr_.lock();
}
inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
@@ -189,7 +166,7 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
frame_ptr_ = _frm_ptr;
}
-inline FeatureBasePtrList& CaptureBase::getFeatureList()
+inline const FeatureBasePtrList& CaptureBase::getFeatureList() const
{
return feature_list_;
}
@@ -199,11 +176,12 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& CaptureBase::getConstrainedByList()
+inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
+
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
@@ -229,13 +207,6 @@ 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
diff --git a/include/core/capture/capture_buffer.h b/include/core/capture/capture_buffer.h
deleted file mode 100644
index 6ecabc34ea5d62b3055c84b7cbf0d5cb4a94efed..0000000000000000000000000000000000000000
--- a/include/core/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 "core/common/wolf.h"
-#include "core/common/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/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
new file mode 100644
index 0000000000000000000000000000000000000000..e9797427f3550289906e159c40fefe681d2e55ad
--- /dev/null
+++ b/include/core/capture/capture_diff_drive.h
@@ -0,0 +1,45 @@
+/**
+ * \file diff_drive_tools.h
+ *
+ * Created on: Oct 20, 2016
+ * \author: Jeremie Deray
+ */
+
+#ifndef CAPTURE_DIFF_DRIVE_H_
+#define CAPTURE_DIFF_DRIVE_H_
+
+//wolf includes
+#include "core/capture/capture_motion.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureDiffDrive)
+
+/**
+ * @brief The CaptureWheelJointPosition class
+ *
+ * Represents a list of wheel positions in radian.
+ */
+class CaptureDiffDrive : public CaptureMotion
+{
+
+public:
+
+ /**
+ * \brief Constructor
+ **/
+ CaptureDiffDrive(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr,
+ StateBlockPtr _p_ptr = nullptr,
+ StateBlockPtr _o_ptr = nullptr,
+ StateBlockPtr _intr_ptr = nullptr);
+
+ ~CaptureDiffDrive() override = default;
+};
+
+} // namespace wolf
+
+#endif /* CAPTURE_DIFF_DRIVE_H_ */
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index c6cb1ef9ee2ca0bf96cabc0641e4f21937489280..c0d86ee1089740ecf03b3896f9264cdc2de5db88 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -1,5 +1,5 @@
/**
- * \file capture_motion2.h
+ * \file capture_motion.h
*
* Created on: Mar 16, 2016
* \author: jsola
@@ -49,81 +49,86 @@ class CaptureMotion : public CaptureBase
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);
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_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,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
- virtual ~CaptureMotion();
+ ~CaptureMotion() override;
// Type
- virtual bool isMotion() const override { return true; }
+ 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);
+ const Eigen::VectorXd& getData() const;
+ const Eigen::MatrixXd& getDataCovariance() const;
+ void setData(const Eigen::VectorXd& _data);
+ void setDataCovariance(const Eigen::MatrixXd& _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);
+ VectorXd getCalibrationPreint() const;
+ void setCalibrationPreint(const VectorXd& _calib_preint);
+ MatrixXd getJacobianCalib() const;
+ MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
// 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);
+ VectorXd getDeltaPreint() const;
+ VectorXd getDeltaPreint(const TimeStamp& _ts) const;
+ MatrixXd getDeltaPreintCov() const;
+ MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
+
+ // Origin frame and capture
+ CaptureBasePtr getOriginCapture();
+ CaptureBasePtr getOriginCapture() const;
+ void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
+
+ bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance);
+
+ void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
// 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
+ Eigen::VectorXd data_; ///< Motion data in form of vector mandatory
+ Eigen::MatrixXd data_cov_; ///< Motion data covariance
+ Eigen::VectorXd calib_preint_; ///< Calibration parameters used during pre-integration
+ MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
+ CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion
};
-inline const Eigen::VectorXs& CaptureMotion::getData() const
+inline const Eigen::VectorXd& CaptureMotion::getData() const
{
return data_;
}
-inline const Eigen::MatrixXs& CaptureMotion::getDataCovariance() const
+inline const Eigen::MatrixXd& CaptureMotion::getDataCovariance() const
{
return data_cov_;
}
-inline void CaptureMotion::setData(const Eigen::VectorXs& _data)
+inline void CaptureMotion::setData(const Eigen::VectorXd& _data)
{
assert(_data.size() == data_.size() && "Wrong size of data vector!");
data_ = _data;
}
-inline void CaptureMotion::setDataCovariance(const Eigen::MatrixXs& _data_cov)
+inline void CaptureMotion::setDataCovariance(const Eigen::MatrixXd& _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!");
@@ -140,58 +145,57 @@ inline MotionBuffer& CaptureMotion::getBuffer()
return buffer_;
}
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib()
+inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const
{
- return getBuffer().get().back().jacobian_calib_;
+ return getBuffer().back().jacobian_calib_;
}
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts)
+inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).jacobian_calib_;
}
-inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
+inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
{
- 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;
+ return capture_origin_ptr_.lock();
}
-inline FrameBasePtr CaptureMotion::getOriginFrame()
+inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture() const
{
- return origin_frame_ptr_.lock();
+ return capture_origin_ptr_.lock();
}
-inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
+inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr)
{
- origin_frame_ptr_ = _frame_ptr;
+ capture_origin_ptr_ = _capture_origin_ptr;
}
-inline VectorXs CaptureMotion::getCalibrationPreint() const
+inline VectorXd CaptureMotion::getCalibrationPreint() const
{
- return getBuffer().getCalibrationPreint();
+ return calib_preint_;
}
-inline void CaptureMotion::setCalibrationPreint(const VectorXs& _calib_preint)
+inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint)
{
- getBuffer().setCalibrationPreint(_calib_preint);
+ calib_preint_ = _calib_preint;
}
-inline VectorXs CaptureMotion::getDeltaPreint()
+inline VectorXd CaptureMotion::getDeltaPreint() const
{
- return getBuffer().get().back().delta_integr_;
+ return getBuffer().back().delta_integr_;
}
-inline VectorXs CaptureMotion::getDeltaPreint(const TimeStamp& _ts)
+inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_;
}
-inline MatrixXs CaptureMotion::getDeltaPreintCov()
+inline MatrixXd CaptureMotion::getDeltaPreintCov() const
{
- return getBuffer().get().back().delta_integr_cov_;
+ return getBuffer().back().delta_integr_cov_;
}
-inline MatrixXs CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts)
+inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_cov_;
}
diff --git a/include/core/capture/capture_odom_2D.h b/include/core/capture/capture_odom_2D.h
deleted file mode 100644
index 7fedfde3aeafc06ffa8fb652e34c4df67127990f..0000000000000000000000000000000000000000
--- a/include/core/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 "core/capture/capture_motion.h"
-
-#include "core/math/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/core/capture/capture_odom_2d.h b/include/core/capture/capture_odom_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0a81bf2d47cad9b8d564f11eb011574daae76d4
--- /dev/null
+++ b/include/core/capture/capture_odom_2d.h
@@ -0,0 +1,40 @@
+/*
+ * capture_odom_2d.h
+ *
+ * Created on: Oct 16, 2017
+ * Author: jsola
+ */
+
+#ifndef CAPTURE_ODOM_2d_H_
+#define CAPTURE_ODOM_2d_H_
+
+#include "core/capture/capture_motion.h"
+
+#include "core/math/rotations.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(CaptureOdom2d);
+
+class CaptureOdom2d : public CaptureMotion
+{
+ public:
+ CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ ~CaptureOdom2d() override;
+
+};
+
+} /* namespace wolf */
+
+#endif /* CAPTURE_ODOM_2d_H_ */
diff --git a/include/core/capture/capture_odom_3D.h b/include/core/capture/capture_odom_3D.h
deleted file mode 100644
index a8c5d651e8ee8c7dad31d95ee7b66623e08e2e41..0000000000000000000000000000000000000000
--- a/include/core/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 "core/capture/capture_motion.h"
-
-#include "core/math/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/core/capture/capture_odom_3d.h b/include/core/capture/capture_odom_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..054c4f935ca63f73fd6fbbb87bab70a10655e5c3
--- /dev/null
+++ b/include/core/capture/capture_odom_3d.h
@@ -0,0 +1,40 @@
+/*
+ * capture_odom_3d.h
+ *
+ * Created on: Oct 16, 2017
+ * Author: jsola
+ */
+
+#ifndef CAPTURE_ODOM_3d_H_
+#define CAPTURE_ODOM_3d_H_
+
+#include "core/capture/capture_motion.h"
+
+#include "core/math/rotations.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(CaptureOdom3d);
+
+class CaptureOdom3d : public CaptureMotion
+{
+ public:
+ CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ ~CaptureOdom3d() override;
+
+};
+
+} /* namespace wolf */
+
+#endif /* CAPTURE_ODOM_3d_H_ */
diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h
index aff48d40a25903ccc06a633282467ab5d2639459..07db9e0351ddb1e73ea8877c1a31ddc456e7dacd 100644
--- a/include/core/capture/capture_pose.h
+++ b/include/core/capture/capture_pose.h
@@ -3,8 +3,8 @@
//Wolf includes
#include "core/capture/capture_base.h"
-#include "core/factor/factor_pose_2D.h"
-#include "core/factor/factor_pose_3D.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_pose_3d.h"
#include "core/feature/feature_pose.h"
//std includes
@@ -18,16 +18,18 @@ WOLF_PTR_TYPEDEFS(CapturePose);
class CapturePose : public CaptureBase
{
protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
+ Eigen::VectorXd data_; ///< Raw data.
+ Eigen::MatrixXd data_covariance_; ///< Noise of the capture.
public:
- CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+ CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance);
- virtual ~CapturePose();
+ ~CapturePose() override;
virtual void emplaceFeatureAndFactor();
+ Eigen::VectorXd getData(){ return data_;}
+ Eigen::MatrixXd getDataCovariance(){ return data_covariance_;}
};
diff --git a/include/core/capture/capture_velocity.h b/include/core/capture/capture_velocity.h
deleted file mode 100644
index 98bc3b3e395af54388f03777dfe876bf6a8a8429..0000000000000000000000000000000000000000
--- a/include/core/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 "core/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/core/capture/capture_void.h b/include/core/capture/capture_void.h
index c0bd4803d42cc61116265cc9625b83f52e347efb..0ac7b3b2fc9ed44c4ac430fd9f70387435f05227 100644
--- a/include/core/capture/capture_void.h
+++ b/include/core/capture/capture_void.h
@@ -14,7 +14,7 @@ class CaptureVoid : public CaptureBase
{
public:
CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
- virtual ~CaptureVoid();
+ ~CaptureVoid() override;
};
diff --git a/include/core/capture/capture_wheel_joint_position.h b/include/core/capture/capture_wheel_joint_position.h
deleted file mode 100644
index a2dc8b9571d4cd87ebdfccfc3297e2d3711edcaf..0000000000000000000000000000000000000000
--- a/include/core/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 "core/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/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
deleted file mode 100644
index c65b27415643c2c54b4c5a5144465b4a0767884b..0000000000000000000000000000000000000000
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ /dev/null
@@ -1,122 +0,0 @@
-#ifndef CERES_MANAGER_H_
-#define CERES_MANAGER_H_
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//wolf includes
-#include "core/solver/solver_manager.h"
-#include "core/ceres_wrapper/cost_function_wrapper.h"
-#include "core/ceres_wrapper/local_parametrization_wrapper.h"
-#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
-
-namespace ceres {
-typedef std::shared_ptr<CostFunction> CostFunctionPtr;
-}
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CeresManager);
-
-/** \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();
-
- static SolverManagerPtr create(const ProblemPtr& wolf_problem, const paramsServer& _server);
-
- ceres::Solver::Summary getSummary();
-
- std::unique_ptr<ceres::Problem>& getCeresProblem()
- {
- return ceres_problem_;
- }
-
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
- = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
-
- virtual void computeCovariances(const std::vector<StateBlockPtr>& 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();
-
- protected:
-
- 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);
-
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr);
-
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr);
-};
-
-inline ceres::Solver::Summary CeresManager::getSummary()
-{
- return summary_;
-}
-
-inline ceres::Solver::Options& CeresManager::getSolverOptions()
-{
- return ceres_options_;
-}
-
-inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr)
-{
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end()
- && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
-}
-
-inline bool CeresManager::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr)
-{
- return state_blocks_local_param_.find(state_ptr) != state_blocks_local_param_.end()
- && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/ceres_wrapper/cost_function_wrapper.h b/include/core/ceres_wrapper/cost_function_wrapper.h
index 69cbf125bc44f89066377a0754c2af6ada6e36df..c9972fe780e76c243ed1a298f658d6e702fe8d1e 100644
--- a/include/core/ceres_wrapper/cost_function_wrapper.h
+++ b/include/core/ceres_wrapper/cost_function_wrapper.h
@@ -25,9 +25,9 @@ class CostFunctionWrapper : public ceres::CostFunction
CostFunctionWrapper(FactorBasePtr _factor_ptr);
- virtual ~CostFunctionWrapper();
+ ~CostFunctionWrapper() override;
- virtual bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const;
+ bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const override;
FactorBasePtr getFactor() const;
};
diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
index ff60fea02729c791b569c0585f47e5da34bfca19..2504d0b10bf669ececd7ce33e6ab9b15928eb26b 100644
--- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -12,7 +12,6 @@
#include "ceres/numeric_diff_cost_function.h"
// Factors
-#include "core/factor/factor_odom_2D.h"
#include "core/factor/factor_base.h"
namespace wolf {
@@ -33,8 +32,8 @@ inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(Factor
// switch (_fac_ptr->getTypeId())
// {
// // just for testing
-// case FAC_ODOM_2D:
-// return createNumericDiffCostFunctionCeres<FactorOdom2D>(_fac_ptr);
+// case FAC_ODOM_2d:
+// return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr);
//
// /* For adding a new factor, add the #include and a case:
// case FAC_ENUM:
diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h
new file mode 100644
index 0000000000000000000000000000000000000000..1717be546ab820d24e13985227dd19753530440e
--- /dev/null
+++ b/include/core/ceres_wrapper/iteration_update_callback.h
@@ -0,0 +1,60 @@
+/*
+ * iteration_callback.h
+ *
+ * Created on: Jun 15, 2020
+ * Author: joanvallve
+ */
+
+#ifndef INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_
+#define INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_
+
+#include "core/problem/problem.h"
+#include "ceres/ceres.h"
+
+namespace wolf {
+
+class IterationUpdateCallback : public ceres::IterationCallback
+{
+ public:
+ explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
+ : problem_(_problem)
+ , min_num_iterations_(min_num_iterations)
+ , initial_cost_(0)
+ , verbose_(verbose)
+ {}
+
+ ~IterationUpdateCallback() {}
+
+ ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override
+ {
+ if (summary.iteration == 0)
+ initial_cost_ = summary.cost;
+
+ else if (summary.iteration >= min_num_iterations_ and
+ (problem_->getStateBlockNotificationMapSize() != 0 or
+ problem_->getFactorNotificationMapSize() != 0))
+ {
+ if (summary.cost >= initial_cost_)
+ {
+ WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", summary.iteration);
+ return ceres::SOLVER_ABORT;
+ }
+ else
+ {
+ WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", summary.iteration);
+ return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
+ }
+ }
+ return ceres::SOLVER_CONTINUE;
+ }
+
+ private:
+ ProblemPtr problem_;
+ int min_num_iterations_;
+ double initial_cost_;
+ bool verbose_;
+};
+
+}
+
+#endif /* INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_ */
diff --git a/include/core/ceres_wrapper/local_parametrization_wrapper.h b/include/core/ceres_wrapper/local_parametrization_wrapper.h
index cb02cb8f01f4256d1223d9d2bf3e686a2ef5b76f..fcffbbf80ed9b09717dd830fe250facd70bf9985 100644
--- a/include/core/ceres_wrapper/local_parametrization_wrapper.h
+++ b/include/core/ceres_wrapper/local_parametrization_wrapper.h
@@ -21,15 +21,15 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization
LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
- virtual ~LocalParametrizationWrapper() = default;
+ ~LocalParametrizationWrapper() override = default;
- virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const;
+ bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override;
- virtual bool ComputeJacobian(const double* x, double* jacobian) const;
+ bool ComputeJacobian(const double* x, double* jacobian) const override;
- virtual int GlobalSize() const;
+ int GlobalSize() const override;
- virtual int LocalSize() const;
+ int LocalSize() const override;
LocalParametrizationBasePtr getLocalParametrization() const;
};
diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 5c834b404592d72913abb20b95a40679b0f83e4a..691c44166da222b893c1d8acd428c7e63da71539 100644
--- a/include/core/ceres_wrapper/qr_manager.h
+++ b/include/core/ceres_wrapper/qr_manager.h
@@ -17,10 +17,10 @@ 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_;
+ Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_;
+ Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_;
+ Eigen::SparseMatrixd A_;
+ Eigen::VectorXd b_;
std::map<StateBlockPtr, unsigned int> sb_2_col_;
std::map<FactorBasePtr, unsigned int> fac_2_row_;
bool any_state_block_removed_;
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
new file mode 100644
index 0000000000000000000000000000000000000000..4bf6f7de661059e7c9f96e7f12bb444f18bed013
--- /dev/null
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -0,0 +1,216 @@
+#ifndef CERES_MANAGER_H_
+#define CERES_MANAGER_H_
+
+//Ceres includes
+#include "ceres/jet.h"
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+//wolf includes
+#include "core/solver/solver_manager.h"
+#include "core/utils/params_server.h"
+
+namespace ceres {
+typedef std::shared_ptr<CostFunction> CostFunctionPtr;
+}
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(SolverCeres);
+WOLF_PTR_TYPEDEFS(LocalParametrizationWrapper);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
+
+struct ParamsCeres : public ParamsSolver
+{
+ bool update_immediately = false;
+ int min_num_iterations = 1;
+ ceres::Solver::Options solver_options;
+ ceres::Problem::Options problem_options;
+ ceres::Covariance::Options covariance_options;
+
+ ParamsCeres() :
+ ParamsSolver()
+ {
+ loadHardcodedValues();
+ }
+
+ ParamsCeres(std::string _unique_name, const ParamsServer& _server) :
+ ParamsSolver(_unique_name, _server)
+ {
+ loadHardcodedValues();
+
+ // stop solver whenever the problem is updated (via ceres::iterationCallback)
+ update_immediately = _server.getParam<bool>(prefix + "update_immediately");
+ if (update_immediately)
+ min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations");
+
+ // ceres solver options
+ solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations");
+ solver_options.num_threads = _server.getParam<int>(prefix + "n_threads");
+ covariance_options.num_threads = _server.getParam<int>(prefix + "n_threads");
+ }
+
+ void loadHardcodedValues()
+ {
+ solver_options = ceres::Solver::Options();
+ solver_options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH;
+ solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; //ceres::DOGLEG;
+ problem_options = ceres::Problem::Options();
+ covariance_options = ceres::Covariance::Options();
+ problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
+ problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+
+ #if CERES_VERSION_MINOR >= 13
+ covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+ covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
+ #elif CERES_VERSION_MINOR >= 10
+ covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
+ #else
+ covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+ #endif
+ covariance_options.apply_loss_function = false;
+ }
+
+ ~ParamsCeres() override = default;
+};
+
+class SolverCeres : public SolverManager
+{
+ protected:
+
+ std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
+ std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
+
+ // this map is only for handling automatic destruction of localParametrizationWrapper objects
+ std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
+
+ ceres::Solver::Summary summary_;
+ std::unique_ptr<ceres::Problem> ceres_problem_;
+ std::unique_ptr<ceres::Covariance> covariance_;
+
+ ParamsCeresPtr params_ceres_;
+
+ public:
+
+ SolverCeres(const ProblemPtr& _wolf_problem);
+
+ SolverCeres(const ProblemPtr& _wolf_problem,
+ const ParamsCeresPtr& _params);
+
+ WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres);
+
+ ~SolverCeres() override;
+
+ ceres::Solver::Summary getSummary();
+
+ std::unique_ptr<ceres::Problem>& getCeresProblem();
+
+ bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks
+ = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
+
+ bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
+
+ bool hasConverged() override;
+
+ SizeStd iterations() override;
+
+ double initialCost() override;
+
+ double finalCost() override;
+
+ double totalTime() override;
+
+ ceres::Solver::Options& getSolverOptions();
+
+ const Eigen::SparseMatrixd computeHessian() const;
+
+ virtual int numStateBlocksDerived() const override;
+
+ virtual int numFactorsDerived() const override;
+
+ protected:
+
+ bool checkDerived(std::string prefix="") const override;
+
+ std::string solveDerived(const ReportVerbosity report_level) override;
+
+ void addFactorDerived(const FactorBasePtr& fac_ptr) override;
+
+ void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
+
+ void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
+
+ void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
+
+ void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
+
+ void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
+
+ ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
+
+ bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
+
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
+
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
+
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) override;
+
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
+};
+
+inline ceres::Solver::Summary SolverCeres::getSummary()
+{
+ return summary_;
+}
+
+inline std::unique_ptr<ceres::Problem>& SolverCeres::getCeresProblem()
+{
+ return ceres_problem_;
+}
+
+inline ceres::Solver::Options& SolverCeres::getSolverOptions()
+{
+ return params_ceres_->solver_options;
+}
+
+inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
+{
+ return fac_2_residual_idx_.count(fac_ptr) == 1 and
+ fac_2_costfunction_.count(fac_ptr) == 1;
+}
+
+inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
+{
+ if (state_blocks_.count(state_ptr) == 0)
+ return false;
+ return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+}
+
+inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
+{
+ if (state_blocks_.count(st) == 0)
+ return false;
+ return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st));
+};
+
+inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
+{
+ return state_blocks_local_param_.count(st) == 1;
+};
+
+inline int SolverCeres::numStateBlocksDerived() const
+{
+ return ceres_problem_->NumParameterBlocks();
+}
+
+inline int SolverCeres::numFactorsDerived() const
+{
+ return ceres_problem_->NumResidualBlocks();
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h
index 66f49b9c8f868e84f4223408ef6f85980e89bf1b..4b7171f77eaf09cc902ae24aea7dd49c5a4adfca 100644
--- a/include/core/ceres_wrapper/sparse_utils.h
+++ b/include/core/ceres_wrapper/sparse_utils.h
@@ -14,32 +14,32 @@
namespace wolf
{
-void eraseBlockRow(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
+void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
{
- A.middleRows(_row,_n_rows) = Eigen::SparseMatrixs(_n_rows,A.cols());
+ A.middleRows(_row,_n_rows) = Eigen::SparseMatrixd(_n_rows,A.cols());
A.makeCompressed();
}
-void eraseBlockRow(Eigen::SparseMatrix<Scalar, Eigen::ColMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
+void eraseBlockRow(Eigen::SparseMatrix<double, 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.prune([&](int i, int, double) { 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)
+void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
{
- A.middleCols(_col,_n_cols) = Eigen::SparseMatrixs(A.rows(),_n_cols);
+ A.middleCols(_col,_n_cols) = Eigen::SparseMatrixd(A.rows(),_n_cols);
A.makeCompressed();
}
-void eraseBlockCol(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
+void eraseBlockCol(Eigen::SparseMatrix<double, 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.prune([&](int, int j, double) { 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)
+void assignSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_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++)
@@ -49,7 +49,7 @@ void assignSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_O
}
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)
+void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_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++)
@@ -59,7 +59,7 @@ void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_Opti
}
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)
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_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++)
@@ -68,22 +68,22 @@ void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_O
original.makeCompressed();
}
-void assignBlockRow(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& ins, const unsigned int& _row)
+void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen::SparseMatrix<double, 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)
+Eigen::SparseMatrixd createBlockDiagonal(const std::vector<Eigen::MatrixXd>& _diag_blocs)
{
unsigned int dim = _diag_blocs.front().rows();
unsigned int size = dim * _diag_blocs.size();
- Eigen::SparseMatrixs M(size,size);
+ Eigen::SparseMatrixd M(size,size);
unsigned int pos = 0;
- for (const Eigen::MatrixXs& omega_k : _diag_blocs)
+ for (const Eigen::MatrixXd& omega_k : _diag_blocs)
{
insertSparseBlock(omega_k, M, pos, pos);
pos += dim;
@@ -95,12 +95,12 @@ Eigen::SparseMatrixs createBlockDiagonal(const std::vector<Eigen::MatrixXs>& _di
}
template<int _Options, typename _StorageIndex>
-void getDiagonalBlocks(const Eigen::SparseMatrix<Scalar,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXs>& diag_, const unsigned int& dim)
+void getDiagonalBlocks(const Eigen::SparseMatrix<double,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXd>& 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)));
+ diag_.push_back(Eigen::MatrixXd(_M.block(i,i,dim,dim)));
}
} // namespace wolf
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 8152d007c9e8113580ae37e7b33debbf5b30127d..edd07448729ba69ddda1fdc447b2610c22cd298c 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -25,42 +25,68 @@ 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.
+ * This template class implements a generic factory as a singleton.
*
* \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.
+ * \param TypeInput variadic type for the input arguments.
+ *
+ * ### Template specialization
*
* - 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.
+ * The returned data is always a shared 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
+ * - ParamsSensorBase: the Factory creates intrinsic parameters deriving from ParamsSensorBase and returns base pointers ````ParamsSensorBasePtr```` 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.
+ * - The class is variadic-templatized on the types 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).
+ * - ````std::string, wolf::ParamsServer``` is used when nodes are build from parameters in the ParamsServer
+ * - ````YAML::Node```` is used when the input parameter is a YAML node with structured data.
+ * - Any other variadic list of inputs is possible.
+ *
+ * - Examples of specific factories existing in Wolf are:
+ * \code
+ *
+ * // FactorySensor
+ * typedef Factory<SensorBase, // Type of objects to be returned: SensorBasePtr
+ * const std::string&, // Type of the sensor: name of the derived sensor class
+ * const Eigen::VectorXd&, // Sensor extrinsics
+ * const ParamsSensorBasePtr> // Sensor parameters
+ * FactorySensor;
+ *
+ * // FactoryProcessor
+ * typedef Factory<ProcessorBase, // Type of object returned: ProcessorBasePtr
+ * const std::string&, // Type of the processor: name of the derived processor class
+ * const ParamsProcessorBasePtr> // Parameters for creating the processor
+ * FactoryProcessor;
+ *
+ * // AutoConfProcessorFactory
+ * typedef Factory<ProcessorBase, // Type of object returned: ProcessorBasePtr
+ * const std::string&, // Type of the processor: name of the derived processor class
+ * const ParamsServer> // Parameters for creating the processor
+ * AutoConfProcessorFactory;
+ *
+ * // Landmarks from YAML
+ * typedef Factory<LandmarkBase, // Type of node created: LandmarkBasePtr
+ * const YAML::Node&> // YAML node with the lmk params
+ * LandmarkFactory;
+ * \endcode
*
* ### Operation of the factory
*
* #### Rationale
*
- * This factory can create objects of classes deriving from TypeBase.
+ * Once specialized, 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.
+ * > 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 TYPE of object to create is identified with a string. This string matches the name of the derived class.
* - the params may be provided in different forms -- see TypeInput.
*
* The methods to create specific objects are called __creators__.
@@ -72,42 +98,53 @@ namespace wolf
* - 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.
+ * - 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"````
+ * We use a std::string with literally the same text as the derived class name, e.g.:
+ * - ParamsSensorCamera -> ````"ParamsSensorCamera"````
+ * - SensorCamera -> ````"SensorCamera"````
+ * - ParamsSensorLaser2d -> ````"ParamsSensorLaser2d"````
+ * - SensorLaser2d -> ````"SensorLaser2d"````
+ * - LandmarkPolyline2d -> ````"LandmarkPolyline2d"````
* - 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;
+ * typedef Factory<ParamsSensorBase, std::string> FactoryParamsSensor;
+ * typedef Factory<ParamsProcessorBase, std::string> FactoryParamsProcessor;
+ * typedef Factory<LandmarkBase, YAML::Node> FactoryLandmark;
+ * typedef Factory<SensorBase,
+ * const std::string&,
+ * const Eigen::VectorXd&,
+ * const ParamsSensorBasePtr> FactorySensor;
+ *
* \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(),
+ * 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, we internally 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>````.
+ * but this is a private method.
*
- * You can then call the methods you like, e.g. to create a landmark, you use:
+ * The interesting methods are designed static, and are the ones responsible for accessing the Factory instance via .get().
+ *
+ * You must therefore call the methods you like statically, e.g. to create a landmark, you use:
*
* \code
- * LandmarkFactory::get().create(...); // see below for creating objects ...
+ * FactoryLandmark::create(TYPE, args...); // 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.
+ *
+ * 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:
*
@@ -117,13 +154,13 @@ namespace wolf
*
* 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.
+ * - ````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)
+ * static ParamsSensorBasePtr create(const std::string& _intrinsics_dot_yaml)
+ * static LandmarkBasePtr create(const YAML::Node& _lmk_yaml_node)
* \endcode
*
* See further down for an implementation example.
@@ -137,7 +174,7 @@ namespace wolf
* that knows how to create your specific object, e.g.:
*
* \code
- * LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
+ * FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* \endcode
*
* #### Automatic registration
@@ -145,10 +182,10 @@ namespace wolf
* For example, in sensor_camera_yaml.cpp we find the line:
*
* \code
- * const bool registered_camera_intr = IntrinsicsFactory::get().registerCreator("CAMERA", createIntrinsicsCamera);
+ * const bool registered_camera_intr = FactoryParamsSensor::registerCreator("ParamsSensorCamera", createParamsSensorCamera);
* \endcode
*
- * which is a static invocation (i.e., it is placed at global scope outside of the IntrinsicsCamera class).
+ * which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class).
*
* Therefore, at application level, all objects that have a .cpp file compiled are automatically registered.
*
@@ -156,33 +193,33 @@ namespace wolf
* 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");
+ * Factory<MyTypeBase, MyTypeInput>::unregisterCreator("ParamsSensorCamera");
* \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:
+ * 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);
+ * LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::create("LandmarkPolyline2d", 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);
+ * LandmarkBasePtr lmk_ptr = FactoryLandmark::create("LandmarkPolyline2d", lmk_yaml_node);
* \endcode
*
* ### Examples
- * #### Example 1: Writing the creator of LandmarkPolyline2D from a YAML node
+ * #### Example 1: Writing the creator of LandmarkPolyline2d from a YAML node
*
- * You can find this code in the landmark_polyline_2D.cpp file.
+ * 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)
+ * 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>();
@@ -190,43 +227,43 @@ namespace wolf
* 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);
+ * Eigen::MatrixXd points(2,npoints);
* for (unsigned int i = 0; i < npoints; i++)
* {
- * points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2s>();
+ * points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2d>();
* }
*
* // Create a new landmark
- * LandmarkBasePtr lmk_ptr = new LandmarkPolyline2D(points, first_defined, last_defined, first_id);
+ * LandmarkBasePtr lmk_ptr = std::make_shared<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
+ * #### Example 2: Registering the creator of LandmarkPolyline2d from a YAML node
*
- * You can find this code in the landmark_polyline_2D.cpp file.
+ * 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);
+ * const bool registered_lmk_polyline_2d = FactoryLandmark::registerCreator("LandmarkPolyline2d", 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.
+ * - FactoryParamsSensor: typedef of this template to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
+ * - FactoryParamsProcessor: typedef of this template to create processor params structs deriving from ParamsProcessorBase directly from YAML files.
+ * - FactoryLandmark: 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.
+ * - FactorySensor: to create sensors
+ * - FactoryProcessor: to create processors.
* - Problem::installSensor() : to install sensors in WOLF Problem.
* - Problem::installProcessor() : to install processors in WOLF Problem.
*
@@ -234,17 +271,21 @@ namespace wolf
template<class TypeBase, typename... TypeInput>
class Factory
{
+ private:
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();
+ static bool registerCreator(const std::string& _type, CreatorCallback createFn);
+ static bool unregisterCreator(const std::string& _type);
+ static TypeBasePtr create(const std::string& _type, TypeInput... _input);
+ std::string getClass() const;
private:
CallbackMap callbacks_;
@@ -252,7 +293,7 @@ class Factory
// Singleton ---------------------------------------------------
// This class is a singleton. The code below guarantees this.
// See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
- public:
+ private:
static Factory& get();
public:
Factory(const Factory&) = delete;
@@ -265,11 +306,11 @@ class 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;
+ bool reg = get().callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
if (reg)
- std::cout << std::setw(22) << std::left << getClass() << " <-- registered " << _type << std::endl;
+ std::cout << std::setw(26) << std::left << get().getClass() << " <-- registered " << _type << std::endl;
else
- std::cout << std::setw(22) << std::left << getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
+ std::cout << std::setw(26) << std::left << get().getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
return reg;
}
@@ -277,17 +318,17 @@ inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string&
template<class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string& _type)
{
- return callbacks_.erase(_type) == 1;
+ return get().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);
+ typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
- if (creator_callback_it == callbacks_.end())
+ if (creator_callback_it == get().callbacks_.end())
// not found
- throw std::runtime_error(getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
+ throw std::runtime_error(get().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)...);
@@ -301,7 +342,7 @@ inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
}
template<class TypeBase, typename... TypeInput>
-inline std::string Factory<TypeBase, TypeInput...>::getClass()
+inline std::string Factory<TypeBase, TypeInput...>::getClass() const
{
return "Factory<class TypeBase>";
}
@@ -314,50 +355,16 @@ 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;
+ const YAML::Node&> FactoryLandmark;
template<>
-inline std::string LandmarkFactory::getClass()
+inline std::string FactoryLandmark::getClass() const
{
- return "LandmarkFactory";
+ return "FactoryLandmark";
}
-// Frames
-class TimeStamp;
-} // namespace wolf
-#include "core/frame/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))
@@ -372,10 +379,6 @@ inline std::string FrameFactory::getClass()
# 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/core/common/node_base.h b/include/core/common/node_base.h
index 69d8145eaa5b5a591fae3f57c3e2954df93f9f62..dcd7d37176c2eb4c36aea7c8b342aa064b43a896 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -3,6 +3,7 @@
// Wolf includes
#include "core/common/wolf.h"
+#include "core/utils/check_log.h"
namespace wolf {
@@ -27,10 +28,10 @@ namespace wolf {
* - "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"
+ * - "SensorCamera"
+ * - "SensorLaser2d"
+ * - "LandmarkPoint3d"
+ * - "ProcessorLaser2d"
*
* please refer to each base class derived from NodeLinked for better examples of their types.
*
@@ -53,6 +54,7 @@ namespace wolf {
class NodeBase
{
+ friend Problem;
private:
static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
@@ -63,11 +65,12 @@ class NodeBase
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_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().
+ virtual void setProblem(ProblemPtr _prob_ptr);
public:
NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
@@ -82,7 +85,6 @@ class NodeBase
void setType(const std::string& _type);
void setName(const std::string& _name);
ProblemPtr getProblem() const;
- virtual void setProblem(ProblemPtr _prob_ptr);
};
} // namespace wolf
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..b9a4886c290fb9c1dcb4889d55b6d1523c42df78
--- /dev/null
+++ b/include/core/common/params_base.h
@@ -0,0 +1,19 @@
+#ifndef PARAMS_BASE_H_
+#define PARAMS_BASE_H_
+
+#include "core/utils/params_server.h"
+
+namespace wolf {
+ struct ParamsBase
+ {
+ ParamsBase() = default;
+ ParamsBase(std::string _unique_name, const ParamsServer&)
+ {
+ //
+ }
+
+ virtual ~ParamsBase() = default;
+ virtual std::string print() const = 0;
+ };
+}
+#endif
diff --git a/include/core/common/time_stamp.h b/include/core/common/time_stamp.h
index 947fb19db1ba74a6cd6839695ba2bd8634fc40cb..2643f13c8f9629dce921726d6dfacf3171c3417f 100644
--- a/include/core/common/time_stamp.h
+++ b/include/core/common/time_stamp.h
@@ -23,6 +23,7 @@ class TimeStamp
{
protected:
unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
+ bool is_valid_; // time stamp has a valid value
public:
/** \brief Constructor
@@ -44,7 +45,7 @@ class TimeStamp
* Constructor with arguments
*
*/
- TimeStamp(const Scalar& _ts);
+ TimeStamp(const double& _ts);
/** \brief Constructor from sec and nsec
*
@@ -60,6 +61,16 @@ class TimeStamp
*/
~TimeStamp();
+ /** \brief Value of time stamp is valid
+ *
+ */
+ static TimeStamp Invalid ( );
+ bool ok ( ) const;
+ void setOk ( );
+ void setNOk ( );
+
+ static TimeStamp Now();
+
/** \brief Time stamp to now
*/
void setToNow();
@@ -82,14 +93,14 @@ class TimeStamp
* Sets time stamp to a given value passed as a scalar_t (seconds)
*
*/
- void set(const Scalar& ts);
+ void set(const double& ts);
/** \brief Get time stamp
*
* Returns time stamp
*
*/
- Scalar get() const;
+ double get() const;
/** \brief Get time stamp (only seconds)
*
@@ -110,7 +121,7 @@ class TimeStamp
* Assignement operator
*
*/
- void operator =(const Scalar& ts);
+ void operator =(const double& ts);
/** \brief Assignement operator
*
@@ -139,29 +150,29 @@ class TimeStamp
/** \brief Add-assign operator given a scalar_t (seconds)
*/
- void operator +=(const Scalar& dt);
+ void operator +=(const double& dt);
/** \brief Add-assign operator given a scalar_t (seconds)
*/
- TimeStamp operator +(const Scalar& dt) const;
+ TimeStamp operator +(const double& dt) const;
/** \brief Substraction-assign operator given a scalar_t (seconds)
*/
- void operator -=(const Scalar& dt);
+ void operator -=(const double& dt);
/** \brief difference operator
*
* difference operator that returns a scalar_t (seconds)
*
*/
- TimeStamp operator -(const Scalar& ts) const;
+ TimeStamp operator -(const double& ts) const;
/** \brief difference operator
*
* difference operator that returns a Timestamp (seconds)
*
*/
- Scalar operator -(const TimeStamp& ts) const;
+ double operator -(const TimeStamp& ts) const;
/** \brief Prints time stamp to a given ostream
*
@@ -174,24 +185,47 @@ class TimeStamp
};
-inline void TimeStamp::set(const Scalar& ts)
+inline wolf::TimeStamp TimeStamp::Invalid ( )
{
- time_stamp_nano_ = (ts > 0 ? (unsigned long int)(ts*NANOSECS) : 0);
+ return TimeStamp(-1.0);
+}
+
+inline bool TimeStamp::ok ( ) const
+{
+ return is_valid_;
+}
+
+inline void TimeStamp::setOk ( )
+{
+ is_valid_ = true;
+}
+
+inline void TimeStamp::setNOk ( )
+{
+ is_valid_ = false;
+}
+
+inline void TimeStamp::set(const double& ts)
+{
+ time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts*NANOSECS) : 0);
+ is_valid_ = (ts >= 0);
}
inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec)
{
time_stamp_nano_ = sec*NANOSECS+nanosec;
+ is_valid_ = true;
}
inline void TimeStamp::set(const timeval& ts)
{
time_stamp_nano_ = (unsigned long int)(ts.tv_sec*NANOSECS) + (unsigned long int)(ts.tv_usec*1000);
+ is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0);
}
-inline Scalar TimeStamp::get() const
+inline double TimeStamp::get() const
{
- return ((Scalar)( time_stamp_nano_))*1e-9;
+ return ((double)( time_stamp_nano_))*1e-9;
}
inline unsigned long int TimeStamp::getSeconds() const
@@ -207,11 +241,13 @@ inline unsigned long int TimeStamp::getNanoSeconds() const
inline void TimeStamp::operator =(const TimeStamp& ts)
{
time_stamp_nano_ = ts.time_stamp_nano_;
+ is_valid_ = ts.is_valid_;
}
-inline void TimeStamp::operator =(const Scalar& ts)
+inline void TimeStamp::operator =(const double& ts)
{
time_stamp_nano_ = (unsigned long int)(ts*NANOSECS);
+ is_valid_ = (ts >= 0);
}
inline bool TimeStamp::operator ==(const TimeStamp& ts) const
@@ -244,18 +280,16 @@ inline bool TimeStamp::operator >=(const TimeStamp& ts) const
return (time_stamp_nano_ >= ts.time_stamp_nano_);
}
-inline void TimeStamp::operator +=(const Scalar& dt)
+inline void TimeStamp::operator +=(const double& dt)
{
time_stamp_nano_ += (unsigned long int)(dt*NANOSECS);
}
-inline Scalar TimeStamp::operator -(const TimeStamp& ts) const
+inline double 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
+ return double((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/core/common/wolf.h b/include/core/common/wolf.h
index 872364392a3b86749d2a6cc1d6f984fb40770b7b..97f9a50ff1eb1b0a345461af2fbdce2b81b32491 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -11,33 +11,25 @@
// Enable project-specific definitions and macros
#include "core/internal/config.h"
#include "core/utils/logging.h"
-#include "core/utils/params_server.hpp"
//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
+#include <set>
// 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
*
@@ -60,15 +52,15 @@ typedef std::size_t SizeStd;
namespace Constants{
// Wolf standard tolerance
-const Scalar EPS = 1e-8;
+const double EPS = 1e-8;
// Wolf smmmmall tolerance
-const Scalar EPS_SMALL = 1e-16;
+const double EPS_SMALL = 1e-16;
}
} // namespace wolf
///////////////////////////////////////////
-// Construct types for any scalar defined in the typedef Scalar above
+// Construct types for any scalar defined in the typedef double above
////////////////////////////////////////////
/** \brief Namespace extending Eigen definitions
*
@@ -76,55 +68,32 @@ const Scalar EPS_SMALL = 1e-16;
* 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.
+ * - VectorXd Vector of either double of float, depending on the type \b double, 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 RotationBase<wolf::Scalar, 3> Rotation3Ds; ///< Rotation3D of real Scalar type
-
-// 3. Translations
-typedef Translation<wolf::Scalar, 2> Translation2ds;
-typedef Translation<wolf::Scalar, 3> Translation3ds;
-
-// 4. Transformations
-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;
+typedef Matrix<double, 1, 1> Matrix1d; ///< 1x1 matrix of real double type
+typedef Matrix<double, 5, 5> Matrix5d; ///< 5x5 matrix of real double type
+typedef Matrix<double, 6, 6> Matrix6d; ///< 6x6 matrix of real double type
+typedef Matrix<double, 7, 7> Matrix7d; ///< 7x7 matrix of real double type
+typedef Matrix<double, 8, 8> Matrix8d; ///< 8x8 matrix of real double type
+typedef Matrix<double, 9, 9> Matrix9d; ///< 9x9 matrix of real double type
+typedef Matrix<double, 10, 10> Matrix10d; ///< 10x10 matrix of real double type
+typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type
+typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type
+typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type
+typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type
+typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type
+typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type
+typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type
+
+// 2. Sparse matrix
+typedef SparseMatrix<double> SparseMatrixd;
+
+// 3. Row major matrix
+typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmajor matrix of real double type
}
namespace wolf {
@@ -134,7 +103,7 @@ namespace wolf {
*
* Help:
*
- * The WOLF project implements many template functions using Eigen Matrix and Quaternions, in different versions
+ * The WOLF project implements many template functions using Eigen Matrix and Quaterniond, 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):
@@ -215,9 +184,14 @@ struct MatrixSizeCheck
#define WOLF_LIST_TYPEDEFS(ClassName) \
class ClassName; \
- typedef std::list<ClassName##Ptr> ClassName##PtrList; \
+ typedef std::list<ClassName##Ptr> ClassName##PtrList; \
typedef ClassName##PtrList::iterator ClassName##Iter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter;
+ typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
+ typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
@@ -230,6 +204,10 @@ WOLF_PTR_TYPEDEFS(NodeBase);
// Problem
WOLF_PTR_TYPEDEFS(Problem);
+// Tree Manager
+WOLF_PTR_TYPEDEFS(TreeManagerBase);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerBase);
+
// Hardware
WOLF_PTR_TYPEDEFS(HardwareBase);
@@ -237,8 +215,8 @@ WOLF_PTR_TYPEDEFS(HardwareBase);
WOLF_PTR_TYPEDEFS(SensorBase);
WOLF_LIST_TYPEDEFS(SensorBase);
-// - - Intrinsics
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsBase);
+// - - ParamsSensor
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase);
// - Processors
WOLF_PTR_TYPEDEFS(ProcessorBase);
@@ -248,15 +226,19 @@ WOLF_LIST_TYPEDEFS(ProcessorBase);
WOLF_PTR_TYPEDEFS(ProcessorMotion);
// - - Processor params
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsBase);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorBase);
// Trajectory
WOLF_PTR_TYPEDEFS(TrajectoryBase);
+
// - Frame
WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
+class TimeStamp;
+typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
+
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
WOLF_LIST_TYPEDEFS(CaptureBase);
@@ -294,87 +276,9 @@ inline bool file_exists(const std::string& _name)
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);
+inline const Eigen::Vector3d gravity(void) {
+ return Eigen::Vector3d(0,0,-9.806);
}
-
-#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;
-}
-
-//===================================================
-struct ParamsBase
-{
- ParamsBase() = default;
- ParamsBase(std::string _unique_name, const paramsServer&)
- {
- //
- }
-
- virtual ~ParamsBase() = default;
-};
} // namespace wolf
#endif /* WOLF_H_ */
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index 5435fa053874bca290f332d1d9bb2a085dea8e8b..93daadcee28914f0688d4d2e2169614929da41a0 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -18,59 +18,42 @@ class FactorAnalytic: public FactorBase
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;
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr = nullptr,
+ StateBlockPtr _state2Ptr = nullptr,
+ StateBlockPtr _state3Ptr = nullptr,
+ StateBlockPtr _state4Ptr = nullptr,
+ StateBlockPtr _state5Ptr = nullptr,
+ StateBlockPtr _state6Ptr = nullptr,
+ StateBlockPtr _state7Ptr = nullptr,
+ StateBlockPtr _state8Ptr = nullptr,
+ StateBlockPtr _state9Ptr = nullptr );
+
+ ~FactorAnalytic() override = default;
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this factor depends
*
**/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
+ 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;
+ std::vector<unsigned int> getStateSizes() const override;
/** \brief Returns the factor residual size
*
@@ -81,57 +64,67 @@ class FactorAnalytic: public FactorBase
/** \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
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// load parameters evaluation value
- std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
+ std::vector<Eigen::Map<const Eigen::VectorXd>> 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]));
+ state_blocks_map_.emplace_back((double*)parameters[i], state_block_sizes_vector_[i]);
// residuals
- Eigen::Map<Eigen::VectorXs> residuals_map((Scalar*)residuals, getSize());
+ Eigen::Map<Eigen::VectorXd> residuals_map((double*)residuals, getSize());
residuals_map = evaluateResiduals(state_blocks_map_);
// also compute jacobians
if (jacobians != nullptr)
{
- std::vector<Eigen::Map<Eigen::MatrixXs>> jacobians_map_;
+ std::vector<Eigen::Map<Eigen::MatrixRowXd>> 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]));
+ jacobians_map_.emplace_back((double*)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
+ jacobians_map_.emplace_back(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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
+ assert(_states_ptr.size() == state_block_sizes_vector_.size());
+
+ // load parameters evaluation value
+ std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
+ for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
+ state_blocks_map_.emplace_back(_states_ptr[i], state_block_sizes_vector_[i]);
+
+ // residuals
+ residual_ = evaluateResiduals(state_blocks_map_);
+
+ // compute jacobians
+ jacobians_.resize(state_block_sizes_vector_.size());
+ evaluateJacobians(state_blocks_map_, jacobians_, std::vector<bool>(state_block_sizes_vector_.size(),true));
};
/** \brief Returns the residual evaluated in the states provided
*
- * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
+ * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
- virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const = 0;
+ virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _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.
+ * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
@@ -139,7 +132,12 @@ class FactorAnalytic: public FactorBase
* \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;
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const = 0;
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const = 0;
/** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
*
@@ -148,14 +146,14 @@ class FactorAnalytic: public FactorBase
* \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;
+ virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const = 0;
/** \brief Returns the jacobians computation method
*
* Returns the jacobians computation method
*
**/
- virtual JacobianMethod getJacobianMethod() const override;
+ JacobianMethod getJacobianMethod() const override;
private:
void resizeVectors();
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index ca8d793574fe29fe8a9b3c4de807fdac40ef42af..68c820c6f09246f6f7c1eba0f68dd61689d53301 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -15,7 +15,7 @@
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>
+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, unsigned int B10 = 0, unsigned int B11 = 0>
class FactorAutodiff : public FactorBase
{
public:
@@ -31,34 +31,41 @@ class FactorAutodiff : public FactorBase
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 unsigned int block10Size = B10;
+ static const unsigned int block11Size = B11;
+ static const unsigned int n_blocks = 12;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9> WolfJet;
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 +
+ B10 + B11> 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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
public:
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
**/
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -75,61 +82,59 @@ class FactorAutodiff : public FactorBase
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>)
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- 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
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
@@ -139,7 +144,7 @@ class FactorAutodiff : public FactorBase
* Returns the residual and jacobians given the state values
*
**/
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -154,39 +159,43 @@ class FactorAutodiff : public FactorBase
parameters[7],
parameters[8],
parameters[9],
+ parameters[10],
+ parameters[11],
residuals);
}
// also compute jacobians
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- jets_9_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -195,75 +204,81 @@ class FactorAutodiff : public FactorBase
/** \brief Updates all jets real part with values of parameters
*
**/
- void updateJetsRealPart(const std::vector<Scalar const*>& parameters) const
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
+ jets_8_[i].a = parameters[8][i];
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i].a = parameters[9][i];
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -275,7 +290,7 @@ class FactorAutodiff : public FactorBase
* Returns a vector of pointers to the state in which this factor depends
*
**/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
@@ -283,7 +298,7 @@ class FactorAutodiff : public FactorBase
/** \brief Returns a vector of the states sizes
*
**/
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
@@ -293,16 +308,539 @@ class FactorAutodiff : public FactorBase
* Returns the residual size
*
**/
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
};
+
+////////////////// SPECIALIZATION 11 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,unsigned int B10>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = B10;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 11;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 + B10> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+// for (unsigned int i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 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>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 10;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+// for (unsigned int i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
{
public:
@@ -317,11 +855,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
static const unsigned int block7Size = B7;
static const unsigned int block8Size = B8;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 9;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
B5 + B6 + B7 + B8> WolfJet;
protected:
@@ -329,21 +869,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -360,62 +901,53 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
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>)
- {
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- 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
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -435,117 +967,117 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -554,7 +1086,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
{
public:
@@ -569,11 +1101,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
static const unsigned int block7Size = B7;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 8;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
B5 + B6 + B7> WolfJet;
protected:
@@ -581,20 +1115,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -610,58 +1145,50 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
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>)
- {
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- 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
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -680,113 +1207,113 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -795,7 +1322,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
{
public:
@@ -810,11 +1337,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 7;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
B5 + B6> WolfJet;
protected:
@@ -822,19 +1351,20 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -849,54 +1379,47 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
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>)
- {
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- 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
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -914,109 +1437,109 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -1025,7 +1548,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1040,11 +1563,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 6;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
B5> WolfJet;
protected:
@@ -1052,18 +1577,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1077,50 +1603,44 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
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>)
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- 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
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -1137,101 +1657,101 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -1240,7 +1760,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1255,28 +1775,31 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 5;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4> WolfJet;
+ typedef ceres::Jet<double, 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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1289,46 +1812,41 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
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>)
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete residuals_jets_;
}
- virtual JacobianMethod getJacobianMethod() const
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -1344,97 +1862,97 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -1443,7 +1961,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1458,27 +1976,30 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 4;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3> WolfJet;
+ typedef ceres::Jet<double, 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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1490,42 +2011,38 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
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>)
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete residuals_jets_;
}
- virtual JacobianMethod getJacobianMethod() const
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -1540,97 +2057,97 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -1639,7 +2156,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1654,26 +2171,29 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 3;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1 + B2> WolfJet;
+ typedef ceres::Jet<double, 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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1684,38 +2204,35 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
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>)
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete residuals_jets_;
}
- virtual JacobianMethod getJacobianMethod() const
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -1729,93 +2246,93 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -1824,7 +2341,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1839,25 +2356,28 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 2;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0 + B1> WolfJet;
+ typedef ceres::Jet<double, 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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1867,34 +2387,32 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
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>)
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- delete jets_0_;
- delete jets_1_;
- delete residuals_jets_;
}
- virtual JacobianMethod getJacobianMethod() const
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -1907,89 +2425,89 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -1998,7 +2516,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// 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
+class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -2013,24 +2531,27 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 1;
static const std::vector<unsigned int> state_block_sizes_;
- typedef ceres::Jet<Scalar, B0> WolfJet;
+ typedef ceres::Jet<double, 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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2039,30 +2560,29 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
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>)
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
- virtual ~FactorAutodiff()
+ ~FactorAutodiff() override
{
- delete jets_0_;
- delete residuals_jets_;
}
- virtual JacobianMethod getJacobianMethod() const
+ JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
- virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
if (jacobians == nullptr)
@@ -2074,85 +2594,85 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
else
{
// update jets real part
- std::vector<Scalar const*> param_vec;
+ std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- residuals_jets_->data());
+ (*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;
+ 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),
+ 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
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ 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
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
assert(_states_ptr.size() == n_blocks);
- // 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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians_[i].data() + row * state_block_sizes_.at(i));
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
{
return state_ptrs_;
}
- virtual std::vector<unsigned int> getStateSizes() const
+ std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
- virtual unsigned int getSize() const
+ unsigned int getSize() const override
{
return RES;
}
@@ -2162,113 +2682,146 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
// STATIC CONST VECTORS INITIALIZATION
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
+// 12 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,unsigned int B10,unsigned int B11>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11};
+// 11 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,unsigned int B10>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10};
// 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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0>::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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
// jacobian_locations_
+// 12 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,unsigned int B10,unsigned int B11>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::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,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10};
+// 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,unsigned int B10>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,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,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9};
// 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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,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,
+ 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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,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,
+ 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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,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,
+ 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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,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};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
} // namespace wolf
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 884f622a8c00a55d20d6cc2022cb90f2f795ec3f..e3dd39803de616a707c2d25a21da04bee6bb9e6e 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -4,6 +4,8 @@
// Forward declarations for node templates
namespace wolf{
class FeatureBase;
+class TrajectoryIter;
+class TrajectoryRevIter;
}
//Wolf includes
@@ -20,10 +22,24 @@ namespace wolf {
*/
typedef enum
{
- FAC_INACTIVE = 0, ///< Factor established with a frame (odometry).
- FAC_ACTIVE = 1 ///< Factor established with absolute reference.
+ FAC_INACTIVE = 0, ///< Inactive factor, it is not included in the SLAM problem.
+ FAC_ACTIVE = 1 ///< Normal active factor, playing its role in the solution.
} FactorStatus;
+/** \brief Enumeration of factor topologies
+ *
+ * You may add items to this list as needed. Be concise with names, and document your entries.
+ */
+typedef enum
+{
+ TOP_ABS, ///< absolute factor
+ TOP_MOTION, ///<motion factor, e.g. odometry, IMU
+ TOP_LOOP, ///<loop closure factor
+ TOP_LMK, ///<landmark observation factor
+ TOP_GEOM, ///<some geometric relation, e.g. distance
+ TOP_OTHER ///<other topologies
+} FactorTopology;
+
/** \brief Enumeration of jacobian computation method
*
* You may add items to this list as needed. Be concise with names, and document your entries.
@@ -38,53 +54,78 @@ typedef enum
//class FactorBase
class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBase>
{
+ friend FeatureBase;
private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ 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
-
+ FactorTopology topology_; ///< topology of factor
+ FactorStatus status_; ///< status of factor
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
+ FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
+ CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
+ FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
+ LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
+ ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_sqrt_information_upper_;///< the squared root information matrix ///< ProcessorBase pointer list
+
+ void setProblem(ProblemPtr) override final;
public:
- /** \brief Constructor of category FAC_ABSOLUTE
+ /** \brief Default constructor.
+ *
+ * IMPORTANT: "other" means "of another branch of the wolf tree".
+ * You should only provide a non-nullptr in frame/capture/feature_other_ptr in case of a frame/capture/feature involved in this factor
+ * that does not located in the same branch.
**/
FactorBase(const std::string& _tp,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
- /** \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);
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
- virtual ~FactorBase() = default;
- virtual void remove();
+
+ ~FactorBase() override = default;
+
+ virtual void remove(bool viral_remove_empty_parent=false);
unsigned int id() const;
+ /** \brief get the main topological characteristic
+ **/
+ virtual FactorTopology getTopology() const final
+ {
+ return topology_;
+ }
+
/** \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;
+ virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _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;
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const = 0;
/** \brief Returns the jacobians computation method
**/
@@ -98,27 +139,30 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
**/
virtual std::vector<unsigned int> getStateSizes() const = 0;
- /** \brief Returns a reference to the feature measurement
+ /** \brief Returns a reference to the measurement
**/
- virtual const Eigen::VectorXs& getMeasurement() const;
+ virtual const Eigen::VectorXd& getMeasurement() const;
- /** \brief Returns a reference to the feature measurement covariance
+ /** \brief Returns a reference to the measurement square root information
**/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const;
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
+ virtual const Eigen::MatrixXd& 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 a pointer to its frame
+ **/
+ FrameBasePtr getFrame() const;
+
+ /** \brief Returns a pointer to its capture's sensor
+ **/
+ SensorBasePtr getSensor() const;
+
/** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const = 0;
@@ -133,53 +177,72 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Gets the apply loss function flag
*/
- bool getApplyLossFunction();
-
- /** \brief Gets the apply loss function flag
- */
- void setApplyLossFunction(const bool _apply);
+ bool getApplyLossFunction() const;
- /** \brief Returns a pointer to the frame constrained to
+ /** \brief Returns a pointer to the first frame constrained to
**/
- FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
- void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
+ FrameBasePtr getFrameOther() const;
- /** \brief Returns a pointer to the frame constrained to
+ /** \brief Returns a pointer to the first capture constrained to
**/
- CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
- void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
+ CaptureBasePtr getCaptureOther() const;
- /** \brief Returns a pointer to the feature constrained to
+ /** \brief Returns a pointer to the first feature constrained to
**/
- FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
- void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
+ FeatureBasePtr getFeatureOther() const;
- /** \brief Returns a pointer to the landmark constrained to
+ /** \brief Returns a pointer to the first landmark constrained to
**/
- LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
- void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
+ LandmarkBasePtr getLandmarkOther() const;
+
+ // get pointer lists to other nodes
+ FrameBaseWPtrList getFrameOtherList() const { return frame_other_list_; }
+ CaptureBaseWPtrList getCaptureOtherList() const { return capture_other_list_; }
+ FeatureBaseWPtrList getFeatureOtherList() const { return feature_other_list_; }
+ LandmarkBaseWPtrList getLandmarkOtherList() const { return landmark_other_list_; }
+
+ bool hasFrameOther(const FrameBasePtr& _frm_other) const;
+ bool hasCaptureOther(const CaptureBasePtr& _cap_other) const;
+ bool hasFeatureOther(const FeatureBasePtr& _ftr_other) const;
+ bool hasLandmarkOther(const LandmarkBasePtr& _lmk_other) const;
+ public:
/**
* @brief getProcessor
* @return
*/
ProcessorBasePtr getProcessor() const;
+ void link(FeatureBasePtr ftr);
+ template<typename classType, typename... T>
+ static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_ptr, T&&... all);
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
+ private:
+
+ void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
+
/**
* @brief setProcessor
* @param _processor_ptr
*/
void setProcessor(const ProcessorBasePtr& _processor_ptr);
-
- void link(FeatureBasePtr);
- template<typename classType, typename... T>
- static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
-
-// 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
};
}
@@ -194,34 +257,14 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
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);
-// }
-//}
-
template<typename classType, typename... T>
-std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
+std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
{
- FactorBasePtr ctr = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> ctr = std::make_shared<classType>(std::forward<T>(all)...);
ctr->link(_ftr_ptr);
return ctr;
}
-
inline unsigned int FactorBase::id() const
{
return factor_id_;
@@ -237,11 +280,43 @@ inline FactorStatus FactorBase::getStatus() const
return status_;
}
-inline bool FactorBase::getApplyLossFunction()
+inline bool FactorBase::getApplyLossFunction() const
{
return apply_loss_function_;
}
+inline FrameBasePtr FactorBase::getFrameOther() const
+{
+ if (frame_other_list_.empty()) return nullptr;
+ if (frame_other_list_.front().expired()) return nullptr;
+
+ return frame_other_list_.front().lock();
+}
+
+inline CaptureBasePtr FactorBase::getCaptureOther() const
+{
+ if (capture_other_list_.empty()) return nullptr;
+ if (capture_other_list_.front().expired()) return nullptr;
+
+ return capture_other_list_.front().lock();
+}
+
+inline FeatureBasePtr FactorBase::getFeatureOther() const
+{
+ if (feature_other_list_.empty()) return nullptr;
+ if (feature_other_list_.front().expired()) return nullptr;
+
+ return feature_other_list_.front().lock();
+}
+
+inline LandmarkBasePtr FactorBase::getLandmarkOther() const
+{
+ if (landmark_other_list_.empty()) return nullptr;
+ if (landmark_other_list_.front().expired()) return nullptr;
+
+ return landmark_other_list_.front().lock();
+}
+
inline ProcessorBasePtr FactorBase::getProcessor() const
{
return processor_ptr_.lock();
@@ -252,5 +327,15 @@ inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
processor_ptr_ = _processor_ptr;
}
+inline const Eigen::VectorXd& FactorBase::getMeasurement() const
+{
+ return measurement_;
+}
+
+inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const
+{
+ return measurement_sqrt_information_upper_;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 2eed31556681df9d1c6f07150627c8689870c144..58d0570dd25bf480cc9ca02ef6a4de091a6155b3 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -24,23 +24,63 @@ class FactorBlockAbsolute : public FactorAnalytic
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
+ Eigen::MatrixXd 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,
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
+ FactorAnalytic("FactorBlockAbsolute",
+ TOP_ABS,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _sb_ptr),
+ sb_size_(_sb_ptr->getSize()),
+ sb_constrained_start_(0),
+ sb_constrained_size_(sb_size_)
+ {
+ assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
+
+ // precompute Jacobian (Identity)
+ J_ = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_size_);
+ }
+
+ /** \brief Constructor for segment of state block
+ *
+ * \param _sb_ptr the constrained state block
+ * \param _start_idx the index of the first state element that is constrained
+ * \param _start_idx the size of the state segment that is constrained, -1 = all the
+ *
+ */
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
+ unsigned int _start_idx,
+ int _size,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorBlockAbsolute",
+ TOP_ABS,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
_apply_loss_function,
_status,
_sb_ptr),
@@ -52,26 +92,26 @@ class FactorBlockAbsolute : public FactorAnalytic
// precompute Jacobian (Identity)
if (sb_constrained_start_ == 0)
- J_ = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_size_);
+ J_ = Eigen::MatrixXd::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_);
+ J_ = Eigen::MatrixXd::Zero(sb_constrained_size_,sb_size_);
+ J_.middleCols(sb_constrained_start_,sb_constrained_size_) = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_constrained_size_);
}
}
- virtual ~FactorBlockAbsolute() = default;
+ ~FactorBlockAbsolute() override = 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
+ * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
- virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const;
+ Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
/** \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.
+ * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
@@ -79,9 +119,12 @@ class FactorBlockAbsolute : public FactorAnalytic
* \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;
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
*
@@ -90,14 +133,14 @@ class FactorBlockAbsolute : public FactorAnalytic
* \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;
+ void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
/** \brief Returns the factor residual size
**/
- virtual unsigned int getSize() const;
+ unsigned int getSize() const override;
};
-inline Eigen::VectorXs FactorBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
+inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size");
@@ -109,9 +152,24 @@ inline Eigen::VectorXs FactorBlockAbsolute::evaluateResiduals(const std::vector<
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
+inline void FactorBlockAbsolute::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(_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::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& 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!");
@@ -124,7 +182,7 @@ inline void FactorBlockAbsolute::evaluateJacobians(
jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
}
-inline void FactorBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
+inline void FactorBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
new file mode 100644
index 0000000000000000000000000000000000000000..bdf76c063df0af5b25d5afe4fbd3f24a8df44605
--- /dev/null
+++ b/include/core/factor/factor_block_difference.h
@@ -0,0 +1,200 @@
+/**
+ * \file factor_block_difference.h
+ *
+ * Created on: Feb 28, 2020
+ * \author: mfourmy
+ */
+
+#ifndef FACTOR_BLOCK_DIFFERENCE_H_
+#define FACTOR_BLOCK_DIFFERENCE_H_
+
+//Wolf includes
+#include "core/factor/factor_analytic.h"
+#include "core/frame/frame_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorBlockDifference);
+
+//class
+class FactorBlockDifference : public FactorAnalytic
+{
+ private:
+ SizeEigen sb1_size_; // the size of the state blocks
+ SizeEigen sb2_size_; // the size of the state blocks
+ SizeEigen sb1_constrained_start_; // the index of the first state element of sb1 that is constrained
+ SizeEigen sb1_constrained_size_; // the size of sb1 the state segment that is constrained
+ SizeEigen sb2_constrained_start_; // the index of the first state element of sb2 that is constrained
+ SizeEigen sb2_constrained_size_; // the size of sb2 the state segment that is constrained
+ Eigen::MatrixXd J_res_sb1_; // Jacobian
+ Eigen::MatrixXd J_res_sb2_; // Jacobian
+
+ public:
+
+ /** \brief Constructor
+ *
+ * \param _sb_ptr the constrained state block
+ *
+ */
+ FactorBlockDifference(const FeatureBasePtr& _feature_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
+ unsigned int _start_idx1 = 0,
+ int _size1 = -1,
+ unsigned int _start_idx2 = 0,
+ int _size2 = -1,
+ ProcessorBasePtr _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorBlockDifference",
+ TOP_GEOM,
+ _feature_ptr,
+ _frame_other,
+ _cap_other,
+ _feat_other,
+ _lmk_other,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _sb1_ptr,
+ _sb2_ptr),
+ sb1_size_(_sb1_ptr->getSize()),
+ sb2_size_(_sb2_ptr->getSize()),
+ sb1_constrained_start_(_start_idx1),
+ sb1_constrained_size_(_size1 == -1 ? sb1_size_ : _size1),
+ sb2_constrained_start_(_start_idx2),
+ sb2_constrained_size_(_size2 == -1 ? sb2_size_ : _size2)
+ {
+ // Check if constrained portion is in the stateblock vector range
+ assert(sb1_constrained_size_+sb1_constrained_start_ <= sb1_size_);
+ assert(sb2_constrained_size_+sb2_constrained_start_ <= sb2_size_);
+ assert(sb1_constrained_size_ == sb2_constrained_size_);
+
+ // precompute Jacobian (Identity with zero columns if the stateblock part is not constrained)
+ J_res_sb1_ = Eigen::MatrixXd::Zero(sb1_constrained_size_,sb1_size_);
+ J_res_sb1_.middleCols(sb1_constrained_start_,sb1_constrained_size_) = Eigen::MatrixXd::Identity(sb1_constrained_size_,sb1_constrained_size_);
+
+ J_res_sb2_ = Eigen::MatrixXd::Zero(sb2_constrained_size_,sb2_size_);
+ J_res_sb2_.middleCols(sb2_constrained_start_,sb1_constrained_size_) = - Eigen::MatrixXd::Identity(sb2_constrained_size_,sb2_constrained_size_);
+ }
+
+ ~FactorBlockDifference() override = default;
+
+ /** \brief Returns the residual evaluated in the states provided
+ *
+ * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
+ *
+ **/
+ Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
+
+ /** \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::VectorXd.
+ * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+ *
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+ *
+ **/
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+
+ /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ *
+ * 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
+ *
+ **/
+ void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
+
+ /** \brief Returns the factor residual size
+ **/
+ unsigned int getSize() const override;
+};
+
+inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
+{
+ // Check measurement and cov sizes
+ assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+ assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().cols());
+ assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().rows());
+ assert(getMeasurement().size() == sb1_constrained_size_);
+ assert(getMeasurement().size() == sb2_constrained_size_);
+
+ // residual: meas -> measurement of the difference between partial stateblocks
+ return getMeasurementSquareRootInformationUpper() * (getMeasurement() - (_st_vector[1].segment(sb2_constrained_start_, sb2_constrained_size_)
+ - _st_vector[0].segment(sb1_constrained_start_, sb1_constrained_size_)));
+}
+
+inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& _jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+ assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
+ assert(_compute_jacobian.size() == 2 && "Wrong number of _compute_jacobian booleans!");
+ assert(_st_vector[0].size() == sb1_size_ && "Wrong StateBlock size");
+ assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
+ assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+ assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
+}
+
+inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& _jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+ assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
+ assert(_compute_jacobian.size() == 2 && "Wrong number of _compute_jacobian booleans!");
+ assert(_st_vector[0].size() == sb1_size_ && "Wrong StateBlock size");
+ assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
+ assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+ assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
+}
+
+inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
+{
+ assert(_pure_jacobians.size() == 1 && "Wrong number of jacobians!");
+
+ _pure_jacobians[0] = J_res_sb1_;
+ _pure_jacobians[1] = J_res_sb2_;
+}
+
+inline unsigned int FactorBlockDifference::getSize() const
+{
+ return sb1_constrained_size_;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/factor/factor_container.h b/include/core/factor/factor_container.h
deleted file mode 100644
index 6ad3e7d0f7ea05b62fc423354843a8e8b6b928c1..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_container.h
+++ /dev/null
@@ -1,143 +0,0 @@
-#ifndef FACTOR_CONTAINER_H_
-#define FACTOR_CONTAINER_H_
-
-//Wolf includes
-#include "core/common/wolf.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/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/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 95c44592da4006391c246e5bd2be1e0caa7ca6b9..72677196437b31bd22d8b72408afbd5f813d7fe8 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -3,135 +3,144 @@
*
* Created on: Oct 27, 2017
* \author: Jeremie Deray
+ * \adapted: Joan Sola - July 2019
*/
#ifndef WOLF_FACTOR_DIFF_DRIVE_H_
#define WOLF_FACTOR_DIFF_DRIVE_H_
//Wolf includes
+#include "core/capture/capture_diff_drive.h"
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
-#include "core/feature/feature_diff_drive.h"
-#include "core/capture/capture_wheel_joint_position.h"
+#include "core/feature/feature_motion.h"
+#include "core/math/rotations.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;
+constexpr std::size_t POSITION_SIZE = 2;
+constexpr std::size_t ORIENTATION_SIZE = 1;
+constexpr std::size_t INTRINSICS_SIZE = 3;
+constexpr std::size_t RESIDUAL_SIZE = 3;
}
-namespace wolf {
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorDiffDrive);
-class FactorDiffDrive :
- public FactorAutodiff< FactorDiffDrive,
- RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
- INTRINSICS_STATE_BLOCK_SIZE >
+class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>
{
- using Base = FactorAutodiff<FactorDiffDrive,
- RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
- INTRINSICS_STATE_BLOCK_SIZE>;
-
-public:
-
- FactorDiffDrive(const FeatureDiffDrivePtr& _ftr_ptr,
- const CaptureWheelJointPositionPtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- const bool _apply_loss_function = false,
- const FactorStatus _status = FAC_ACTIVE) :
- Base("DIFF DRIVE", _capture_origin_ptr->getFrame(), _capture_origin_ptr,
- nullptr, nullptr, _processor_ptr,
- _apply_loss_function, _status,
- _frame_ptr->getP(), _frame_ptr->getO(),
- _capture_origin_ptr->getFrame()->getP(),
- _capture_origin_ptr->getFrame()->getO(),
- _capture_origin_ptr->getFrame()->getV(),
- _capture_origin_ptr->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
- J_delta_calib_(_ftr_ptr->getJacobianFactor())
- {
- //
- }
-
- /**
- * \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 _c1,
- const T* const _p2, const T* const _o2, const T* const _c2,
- T* _residuals) const;
-
- /**
- * \brief Returns the jacobians computation method
- **/
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
-protected:
-
- Eigen::MatrixXs J_delta_calib_;
+ using Base = FactorAutodiff<FactorDiffDrive, // @suppress("Type cannot be resolved")
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>;
+
+ public:
+
+ FactorDiffDrive(const FeatureMotionPtr& _ftr_ptr,
+ const CaptureBasePtr& _capture_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ const bool _apply_loss_function,
+ const FactorStatus _status = FAC_ACTIVE) :
+ Base("FactorDiffDrive",
+ TOP_MOTION,
+ _ftr_ptr,
+ _capture_origin_ptr->getFrame(),
+ _capture_origin_ptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _capture_origin_ptr->getFrame()->getP(),
+ _capture_origin_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _capture_origin_ptr->getSensorIntrinsic()),
+ calib_preint_(_ftr_ptr->getCalibrationPreint()),
+ J_delta_calib_(_ftr_ptr->getJacobianCalibration())
+ {
+ //
+ }
+
+ /**
+ * \brief Default destructor (not recommended)
+ *
+ * Default destructor.
+ *
+ **/
+ ~FactorDiffDrive() override = 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;
+
+ VectorXd residual();
+
+ protected:
+
+ Eigen::Vector3d calib_preint_;
+ Eigen::MatrixXd J_delta_calib_;
};
template<typename T>
-inline bool
-FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
- const T* const _p2, const T* const _o2, const T* const _c2,
- T* _residuals) const
+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
{
- // 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);
+ // MAPS
+ Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals(_residuals);
- Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c1_map(_c1);
- Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c2_map(_c2);
+ Eigen::Map<const Eigen::Matrix<T, POSITION_SIZE, 1> > p1(_p1);
+ Eigen::Map<const Eigen::Matrix<T, POSITION_SIZE, 1> > p2(_p2);
- // Compute corrected delta
+ Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
- /// Is this my delta_preint ?
- const Eigen::Matrix<T, 3, 1> measurement = getMeasurement().cast<T>();
- Eigen::Matrix<T, 3, 1> delta_corrected = measurement + J_delta_calib_.cast<T>() * (c2_map - c1_map);
+ // Correct delta due to changes in the calibration parameters
+ auto delta_corrected = getMeasurement() + J_delta_calib_ * (c - calib_preint_);
- // First 2d pose residual
- Eigen::Matrix<T, 3, 1> delta_predicted;
+ // Predict delta from states
+ Eigen::Matrix<T, 3, 1> delta_predicted;
+ // position
+ delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1);
+ // orientation
+ delta_predicted(2) = pi2pi(_o2[0] - _o1[0]);
- // position
- delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2_map - p1_map);
+ // 2d pose residual
+ residuals = delta_corrected - delta_predicted;
- // orientation
- delta_predicted(2) = _o2[0] - _o1[0];
+ // angle remapping
+ pi2pi(residuals(2));
- // residual
- residuals_map = delta_corrected - delta_predicted;
+ // weighted residual
+ residuals = getMeasurementSquareRootInformationUpper() * residuals;
- // 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;
+}
- return true;
+inline Eigen::VectorXd FactorDiffDrive::residual()
+{
+ VectorXd residual(3);
+ operator ()(getFrameOther() ->getP() ->getState() .data(),
+ getFrameOther() ->getO() ->getState() .data(),
+ getFrame() ->getP() ->getState() .data(),
+ getFrame() ->getO() ->getState() .data(),
+ getCaptureOther() ->getSensorIntrinsic() ->getState() .data(),
+ residual.data());
+ return residual;
}
} // namespace wolf
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_distance_3d.h
similarity index 64%
rename from include/core/factor/factor_autodiff_distance_3D.h
rename to include/core/factor/factor_distance_3d.h
index d4932885e58e941ad787c52a0428aa00ea6eec5e..61f6767a2a9813950fe49e0eda6c200677842881 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_distance_3d.h
@@ -1,30 +1,32 @@
/**
- * \file factor_autodiff_distance_3D.h
+ * \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_
+#ifndef FACTOR_DISTANCE_3d_H_
+#define FACTOR_DISTANCE_3d_H_
#include "core/factor/factor_autodiff.h"
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorAutodiffDistance3D);
+WOLF_PTR_TYPEDEFS(FactorDistance3d);
-class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D, 1, 3, 3>
+class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
{
public:
- FactorAutodiffDistance3D(const FeatureBasePtr& _feat,
+ FactorDistance3d(const FeatureBasePtr& _feat,
const FrameBasePtr& _frm_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff("DISTANCE 3D",
- _frm_other,
+ FactorAutodiff("FactorAutodiffDistance3d",
+ TOP_GEOM,
+ _feat,
+ _frm_other,
nullptr,
nullptr,
nullptr,
@@ -34,10 +36,9 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
_feat->getFrame()->getP(),
_frm_other->getP())
{
- setFeature(_feat);
}
- virtual ~FactorAutodiffDistance3D() { /* nothing */ }
+ ~FactorDistance3d() override { /* nothing */ }
template<typename T>
bool operator () (const T* const _pos1,
@@ -56,10 +57,12 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
norm_squared += (T)1e-8;
}
Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
- Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
- Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
+ // Matrix<T,1,1> dist_meas (getMeasurement().cast<T>());
+ // Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper();
- res = sqrt_info_upper * (dist_meas - dist_exp);
+ // res = sqrt_info_upper * (dist_meas - dist_exp);
+
+ res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
return true;
}
@@ -67,4 +70,4 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
} /* namespace wolf */
-#endif /* FACTOR_AUTODIFF_DISTANCE_3D_H_ */
+#endif /* FACTOR_DISTANCE_3d_H_ */
diff --git a/include/core/factor/factor_feature_dummy.h b/include/core/factor/factor_feature_dummy.h
deleted file mode 100644
index 52831f246cf132bd1c536004e4757baec35b48a8..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_feature_dummy.h
+++ /dev/null
@@ -1,69 +0,0 @@
-#ifndef FACTOR_EPIPOLAR_H
-#define FACTOR_EPIPOLAR_H
-
-#include "core/factor/factor_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorFeatureDummy);
-
-
-class FactorFeatureDummy : public FactorBase
-{
- public:
- FactorFeatureDummy(const FeatureBasePtr& _feature_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorFeatureDummy() = 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 FactorFeatureDummy::FactorFeatureDummy(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 FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr)
-{
- return std::make_shared<FactorFeatureDummy>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
-}
-
-} // namespace wolf
-
-#endif // FACTOR_EPIPOLAR_H
diff --git a/include/core/factor/factor_odom_2D_analytic.h b/include/core/factor/factor_odom_2D_analytic.h
deleted file mode 100644
index 38eaa7fae16f4031ca4cab3dc2209d26440cc132..0000000000000000000000000000000000000000
--- a/include/core/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 "core/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/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
deleted file mode 100644
index e4caa3b2dbd2ed26ef8b23a83443b0662b1ec823..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_pose_2D.h
+++ /dev/null
@@ -1,76 +0,0 @@
-
-#ifndef FACTOR_POSE_2D_H_
-#define FACTOR_POSE_2D_H_
-
-//Wolf includes
-#include "core/factor/factor_autodiff.h"
-#include "core/frame/frame_base.h"
-#include "core/math/rotations.h"
-
-//#include "ceres/jet.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(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/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..4357aac836f852aaa88caa242730262db984c525
--- /dev/null
+++ b/include/core/factor/factor_pose_2d.h
@@ -0,0 +1,83 @@
+
+#ifndef FACTOR_POSE_2d_H_
+#define FACTOR_POSE_2d_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPose2d);
+
+//class
+class FactorPose2d: public FactorAutodiff<FactorPose2d,3,2,1>
+{
+ public:
+ FactorPose2d(FeatureBasePtr _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose2d, 3, 2, 1>("FactorPose2d",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ {
+// std::cout << "created FactorPose2d " << std::endl;
+ }
+
+ ~FactorPose2d() override = 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>();
+ auto& meas = getMeasurement();
+
+ // error
+ Eigen::Matrix<T,3,1> er;
+ er(0) = meas(0) - _p[0];
+ er(1) = meas(1) - _p[1];
+ er(2) = pi2pi(meas(2) - _o[0]);
+
+ // residual
+ Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
+ res = getMeasurementSquareRootInformationUpper() * er;
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXd J(3,3);
+// J.row(0) = ((Jet<double, 3>)(er(0))).v;
+// J.row(1) = ((Jet<double, 3>)(er(1))).v;
+// J.row(2) = ((Jet<double, 3>)(er(2))).v;
+// J.row(0) = ((Jet<double, 3>)(res(0))).v;
+// J.row(1) = ((Jet<double, 3>)(res(1))).v;
+// J.row(2) = ((Jet<double, 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/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
deleted file mode 100644
index d36bcdb32a832695b29bbbe5a0610ded37746218..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_pose_3D.h
+++ /dev/null
@@ -1,58 +0,0 @@
-
-#ifndef FACTOR_POSE_3D_H_
-#define FACTOR_POSE_3D_H_
-
-//Wolf includes
-#include "core/factor/factor_autodiff.h"
-#include "core/frame/frame_base.h"
-#include "core/math/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/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..139b87b65384063540d2546f465180937f646156
--- /dev/null
+++ b/include/core/factor/factor_pose_3d.h
@@ -0,0 +1,68 @@
+
+#ifndef FACTOR_POSE_3d_H_
+#define FACTOR_POSE_3d_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPose3d);
+
+//class
+class FactorPose3d: public FactorAutodiff<FactorPose3d,6,3,4>
+{
+ public:
+
+ FactorPose3d(FeatureBasePtr _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose3d,6,3,4>("FactorPose3d",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ {
+ //
+ }
+
+ ~FactorPose3d() override = 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::Map<const Eigen::Matrix<T, 3, 1>> p(_p);
+ Eigen::Map<const Eigen::Quaternion<T>> q(_o);
+
+ // measurements
+ Eigen::Vector3d p_measured(getMeasurement().data() + 0);
+ Eigen::Quaterniond q_measured(getMeasurement().data() + 3);
+
+ // error
+ Eigen::Matrix<T, 6, 1> er;
+ er.head(3) = p_measured - p;
+ er.tail(3) = q2v(q.conjugate() * q_measured.cast<T>());
+
+ // residual
+ Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
+ res = getMeasurementSquareRootInformationUpper() * er;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..ad35fb6c2d4a317c0f48051dc7920bdea76ab77b
--- /dev/null
+++ b/include/core/factor/factor_pose_3d_with_extrinsics.h
@@ -0,0 +1,138 @@
+#ifndef FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
+#define FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPose3dWithExtrinsics);
+
+//class
+class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>
+{
+ public:
+ FactorPose3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>("FactorPose3dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+ {
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+ }
+
+ ~FactorPose3dWithExtrinsics() override = default;
+
+ template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
+ inline void error(const Eigen::MatrixBase<D1> & w_p_wb,
+ const Eigen::QuaternionBase<D2> & w_q_b,
+ const Eigen::MatrixBase<D3> & b_p_bm,
+ const Eigen::QuaternionBase<D4> & b_q_m,
+ const Eigen::MatrixBase<D5> & w_p_wm,
+ const Eigen::QuaternionBase<D6> & w_q_m,
+ Eigen::MatrixBase<D7> & p_err,
+ Eigen::MatrixBase<D7> & o_err) const;
+
+ inline Eigen::Vector6d error() const;
+
+ template<typename T>
+ bool operator ()(const T* const _p,
+ const T* const _q,
+ const T* const _sp,
+ const T* const _sq,
+ T* _residuals) const;
+};
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
+inline void FactorPose3dWithExtrinsics::error(const Eigen::MatrixBase<D1> & w_p_wb,
+ const Eigen::QuaternionBase<D2> & w_q_b,
+ const Eigen::MatrixBase<D3> & b_p_bm,
+ const Eigen::QuaternionBase<D4> & b_q_m,
+ const Eigen::MatrixBase<D5> & w_p_wm,
+ const Eigen::QuaternionBase<D6> & w_q_m,
+ Eigen::MatrixBase<D7> & p_err,
+ Eigen::MatrixBase<D7> & o_err) const
+{
+ // Transformation definitions:
+ // w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame)
+ // w_p_wb, w_q_b: world to robot base frame
+ // b_p_bm, b_q_m: base to measurement frame (extrinsics)
+ // Error function:
+ // err = w_T_m |-| (w_T_b*b_T_m)
+
+ p_err = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
+ o_err = q2v((w_q_b * b_q_m).conjugate() * w_q_m);
+}
+
+
+inline Eigen::Vector6d FactorPose3dWithExtrinsics::error() const
+{
+ // get current frame and extrinsics estimates
+ const Eigen::Vector3d w_p_wb = (getFeature()->getFrame()->getP()->getState());
+ const Eigen::Quaterniond w_q_b (getFeature()->getFrame()->getO()->getState().data());
+ const Eigen::Vector3d b_p_bm = (getFeature()->getCapture()->getSensorP()->getState());
+ const Eigen::Quaterniond b_q_m (getFeature()->getCapture()->getSensorO()->getState().data());
+
+ // measurements
+ Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements
+ Eigen::Quaterniond w_q_m (getMeasurement().data() + 3); // measurements
+
+ Vector6d err;
+ Eigen::Map<Vector3d> p_err(err.data() + 0);
+ Eigen::Map<Vector3d> o_err(err.data() + 3);
+
+ error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm, w_q_m, p_err, o_err);
+
+ return err;
+}
+
+
+template<typename T>
+inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
+ const T* const _q,
+ const T* const _sp,
+ const T* const _sq,
+ T* _residuals) const
+{
+ // MAPS
+ Eigen::Map<const Eigen::Matrix<T,3,1> > w_p_wb(_p);
+ Eigen::Map<const Eigen::Quaternion<T> > w_q_b(_q);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > b_p_bm(_sp);
+ Eigen::Map<const Eigen::Quaternion<T> > b_q_m(_sq);
+
+ // measurements
+ Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements
+ Eigen::Quaterniond w_q_m (getMeasurement().data() + 3); // measurements
+
+ Eigen::Matrix<T, 6, 1> err; // error
+ Eigen::Map<Matrix<T, 3, 1> > p_err(err.data() + 0);
+ Eigen::Map<Matrix<T, 3, 1> > o_err(err.data() + 3);
+ error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm.cast<T>(), w_q_m.cast<T>(), p_err, o_err);
+
+ // Residuals
+ Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
+ res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 477db6368bb4217b41e154095cbca3bd2c00cf6f..3122e80977c9b0816ca6eb42afa4db494d7c2ea5 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -23,14 +23,27 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
{
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)
+ FactorQuaternionAbsolute(FeatureBasePtr _ftr_ptr,
+ StateBlockPtr _sb_ptr,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _sb_ptr)
{
//
}
- virtual ~FactorQuaternionAbsolute() = default;
+ ~FactorQuaternionAbsolute() override = default;
template<typename T>
bool operator ()(const T* const _o, T* _residuals) const;
@@ -45,7 +58,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
Eigen::Quaternion<T> q1(_o);
// measurements
- Eigen::Quaternions q2(getMeasurement().data() + 0); //q_measured
+ Eigen::Quaterniond q2(getMeasurement().data() + 0); //q_measured
/* error
* to compute the difference between both quaternions, we do
@@ -55,7 +68,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
*
* 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'
+ * minus_left(q1,q2) = log_q(q2 * q1.conjugate()); --> this is a global 'minus'
*/
Eigen::Matrix<T, 3, 1> er;
@@ -63,7 +76,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
return true;
}
diff --git a/include/core/factor/factor_relative_2D_analytic.h b/include/core/factor/factor_relative_2D_analytic.h
deleted file mode 100644
index c5636621c0413e229c4603891ba52ea35cf0d297..0000000000000000000000000000000000000000
--- a/include/core/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 "core/factor/factor_analytic.h"
-#include "core/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/core/factor/factor_relative_pose_2d.h b/include/core/factor/factor_relative_pose_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..a51c9a7fc9e9013eb4a79746c6bbc024c4d9e386
--- /dev/null
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -0,0 +1,258 @@
+#ifndef FACTOR_RELATIVE_POSE_2d_H_
+#define FACTOR_RELATIVE_POSE_2d_H_
+
+//Wolf includes
+#include "core/factor/factor_analytic.h"
+#include "core/landmark/landmark_base.h"
+#include "core/math/rotations.h"
+#include <Eigen/StdVector>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePose2d);
+
+//class
+class FactorRelativePose2d : public FactorAnalytic
+{
+ public:
+
+ /** \brief Constructor of category FAC_FRAME
+ **/
+ FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePose2d",
+ _top,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_FEATURE
+ **/
+ FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePose2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ _ftr_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_other_ptr->getFrame()->getP(),
+ _ftr_other_ptr->getFrame()->getO() )
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_LANDMARK
+ **/
+ FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePose2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP(),
+ _landmark_other_ptr->getO())
+ {
+ //
+ }
+
+ ~FactorRelativePose2d() override = default;
+
+ /** \brief Returns the factor residual size
+ **/
+ 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::VectorXd
+ **/
+ Eigen::VectorXd evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
+
+ /** \brief Returns the jacobians evaluated in the states provided
+ *
+ * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
+ * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+ *
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+ *
+ **/
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+
+ /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ *
+ **/
+ void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
+
+};
+
+/// IMPLEMENTATION ///
+
+inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
+{
+ Eigen::VectorXd residual(3);
+ Eigen::VectorXd expected_measurement(3);
+ // Expected measurement
+ Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_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();
+ residual(2) = pi2pi(residual(2));
+ residual = getMeasurementSquareRootInformationUpper() * residual;
+ return residual;
+}
+
+inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 4);
+ assert(_st_vector.size() == 4);
+ assert(_compute_jacobian.size() == 4);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,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).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[3])
+ {
+ jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,1) <<
+ 0, 0, 1).finished();
+ }
+}
+
+inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 4);
+ assert(_st_vector.size() == 4);
+ assert(_compute_jacobian.size() == 4);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,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).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[3])
+ {
+ jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,1) <<
+ 0, 0, 1).finished();
+ }
+}
+
+inline void FactorRelativePose2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+{
+ assert(jacobians.size() == 4);
+
+ jacobians[0] = (Eigen::MatrixXd(3,2) <<
+ -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)), -cos(getStateBlockPtrVector()[1]->getState()(0)),
+ 0, 0).finished();
+
+ jacobians[1] = (Eigen::MatrixXd(3,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).finished();
+
+ jacobians[2] = (Eigen::MatrixXd(3,2) <<
+ cos(getStateBlockPtrVector()[1]->getState()(0)), sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)), cos(getStateBlockPtrVector()[1]->getState()(0)),
+ 0, 0).finished();
+
+ jacobians[3] = (Eigen::MatrixXd(3,1) << 0, 0, 1).finished();
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..1d7e85804d2e1e94eeca7623f382e0ef19f05dbd
--- /dev/null
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -0,0 +1,127 @@
+#ifndef FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
+
+//class
+class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
+{
+ public:
+ FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+ {
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+ }
+
+ ~FactorRelativePose2dWithExtrinsics() override = 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 _sp,
+ const T* const _so,
+ T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _ps,
+ const T* const _os,
+ 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);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
+ T o1 = _o1[0];
+ T o2 = _o2[0];
+ T os = _os[0];
+
+ Eigen::Matrix<T, 3, 1> expected_measurement;
+ Eigen::Matrix<T, 3, 1> er; // error
+
+ // Expected measurement
+ // r1_p_r1_s1 = ps
+ // r2_p_r2_s2 = ps
+ // r1_R_s1 = R(os)
+ // r2_R_s2 = R(os)
+ // w_R_r1 = R(o1)
+ // w_R_r2 = R(o2)
+ // ----------------------------------------
+
+ // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
+ // s1_R_r1*r1_R_w*w_p_r1_w +
+ // s1_R_r1*r1_R_w*w_p_w_r2 +
+ // s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
+
+ // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
+ expected_measurement(2) = o2 - o1;
+
+ // Error
+ er = expected_measurement - getMeasurement().cast<T>();
+ er(2) = pi2pi(er(2));
+
+ // 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 << "FactorRelativePose2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePose2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePose2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_relative_pose_3d.h
similarity index 57%
rename from include/core/factor/factor_odom_3D.h
rename to include/core/factor/factor_relative_pose_3d.h
index 587472541124f2d0d158b3d999c9b65bb1bc5dcb..afcf8daf6884c9c08eb800433fa1f2392a8ecd0c 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -1,12 +1,12 @@
/*
- * factor_odom_3D.h
+ * factor_relative_pose_3d.h
*
* Created on: Oct 7, 2016
* Author: jsola
*/
-#ifndef FACTOR_ODOM_3D_H_
-#define FACTOR_ODOM_3D_H_
+#ifndef FACTOR_RELATIVE_POSE_3D_H_
+#define FACTOR_RELATIVE_POSE_3D_H_
#include "core/factor/factor_autodiff.h"
#include "core/math/rotations.h"
@@ -14,19 +14,20 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorOdom3D);
+WOLF_PTR_TYPEDEFS(FactorRelativePose3d);
//class
-class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
+class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,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);
+ FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
- virtual ~FactorOdom3D() = default;
+ ~FactorRelativePose3d() override = default;
template<typename T>
bool operator ()(const T* const _p_current,
@@ -43,16 +44,17 @@ class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
T* _expectation_dp,
T* _expectation_dq) const;
- Eigen::VectorXs expectation() const;
+ Eigen::VectorXd expectation() const;
private:
template<typename T>
void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
+ std::string topology_;
};
template<typename T>
-inline void FactorOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
+inline void FactorRelativePose3d::printRes(const Eigen::Matrix<T, 6, 1>& r) const
{
std::cout << "Jet residual = " << std::endl;
std::cout << r(0) << std::endl;
@@ -64,43 +66,44 @@ inline void FactorOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
}
template<>
-inline void FactorOdom3D::printRes (const Eigen::Matrix<Scalar,6,1> & r) const
+inline void FactorRelativePose3d::printRes (const Eigen::Matrix<double,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
+inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ _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());
- //
+ MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<6,6>::check(_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
+inline bool FactorRelativePose3d::expectation(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _expectation_dp,
+ T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
@@ -139,15 +142,15 @@ inline bool FactorOdom3D::expectation(const T* const _p_current, const T* const
return true;
}
-inline Eigen::VectorXs FactorOdom3D::expectation() const
+inline Eigen::VectorXd FactorRelativePose3d::expectation() const
{
- Eigen::VectorXs exp(7);
+ Eigen::VectorXd 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();
+ const Eigen::VectorXd frame_current_pos = frm_current->getP()->getState();
+ const Eigen::VectorXd frame_current_ori = frm_current->getO()->getState();
+ const Eigen::VectorXd frame_past_pos = frm_past->getP()->getState();
+ const Eigen::VectorXd frame_past_ori = frm_past->getO()->getState();
// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
@@ -162,8 +165,11 @@ inline Eigen::VectorXs FactorOdom3D::expectation() const
}
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
+inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _residuals) const
{
/* Residual expression
@@ -208,7 +214,8 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
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::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
+ auto& dp_m = getMeasurement().head<3>();
Eigen::Quaternion<T> dq_m(getMeasurement().tail<4>().cast<T>());
Eigen::Matrix<T,3,1> dp = expected.head(3);
@@ -221,11 +228,11 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
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;
+ residuals = getMeasurementSquareRootInformationUpper() * residuals;
return true;
}
} /* namespace wolf */
-#endif /* FACTOR_ODOM_3D_H_ */
+#endif /* FACTOR_RELATIVE_POSE_3D_H_ */
diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..115ea57f1803aa7a907c6e895e04d7731eab1560
--- /dev/null
+++ b/include/core/factor/factor_velocity_local_direction_3d.h
@@ -0,0 +1,149 @@
+
+#ifndef FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
+#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d);
+
+//class
+class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>
+{
+ protected:
+ double min_vel_sq_norm_; // stored in squared norm for efficiency
+ int orthogonal_axis_; // 0: X, 1: Y, 2: Z
+
+ public:
+ FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
+ const double& _min_vel_norm,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorVelocityLocalDirection3d() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
+
+ template<typename T>
+ Eigen::Matrix<T,2,1> computeElevationAzimuth(const Eigen::Matrix<T,3,1> vector) const
+ {
+ Eigen::Matrix<T,2,1> elev_azim;
+
+ // plane YZ
+ if (orthogonal_axis_ == 0)
+ {
+ elev_azim(0) = asin(vector(0) / vector.norm());
+ elev_azim(1) = atan2(vector(2), vector(1));
+ }
+ // plane XZ
+ if (orthogonal_axis_ == 1)
+ {
+ elev_azim(0) = asin(vector(1) / vector.norm());
+ elev_azim(1) = atan2(vector(0), vector(2));
+ }
+ // plane XY
+ if (orthogonal_axis_ == 2)
+ {
+ elev_azim(0) = asin(vector(2) / vector.norm());
+ elev_azim(1) = atan2(vector(1), vector(0));
+ }
+
+ return elev_azim;
+ }
+};
+
+FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
+ const double& _min_vel_norm,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>("FactorVelocityLocalDirection3d",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getV(),
+ _ftr_ptr->getFrame()->getO()),
+ min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
+{
+ MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
+ MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper());
+
+ /* Decide the plane (two axis A1, A2 from X, Y, Z in local coordinates) in which compute elevation and azimuth
+ * - elevation w.r.t. the plane
+ * - azimuth computed with atan2(A2, A1)
+ *
+ * This is done to avoid the degenerated case: elevation = +/-PI/2
+ * We select the orthogonal axis as the farthest to the desired velocity in local coordinates,
+ * so the component one with lower value.
+ */
+
+ measurement_.minCoeff(&orthogonal_axis_);
+
+ // measurement: elevation & azimuth (w.r.t. selected plane)
+ measurement_ = computeElevationAzimuth(Eigen::Vector3d(measurement_));
+ measurement_sqrt_information_upper_ = Matrix2d::Identity() * measurement_sqrt_information_upper_(0,0);
+}
+
+template<typename T>
+inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
+ Eigen::Map<const Eigen::Quaternion<T> > q(_q);
+ Eigen::Map<Eigen::Matrix<T,2,1> > residuals(_residuals);
+
+ if (v.squaredNorm() < min_vel_sq_norm_)
+ {
+ _residuals[0] = T(0);
+ _residuals[1] = T(0);
+ return true;
+ }
+
+// std::cout << "----------\n";
+// std::cout << "desired elevation: " << getMeasurement()(0) << "\n";
+// std::cout << "desired azimuth: " << getMeasurement()(1) << "\n";
+
+// std::cout << "v: \n\t" << v(0) << "\n\t"
+// << v(1) << "\n\t"
+// << v(2) << "\n";
+//
+// std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t"
+// << q.coeffs()(1) << "\n\t"
+// << q.coeffs()(2) << "\n\t"
+// << q.coeffs()(3) << "\n";
+
+ Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
+// std::cout << "v (local): \n\t" << v_local(0) << "\n\t"
+// << v_local(1) << "\n\t"
+// << v_local(2) << "\n";
+
+ // expectation
+ Eigen::Matrix<T,2,1> exp_elev_azim = computeElevationAzimuth(v_local);
+// std::cout << "expected elevation: " << exp_elev_azim(0) << "\n";
+// std::cout << "expected azimuth: " << exp_elev_azim(1) << "\n";
+
+ // error
+ Eigen::Matrix<T,2,1> error = getMeasurement() - exp_elev_azim;
+ pi2pi(error(1));
+// std::cout << "error (corrected): \n\t" << error(0) << "\n\t"
+// << error(1) << "\n;
+
+ // residuals
+ residuals = getMeasurementSquareRootInformationUpper() * error;
+// std::cout << "residuals: \n\t" << residuals(0) << "\n\t"
+// << residuals(1) << "\n;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 72a2be8a46ba1f4e95b448570ac649d478a86054..43735ff44e297f6e6b71a43122d8e6a71387b340 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -18,6 +18,10 @@ namespace wolf {
//class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
+ friend FactorBase;
+ friend CaptureBase;
+ friend SensorBase;
+
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
@@ -29,11 +33,12 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
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
-
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
+ Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
+ Eigen::VectorXd expectation_; ///< expectation
+ void setProblem(ProblemPtr _problem) override final;
+
public:
typedef enum
@@ -48,18 +53,20 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
* \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_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
+ FeatureBase(const std::string& _type,
+ const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_uncertainty,
+ UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
- virtual ~FeatureBase();
- virtual void remove();
+ ~FeatureBase() override;
+ virtual void remove(bool viral_remove_empty_parent=false);
- virtual void setProblem(ProblemPtr _problem) final;
// properties
- unsigned int id();
- unsigned int trackId(){return track_id_;}
+ unsigned int id() const;
+ unsigned int trackId() const {return track_id_;}
void setTrackId(unsigned int _tr_id){track_id_ = _tr_id;}
- unsigned int landmarkId(){return landmark_id_;}
+ unsigned int landmarkId() const {return landmark_id_;}
void setLandmarkId(unsigned int _lmk_id){landmark_id_ = _lmk_id;}
// values
@@ -67,40 +74,68 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
*
* 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);
+ double getMeasurement(unsigned int _ii) const;
+ const Eigen::VectorXd& getMeasurement() const;
+
+ private:
+ /* \brief Set the measurement and its noise
+ *
+ * WARNING: To be used once in the constructor only.
+ */
+ void setMeasurement(const Eigen::VectorXd& _meas);
+ void setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov);
+ void setMeasurementInformation(const Eigen::MatrixXd & _meas_info);
+
+ public:
+ const Eigen::MatrixXd& getMeasurementCovariance() const;
+ Eigen::MatrixXd getMeasurementInformation() const;
+ const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
+
+ const Eigen::VectorXd& getExpectation() const;
+ void setExpectation(const Eigen::VectorXd& 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();
+ const FactorBasePtrList& getFactorList() const;
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
+
+
// all factors
- void getFactorList(FactorBasePtrList & _fac_list);
+ void getFactorList(FactorBasePtrList & _fac_list) const;
- void link(CaptureBasePtr);
+ void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
- static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
+ static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, FeatureBasePtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
- Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
+ void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
+ FactorBasePtr addFactor(FactorBasePtr _co_ptr);
+ void removeFactor(FactorBasePtr _co_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
@@ -112,9 +147,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
namespace wolf{
template<typename classType, typename... T>
- std::shared_ptr<FeatureBase> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
+ std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
{
- FeatureBasePtr ftr = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
ftr->link(_cpt_ptr);
return ftr;
}
@@ -124,12 +159,12 @@ inline unsigned int FeatureBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& FeatureBase::getConstrainedByList()
+inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
-inline unsigned int FeatureBase::id()
+inline unsigned int FeatureBase::id() const
{
return feature_id_;
}
@@ -139,42 +174,37 @@ inline CaptureBasePtr FeatureBase::getCapture() const
return capture_ptr_.lock();
}
-inline Scalar FeatureBase::getMeasurement(unsigned int _ii) const
+inline double FeatureBase::getMeasurement(unsigned int _ii) const
{
return measurement_(_ii);
}
-inline const Eigen::VectorXs& FeatureBase::getMeasurement() const
+inline const Eigen::VectorXd& FeatureBase::getMeasurement() const
{
return measurement_;
}
-inline const Eigen::MatrixXs& FeatureBase::getMeasurementCovariance() const
+inline const Eigen::MatrixXd& FeatureBase::getMeasurementCovariance() const
{
return measurement_covariance_;
}
-inline Eigen::MatrixXs FeatureBase::getMeasurementInformation() const
+inline Eigen::MatrixXd FeatureBase::getMeasurementInformation() const
{
return measurement_sqrt_information_upper_.transpose() * measurement_sqrt_information_upper_;
}
-inline const Eigen::MatrixXs& FeatureBase::getMeasurementSquareRootInformationUpper() const
+inline const Eigen::MatrixXd& 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
+inline const Eigen::VectorXd& FeatureBase::getExpectation() const
{
return expectation_;
}
-inline void FeatureBase::setExpectation(const Eigen::VectorXs& expectation)
+inline void FeatureBase::setExpectation(const Eigen::VectorXd& expectation)
{
expectation_ = expectation;
}
diff --git a/include/core/feature/feature_diff_drive.h b/include/core/feature/feature_diff_drive.h
index 6052c24406a2dcee27909e424948b67322311cb7..203e8f05facaf95ef18543f0ffe6508a69a70060 100644
--- a/include/core/feature/feature_diff_drive.h
+++ b/include/core/feature/feature_diff_drive.h
@@ -9,29 +9,25 @@
#define _WOLF_FEATURE_DIFF_DRIVE_H_
//Wolf includes
-#include "core/feature/feature_base.h"
+#include "core/feature/feature_motion.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
-class FeatureDiffDrive : public FeatureBase
+class FeatureDiffDrive : public FeatureMotion
{
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);
+ FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::VectorXd& _diff_drive_params,
+ const Eigen::MatrixXd& _jacobian_diff_drive_params);
virtual ~FeatureDiffDrive() = default;
- const Eigen::MatrixXs& getJacobianFactor() const;
-
protected:
- Eigen::VectorXs diff_drive_factors_;
- Eigen::MatrixXs jacobian_diff_drive_factors_;
};
} /* namespace wolf */
diff --git a/include/core/feature/feature_match.h b/include/core/feature/feature_match.h
index 7fbb0e204ecbe06f354f6b168f155b9e915cb186..0ace09b3e99b53f3eaa63d31db91a846632bf00b 100644
--- a/include/core/feature/feature_match.h
+++ b/include/core/feature/feature_match.h
@@ -27,7 +27,7 @@ typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is al
struct FeatureMatch
{
FeatureBasePtr feature_ptr_; ///< Corresponding feature
- Scalar normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
+ double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
};
}//end namespace
diff --git a/include/core/feature/feature_motion.h b/include/core/feature/feature_motion.h
index 7471510337a30c36c0acf5a525efa01739224d02..0f5ecc2a66930370b3a314a1284ff366fc76434f 100644
--- a/include/core/feature/feature_motion.h
+++ b/include/core/feature/feature_motion.h
@@ -14,46 +14,42 @@
namespace wolf
{
+WOLF_PTR_TYPEDEFS(FeatureMotion);
+
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;
+ FeatureMotion(const std::string& _type,
+ const VectorXd& _delta_preint,
+ const MatrixXd _delta_preint_cov,
+ const VectorXd& _calib_preint,
+ const MatrixXd& _jacobian_calib);
+ ~FeatureMotion() override;
- virtual Eigen::VectorXs correctDelta(const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta_correction) const = 0;
+ const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement()
+ const Eigen::VectorXd& getCalibrationPreint() const;
+ const Eigen::MatrixXd& getJacobianCalibration() const;
private:
- Eigen::VectorXs& calib_preint_;
- Eigen::MatrixXs& jacobian_calib_;
+ Eigen::VectorXd calib_preint_;
+ Eigen::MatrixXd jacobian_calib_;
};
-inline Eigen::VectorXs FeatureMotion::getCalibrationPreint()
+inline const Eigen::VectorXd& FeatureMotion::getDeltaPreint() const
{
- return calib_preint_;
+ return measurement_;
}
-inline Eigen::MatrixXs FeatureMotion::getJacobianCalibration()
+inline const Eigen::VectorXd& FeatureMotion::getCalibrationPreint() const
{
- return jacobian_calib_;
+ return calib_preint_;
}
-inline Eigen::VectorXs FeatureMotion::computeDeltaCorrection(const Eigen::VectorXs& _calib) const
+inline const Eigen::MatrixXd& FeatureMotion::getJacobianCalibration() const
{
- return jacobian_calib_ * (_calib - calib_preint_);
+ return jacobian_calib_;
}
-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 */
diff --git a/include/core/feature/feature_odom_2D.h b/include/core/feature/feature_odom_2D.h
deleted file mode 100644
index 5df94a665faecac2d17710df0f0b4d3496eb0a4d..0000000000000000000000000000000000000000
--- a/include/core/feature/feature_odom_2D.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef FEATURE_ODOM_2D_H_
-#define FEATURE_ODOM_2D_H_
-
-//Wolf includes
-#include "core/feature/feature_base.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/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/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..91016cbfc53c4437e7bb974b068dcfcc2dc3add6
--- /dev/null
+++ b/include/core/feature/feature_odom_2d.h
@@ -0,0 +1,31 @@
+#ifndef FEATURE_ODOM_2d_H_
+#define FEATURE_ODOM_2d_H_
+
+//Wolf includes
+#include "core/feature/feature_base.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::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+
+ ~FeatureOdom2d() override;
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/feature/feature_pose.h b/include/core/feature/feature_pose.h
index 3fe413f6ad43d52c13463a11b53adb0bfcede846..701967949c395fa02371f55d9313c508174eabeb 100644
--- a/include/core/feature/feature_pose.h
+++ b/include/core/feature/feature_pose.h
@@ -22,8 +22,8 @@ class FeaturePose : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
- virtual ~FeaturePose();
+ FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+ ~FeaturePose() override;
};
} // namespace wolf
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 44e1cfbd2981f6d7afe67311c249b9fc67897638..ed36d9b509703b395ac63111549a1c38bf3c3b27 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -1,4 +1,5 @@
+
#ifndef FRAME_BASE_H_
#define FRAME_BASE_H_
@@ -13,75 +14,66 @@ class StateBlock;
#include "core/common/wolf.h"
#include "core/common/time_stamp.h"
#include "core/common/node_base.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
namespace wolf {
-/** \brief Enumeration of frame types
- */
-typedef enum
-{
- KEY = 2, ///< key frame. It plays at optimizations (estimated).
- AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated).
- NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
-} FrameType;
//class FrameBase
-class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
+class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<FrameBase>
{
+ friend CaptureBase;
+ friend FactorBase;
+
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);
+ FrameBase(const TimeStamp& _ts,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr = nullptr,
+ StateBlockPtr _v_ptr = nullptr);
- FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x);
+ FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
+ const VectorComposite& _state);
- virtual ~FrameBase();
- virtual void remove();
+ FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
+ const std::list<VectorXd>& _vectors);
- // Frame properties -----------------------------------------------
- public:
- unsigned int id();
+ ~FrameBase() override;
+
+ // Add and remove from WOLF tree ---------------------------------
+ template<typename classType, typename... T>
+ static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
+
+ void link(TrajectoryBasePtr);
+ void link(ProblemPtr _prb);
- // get type
- bool isKey() const;
- bool isAux() const;
- bool isKeyOrAux() const;
+ virtual void remove(bool viral_remove_empty_parent=false);
- // set type
- void setKey();
- void setAux();
+ // Frame properties -----------------------------------------------
+ public:
+ unsigned int id() const;
// Frame values ------------------------------------------------
public:
@@ -89,77 +81,72 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
TimeStamp getTimeStamp() const;
void getTimeStamp(TimeStamp& _ts) const;
- // State blocks
+ // 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;
+ protected:
+ void setProblem(ProblemPtr _problem) override final;
// 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;
+ bool getCovariance(Eigen::MatrixXd& _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);
+ const CaptureBasePtrList& getCaptureList() const;
+ template <class C>
+ CaptureBasePtr getCaptureOfType() const;
+ CaptureBasePtr getCaptureOfType(const std::string& type) const;
+ template <class C>
+ CaptureBasePtrList getCapturesOfType() const;
+ CaptureBasePtrList getCapturesOfType(const std::string& type) const;
+ CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
+ CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
+ CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
+
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
+ FactorBasePtrList getFactorList() const;
+ void getFactorList(FactorBasePtrList& _fac_list) const;
+
+ unsigned int getHits() const;
+ const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr& _factor) const;
+
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
+ // Debug and info -------------------------------------------------------
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
- void getFactorList(FactorBasePtrList& _fac_list);
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
+ private:
+
+ CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
+ void removeCapture(CaptureBasePtr _capt_ptr);
+ void setTrajectory(TrajectoryBasePtr _trj_ptr);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
- void link(TrajectoryBasePtr);
- template<typename classType, typename... T>
- static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
- 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
@@ -174,33 +161,18 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
namespace wolf {
template<typename classType, typename... T>
-std::shared_ptr<FrameBase> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
+std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
{
- FrameBasePtr frm = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
frm->link(_ptr);
return frm;
}
-inline unsigned int FrameBase::id()
+inline unsigned int FrameBase::id() const
{
return frame_id_;
}
-inline bool FrameBase::isKey() const
-{
- return (type_ == KEY);
-}
-
-inline bool FrameBase::isAux() const
-{
- return (type_ == AUXILIARY);
-}
-
-inline bool FrameBase::isKeyOrAux() const
-{
- return (type_ == KEY || type_ == AUXILIARY);
-}
-
inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
{
_ts = time_stamp_;
@@ -211,54 +183,9 @@ 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;
+ return getStateBlock('V');
}
inline TrajectoryBasePtr FrameBase::getTrajectory() const
@@ -266,37 +193,43 @@ inline TrajectoryBasePtr FrameBase::getTrajectory() const
return trajectory_ptr_.lock();
}
-inline CaptureBasePtrList& FrameBase::getCaptureList()
+inline const CaptureBasePtrList& FrameBase::getCaptureList() const
{
return capture_list_;
}
-inline void FrameBase::resizeStateBlockVec(unsigned int _size)
+inline unsigned int FrameBase::getHits() const
{
- if (_size > state_block_vec_.size())
- state_block_vec_.resize(_size);
+ return constrained_by_list_.size();
}
-inline unsigned int FrameBase::getHits() const
+inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
{
- return constrained_by_list_.size();
+ return constrained_by_list_;
}
-inline void FrameBase::setProblem(ProblemPtr _problem)
+inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
- NodeBase::setProblem(_problem);
- for (auto cap : capture_list_)
- cap->setProblem(_problem);
+ trajectory_ptr_ = _trj_ptr;
}
-inline FactorBasePtrList& FrameBase::getConstrainedByList()
+template <class C>
+inline CaptureBasePtr FrameBase::getCaptureOfType() const
{
- return constrained_by_list_;
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ return capture_ptr;
+ return nullptr;
}
-inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
+template <class C>
+inline CaptureBasePtrList FrameBase::getCapturesOfType() const
{
- trajectory_ptr_ = _trj_ptr;
+ CaptureBasePtrList captures;
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ captures.push_back(capture_ptr);
+ return captures;
}
} // namespace wolf
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 1b4149a6d32671492d1eee3c05e13dacb79e92fa..0ef770b1cb1249491c01329b9ecc4952776a9dd3 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -16,15 +16,35 @@ namespace wolf {
//class HardwareBase
class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase>
{
+ friend SensorBase;
+
private:
SensorBasePtrList sensor_list_;
public:
HardwareBase();
- virtual ~HardwareBase();
+ ~HardwareBase() override;
+
+ const SensorBasePtrList& getSensorList() const;
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
- SensorBasePtrList& getSensorList();
};
} // namespace wolf
@@ -33,7 +53,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
namespace wolf {
-inline SensorBasePtrList& HardwareBase::getSensorList()
+inline const SensorBasePtrList& HardwareBase::getSensorList() const
{
return sensor_list_;
}
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 839ee0f97c9694da18cff81b17e78ce853b7e934..21098d33232129521a5d4f56b52db1930feb5eee 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -11,6 +11,7 @@ class StateBlock;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
@@ -20,19 +21,23 @@ class StateBlock;
namespace wolf {
//class LandmarkBase
-class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
+class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<LandmarkBase>
{
+ friend FactorBase;
+
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O.
+ std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
static unsigned int landmark_id_count_;
protected:
+ // Navigate wolf tree
+ void setProblem(ProblemPtr _problem) override final;
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.
+ Eigen::VectorXd descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
public:
@@ -44,58 +49,64 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
*
**/
- LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
- LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
- virtual ~LandmarkBase();
- virtual void remove();
+ LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
+ ~LandmarkBase() override;
+ virtual void remove(bool viral_remove_empty_parent=false);
virtual YAML::Node saveToYaml() const;
// Properties
- unsigned int id();
+ unsigned int id() const;
void setId(unsigned int _id);
- // Fix / unfix
- void fix();
- void unfix();
- bool isFixed() 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();
+ bool getCovariance(Eigen::MatrixXd& _cov) const;
// Descriptor
public:
- const Eigen::VectorXs& getDescriptor() const;
- Scalar getDescriptor(unsigned int _ii) const;
- void setDescriptor(const Eigen::VectorXs& _descriptor);
+ const Eigen::VectorXd& getDescriptor() const;
+ double getDescriptor(unsigned int _ii) const;
+ void setDescriptor(const Eigen::VectorXd& _descriptor);
- // Navigate wolf tree
- virtual void setProblem(ProblemPtr _problem) final;
- FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
- void setMap(const MapBasePtr _map_ptr);
- MapBasePtr getMap();
+
+ MapBasePtr getMap() const;
void link(MapBasePtr);
template<typename classType, typename... T>
- static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all);
+ static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
+
+ /** \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);
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
+ private:
+ void setMap(const MapBasePtr _map_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
@@ -107,18 +118,14 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
namespace wolf{
template<typename classType, typename... T>
-std::shared_ptr<LandmarkBase> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
+std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
{
- LandmarkBasePtr lmk = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> lmk = std::make_shared<classType>(std::forward<T>(all)...);
lmk->link(_map_ptr);
return lmk;
}
-inline void LandmarkBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
-}
-inline MapBasePtr LandmarkBase::getMap()
+inline MapBasePtr LandmarkBase::getMap() const
{
return map_ptr_.lock();
}
@@ -128,7 +135,7 @@ inline void LandmarkBase::setMap(const MapBasePtr _map_ptr)
map_ptr_ = _map_ptr;
}
-inline unsigned int LandmarkBase::id()
+inline unsigned int LandmarkBase::id() const
{
return landmark_id_;
}
@@ -145,64 +152,23 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& LandmarkBase::getConstrainedByList()
+inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
{
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)
+inline void LandmarkBase::setDescriptor(const Eigen::VectorXd& _descriptor)
{
descriptor_ = _descriptor;
}
-inline Scalar LandmarkBase::getDescriptor(unsigned int _ii) const
+inline double LandmarkBase::getDescriptor(unsigned int _ii) const
{
assert(_ii < descriptor_.size() && "LandmarkBase::getDescriptor: bad index");
return descriptor_(_ii);
}
-inline const Eigen::VectorXs& LandmarkBase::getDescriptor() const
+inline const Eigen::VectorXd& LandmarkBase::getDescriptor() const
{
return descriptor_;
}
diff --git a/include/core/landmark/landmark_match.h b/include/core/landmark/landmark_match.h
index 23d52ab39ea9c55e02dabdb887a0cc50c92fbc4e..891173fc5289c4fc0f49359720a2d98dfb16ae54 100644
--- a/include/core/landmark/landmark_match.h
+++ b/include/core/landmark/landmark_match.h
@@ -19,14 +19,14 @@ typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
struct LandmarkMatch
{
LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark
- Scalar normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match
+ double normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match
LandmarkMatch() :
landmark_ptr_(nullptr), normalized_score_(0)
{
}
- LandmarkMatch(LandmarkBasePtr _landmark_ptr, Scalar _normalized_score) :
+ LandmarkMatch(LandmarkBasePtr _landmark_ptr, double _normalized_score) :
landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
{
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 90ad785458a9a3a62f6882c7294945c19ceedcc8..93663bdf7fed2b4a8af6867c8e6226df6bd76571 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -19,25 +19,45 @@ namespace wolf {
//class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
+ friend LandmarkBase;
+
private:
LandmarkBasePtrList landmark_list_;
public:
MapBase();
- ~MapBase();
+ ~MapBase() override;
+ protected:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
- virtual void addLandmarkList(LandmarkBasePtrList& _landmark_list);
- LandmarkBasePtrList& getLandmarkList();
+ virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
+
+ public:
+ const LandmarkBasePtrList& getLandmarkList() const;
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");
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
std::string dateTimeNow();
};
-inline LandmarkBasePtrList& MapBase::getLandmarkList()
+inline const LandmarkBasePtrList& MapBase::getLandmarkList() const
{
return landmark_list_;
}
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
new file mode 100644
index 0000000000000000000000000000000000000000..ffd8ac8725e49ea921b32140042eb0936cf5d467
--- /dev/null
+++ b/include/core/math/SE2.h
@@ -0,0 +1,395 @@
+/**
+ * \file SE2.h
+ *
+ * Created on: Jul 27, 2019
+ * \author: jsola
+ */
+
+#ifndef MATH_SE2_H_
+#define MATH_SE2_H_
+
+#include "core/common/wolf.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_composite.h"
+
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+
+/*
+ * Some of the functions below are based on:
+ *
+ * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
+ */
+
+namespace wolf
+{
+namespace SO2{
+
+template<typename T>
+inline Eigen::Matrix<T, 2, 2> exp(const T theta)
+{
+ return Eigen::Rotation2D<T>(theta).matrix();
+}
+
+} // namespace SO2
+
+
+namespace SE2{
+
+template<class T>
+inline Eigen::Matrix<T, 2, 2> skew(const T& t)
+{
+ return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished();
+}
+
+/** \brief Compute [1]_x * t = (-t.y ; t.x)
+ */
+template<class D>
+inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t)
+{
+ return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
+}
+
+template<typename T>
+inline Eigen::Matrix2d V_helper(const T theta)
+{
+ T s; // sin(theta) / theta
+ T c_1; // (1-cos(theta)) / theta
+
+ if (fabs(theta) > T(Constants::EPS))
+ {
+ // [1] eq. 158
+ s = sin(theta) / theta;
+ c_1 = (T(1.0) - cos(theta)) / theta;
+ }
+ else
+ {
+ // approx of [1] eq. 158
+ s = T(1.0); // sin(x) ~= x
+ c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2
+ }
+
+ return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
+}
+
+inline VectorComposite identity()
+{
+ VectorComposite v;
+ v['P'] = Vector2d::Zero();
+ v['O'] = Vector1d::Zero();
+
+ return v;
+}
+
+template<class D1, class D2>
+inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
+{
+ MatrixSizeCheck<3, 1>::check(_tau);
+ MatrixSizeCheck<3, 1>::check(_delta);
+
+ // [1] eq. 156
+ _delta.head(2) = V_helper(_tau(2)) * _tau.head(2);
+ _delta(2) = pi2pi(_tau(2));
+}
+
+template<class D, typename T>
+inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
+{
+ MatrixSizeCheck<2, 1>::check (p);
+
+ // Jacobian of t = V(theta)*p wrt theta -- developed in Matlab symbolic, and copied here.
+ T x = p(0);
+ T y = p(1);
+ Matrix<T, 2, 1> J_Vp_th;
+
+ if (fabs(theta) > T(Constants::EPS))
+ {
+ T s_th = sin(theta);
+ T c_th = cos(theta);
+ T theta2 = theta * theta;
+ J_Vp_th << -(y * c_th - y + x * s_th - theta * x * c_th + theta * y * s_th) / theta2,
+ (x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2;
+ }
+ else
+ { // sin(x) ~= x
+ // cos(x) ~= 1 - x^2/2
+ J_Vp_th << -y / 2.0, x / 2.0;
+ }
+
+ return J_Vp_th;
+}
+
+template<class D1, class D2, class D3>
+inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
+{
+ MatrixSizeCheck<3, 1>::check(_tau);
+ MatrixSizeCheck<3, 1>::check(_delta);
+ MatrixSizeCheck<3, 3>::check(_J_delta_tau);
+
+ typedef typename D1::Scalar T;
+
+ // [1] eq. 156
+ T theta = pi2pi(_tau(2));
+ Eigen::Matrix<T, 2, 2> V = V_helper(theta);
+ _delta.head(2) = V * _tau.head(2);
+ _delta(2) = theta;
+
+ // Jacobian is the composite definition [1] eq. 89, with jacobian blocks:
+ // J_Vp_p = V: see V_helper, eq. 158
+ // J_Vp_theta: see fcn helper
+ // J_theta_theta = 1; eq. 126
+ _J_delta_tau << V, J_Vp_theta(_tau.template head<2>(), theta), 0.0, 0.0, 1.0;
+}
+
+inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
+{
+ // [1] eq. 156
+ const auto &p = _tau.at('P');
+ const auto &theta = pi2pi(_tau.at('O')(0));
+ Matrix2d V = V_helper(theta);
+
+ _delta['P'] = V * p;
+ _delta['O'] = Matrix1d(theta);
+}
+
+inline VectorComposite exp(const VectorComposite& tau)
+{
+ VectorComposite res("PO", {2,1});
+
+ exp(tau, res);
+
+ return res;
+}
+
+inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
+{
+ // [1] eq. 156
+ const auto &p = _tau.at('P');
+ const auto &theta = pi2pi(_tau.at('O')(0));
+ Matrix2d V = V_helper(theta);
+
+ _delta['P'] = V * p;
+ _delta['O'] = Matrix1d(theta);
+
+ // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
+ // J_Vp_p = V: see V_helper, eq. 158
+ // J_Vp_theta: see fcn helper
+ // J_theta_theta = 1; eq. 126
+ _J_delta_tau.clear();
+ _J_delta_tau.emplace('P', 'P', V);
+ _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
+ _J_delta_tau.emplace('O', 'P', RowVector2d(0.0, 0.0));
+ _J_delta_tau.emplace('O', 'O', Matrix1d(1));
+}
+
+template<class D1, class D2, class D3>
+inline void compose(const MatrixBase<D1> &_delta1, MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
+{
+ MatrixSizeCheck<3, 1>::check(_delta1);
+ MatrixSizeCheck<3, 1>::check(_delta2);
+ MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
+
+ _delta1_compose_delta2.template head<2>() = _delta1.template head<2>()
+ + SO2::exp(_delta1(2)) * _delta2.template head<2>();
+ _delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+}
+
+template<class D1, class D2, class D3, class D4, class D5>
+inline void compose(const MatrixBase<D1>& _delta1,
+ const MatrixBase<D2>& _delta2,
+ MatrixBase<D3>& _delta1_compose_delta2,
+ MatrixBase<D4>& _J_compose_delta1,
+ MatrixBase<D5>& _J_compose_delta2)
+{
+ MatrixSizeCheck<3, 1>::check(_delta1);
+ MatrixSizeCheck<3, 1>::check(_delta2);
+ MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
+ MatrixSizeCheck<3, 3>::check(_J_compose_delta1);
+ MatrixSizeCheck<3, 3>::check(_J_compose_delta2);
+
+ Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
+
+ // tc = t1 + R1 t2
+ _delta1_compose_delta2.template head<2>() = _delta1.template head<2>() + R1 * _delta2.template head<2>();
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
+ /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
+ *
+ * resulting delta is:
+ *
+ * tc = t1 + R1 t2 (*)
+ * Rc = R1 R2 (**)
+ *
+ * Jacobians have the form:
+ *
+ * [ J_t_t J_t_R ]
+ * [ J_r_t J_R_R ]
+ *
+ * Jacobian blocks are:
+ *
+ * J_tc_t1 = I trivial from (*)
+ * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
+ *
+ * J_Rc_t1 = (0 0) trivial from (**)
+ * J_Rc_R1 = 1 see [1]: eq. (125)
+ *
+ * J_tc_t2 = R1 see [1]: eq. (130)
+ * J_tc_R2 = 0 trivial from (*)
+ *
+ * J_Rc_t2 = 0 trivial from (**)
+ * J_Rc_R2 = 1 see [1]: eq. (125)
+ */
+
+ _J_compose_delta1.setIdentity();
+ _J_compose_delta1.template block<2, 1>(0, 2) = R1 * skewed(_delta2.template head<2>());
+
+ _J_compose_delta2.setIdentity(3, 3);
+ _J_compose_delta2.template block<2, 2>(0, 0) = R1;
+}
+
+inline void compose(const VectorComposite& _x1,
+ const VectorComposite& _x2,
+ VectorComposite& _c)
+{
+ const auto& p1 = _x1.at('P');
+ const auto& a1 = _x1.at('O')(0); // angle
+ const auto& R1 = SO2::exp(a1);
+
+ const auto& p2 = _x2.at('P');
+ const auto& a2 = _x2.at('O')(0); // angle
+
+ // tc = t1 + R1 t2
+ _c['P'] = p1 + R1 * p2;
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
+}
+
+inline void compose(const VectorComposite& _x1,
+ const VectorComposite& _x2,
+ VectorComposite& _c,
+ MatrixComposite& _J_c_x1,
+ MatrixComposite& _J_c_x2)
+{
+ const auto& p1 = _x1.at('P');
+ const auto& a1 = _x1.at('O')(0); // angle
+ Matrix2d R1 = SO2::exp(a1); // need temporary
+
+ const auto& p2 = _x2.at('P');
+ const auto& a2 = _x2.at('O')(0); // angle
+
+ /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
+ *
+ * resulting delta is:
+ *
+ * tc = t1 + R1 t2 (*)
+ * Rc = R1 R2 (**)
+ *
+ * Jacobians have the form:
+ *
+ * [ J_t_t J_t_R ]
+ * [ J_r_t J_R_R ]
+ *
+ * Jacobian blocks are:
+ *
+ * J_tc_t1 = I trivial from (*)
+ * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
+ *
+ * J_Rc_t1 = (0 0) trivial from (**)
+ * J_Rc_R1 = 1 see [1]: eq. (125)
+ *
+ * J_tc_t2 = R1 see [1]: eq. (130)
+ * J_tc_R2 = 0 trivial from (*)
+ *
+ * J_Rc_t2 = 0 trivial from (**)
+ * J_Rc_R2 = 1 see [1]: eq. (125)
+ */
+
+ _J_c_x1.clear();
+ _J_c_x1.emplace('P', 'P', Matrix2d::Identity());
+ _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)) );
+ _J_c_x1.emplace('O', 'P', RowVector2d(0,0));
+ _J_c_x1.emplace('O', 'O', Matrix1d(1));
+
+ _J_c_x2.clear();
+ _J_c_x2.emplace('P', 'P', R1);
+ _J_c_x2.emplace('P', 'O', Vector2d(0,0));
+ _J_c_x2.emplace('O', 'P', RowVector2d(0,0));
+ _J_c_x2.emplace('O', 'O', Matrix1d(1));
+
+
+ // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'.
+
+ // tc = t1 + R1 t2
+ _c['P'] = p1 + R1 * p2;
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
+
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+{
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<1, 1>::check(q1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<1, 1>::check(o2);
+ MatrixSizeCheck<2, 1>::check(plus_p);
+ MatrixSizeCheck<1, 1>::check(plus_q);
+
+ plus_p = p1 + p2;
+ plus_q(0) = pi2pi(q1(0) + o2(0));
+}
+
+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, 2, 1> > p1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 1, 1> > q1 ( & d1(2) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 1, 1> > o2 ( & d2(2) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > p_p ( & d_plus(0) );
+ Map<Matrix<typename D1::Scalar, 1, 1> > q_p ( & d_plus(2) );
+
+ plus(p1, q1, p2, o2, p_p, q_p);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 3, 1> d_plus;
+ plus(d1, d2, d_plus);
+ return d_plus;
+}
+
+inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
+{
+ plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O'));
+}
+
+inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
+{
+ VectorComposite res;
+
+ res['P'] = Vector2d();
+ res['O'] = Vector1d();
+
+ plus(x, tau, res);
+
+ return res;
+}
+
+
+
+} // namespace SE2
+} // namespacs wolf
+
+#endif /* MATH_SE2_H_ */
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 0515376e459be99732f24d97ea9667d7616a2177..685eefabaccf1d0585ec7de7d73c9d4ba2e1ff8d 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -10,11 +10,12 @@
#include "core/common/wolf.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_composite.h"
/*
- * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
+ * 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,
+ * The Delta is defined as a simple 3d pose with the rotation expressed in quaternion form,
* Delta = [Dp, Dq]
* with
* Dp : position delta
@@ -26,16 +27,22 @@
* - 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 )
+ * - log: go from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - exp: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 * exp(d)
+ * - minus: d = log( D1.inv() * D2 )
* - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
+ *
+ * ------------------------------------------------------------------------------------------
+ *
+ * Some of the functions below are based on:
+ *
+ * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
*/
namespace wolf
{
-namespace three_D {
+namespace SE3 {
using namespace Eigen;
template<typename D1, typename D2>
@@ -57,7 +64,14 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
q << T2(0), T2(0), T2(0), T2(1);
}
-template<typename T = Scalar>
+inline void identity(VectorComposite& v)
+{
+ v.clear();
+ v['P'] = Vector3d::Zero();
+ v['O'] = Quaterniond::Identity().coeffs();
+}
+
+template<typename T = double>
inline Matrix<T, 7, 1> identity()
{
Matrix<T, 7, 1> ret;
@@ -67,14 +81,14 @@ inline Matrix<T, 7, 1> identity()
}
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)
+inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
+ MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
{
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(idp);
+ MatrixSizeCheck<3, 1>::check(p);
+ MatrixSizeCheck<3, 1>::check(ip);
- idp = - dq.conjugate() * dp ;
- idq = dq.conjugate() ;
+ ip = - q.conjugate() * p ;
+ iq = q.conjugate() ;
}
template<typename D1, typename D2>
@@ -84,12 +98,12 @@ inline void inverse(const MatrixBase<D1>& d,
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 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & d( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > q ( & d( 3 ) );
+ Map<Matrix<typename D2::Scalar, 3, 1> > ip ( & id( 0 ) );
+ Map<Quaternion<typename D2::Scalar> > iq ( & id( 3 ) );
- inverse(dp, dq, idp, idq);
+ inverse(p, q, ip, iq);
}
template<typename D>
@@ -101,16 +115,35 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
}
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 )
+inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pc, QuaternionBase<D8>& qc )
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(sum_p);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(pc);
- sum_p = dp1 + dq1*dp2;
- sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+ pc = p1 + q1 * p2;
+ qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void compose(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& q2,
+ MatrixBase<D7>& pc, MatrixBase<D8>& qc )
+{
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<4, 1>::check(q1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<4, 1>::check(q2);
+ MatrixSizeCheck<3, 1>::check(pc);
+ MatrixSizeCheck<4, 1>::check(qc);
+
+ Map<const Quaternion<typename D2::Scalar> > mq1 ( & q1( 0 ) );
+ Map<const Quaternion<typename D5::Scalar> > mq2 ( & q2( 0 ) );
+ Map< Quaternion<typename D8::Scalar> > mqc ( & qc( 0 ) );
+
+ compose(p1, mq1, p2, mq2, pc, mqc);
}
template<typename D1, typename D2, typename D3>
@@ -122,14 +155,14 @@ inline void compose(const MatrixBase<D1>& 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 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2( 0 ) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2( 3 ) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pc ( & sum( 0 ) );
+ Map<Quaternion<typename D3::Scalar> > qc ( & sum( 3 ) );
- compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
+ compose(p1, q1, p2, q2, pc, qc);
}
template<typename D1, typename D2>
@@ -155,8 +188,8 @@ inline void compose(const MatrixBase<D1>& 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
+ Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //q1.matrix(); // First Delta, DR
+ Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //q2.matrix(); // Second delta, dR
// Jac wrt first delta
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
@@ -165,43 +198,75 @@ inline void compose(const MatrixBase<D1>& d1,
// 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
+ J_sum_d2.block(0,0,3,3) = dR1; // dDp'/p
+ // J_sum_d2.block(3,3,3,3) = Matrix3d::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);
}
+inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c)
+{
+ compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
+}
+
+inline void compose(const VectorComposite& x1,
+ const VectorComposite& x2,
+ VectorComposite& c,
+ MatrixComposite& J_c_x1,
+ MatrixComposite& J_c_x2)
+{
+
+ // Some useful temporaries
+ const auto R1 = q2R(x1.at('O')); //q1.matrix(); // make temporary
+ const auto& R2 = q2R(x2.at('O')); //q2.matrix(); // do not make temporary, only reference
+
+ // Jac wrt first delta
+ J_c_x1.emplace('P', 'P', Matrix3d::Identity() ) ;
+ J_c_x1.emplace('P', 'O', - R1 * skew(x2.at('P')) ) ;
+ J_c_x1.emplace('O', 'P', Matrix3d::Zero() ) ;
+ J_c_x1.emplace('O', 'O', R2.transpose() ) ;
+
+ // Jac wrt second delta
+ J_c_x2.emplace('P', 'P', R1 );
+ J_c_x2.emplace('P', 'O', Matrix3d::Zero() );
+ J_c_x2.emplace('O', 'P', Matrix3d::Zero() );
+ J_c_x2.emplace('O', 'O', Matrix3d::Identity() );
+
+ // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable
+ compose(x1, x2, c);
+}
+
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)
+inline void between(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& p12, QuaternionBase<D8>& q12)
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dp12);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(p12);
- dp12 = dq1.conjugate() * ( dp2 - dp1 );
- dq12 = dq1.conjugate() * dq2;
+ p12 = q1.conjugate() * ( p2 - p1 );
+ q12 = q1.conjugate() * q2;
}
template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
- MatrixBase<D3>& d2_minus_d1)
+ MatrixBase<D3>& d12)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 1>::check(d2_minus_d1);
+ MatrixSizeCheck<7, 1>::check(d12);
- 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) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > p12 ( & d12(0) );
+ Map<Quaternion<typename D3::Scalar> > q12 ( & d12(3) );
- between(dp1, dq1, dp2, dq2, dp12, dq12);
+ between(p1, q1, p2, q2, p12, q12);
}
template<typename D1, typename D2>
@@ -216,28 +281,28 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
+inline Matrix<typename Derived::Scalar, 6, 1> log(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<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & delta_in(0) );
+ Map<const Quaternion<typename Derived::Scalar> > q_in ( & delta_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > p_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);
+ do_ret = log_q(q_in);
V_inv = jac_SO3_left_inv(do_ret);
- dp_ret = V_inv * dp_in;
+ p_ret = V_inv * p_in;
return ret;
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
+inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<6, 1>::check(d_in);
@@ -247,27 +312,58 @@ Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
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) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & d_in(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > o_in ( & d_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > p ( & ret(0) );
+ Map<Quaternion<typename Derived::Scalar> > q ( & ret(3) );
- dp = V * dp_in;
- dq = exp_q(do_in);
+ p = V * p_in;
+ q = exp_q(o_in);
return ret;
}
+inline VectorComposite exp(const VectorComposite& tau)
+{
+ const auto& p = tau.at('P');
+ const auto& theta = tau.at('O');
+
+ Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
+
+ VectorComposite res;
+
+ res['P'] = V * p ;
+ res['O'] = exp_q(theta).coeffs() ;
+
+ return res;
+}
+
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,
+inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(o2);
MatrixSizeCheck<3, 1>::check(plus_p);
- plus_p = dp1 + dp2;
- plus_q = dq1 * exp_q(do2);
+
+ plus_p = p1 + p2;
+ plus_q = q1 * exp_q(o2);
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+{
+ MatrixSizeCheck<4, 1>::check(q1);
+ MatrixSizeCheck<4, 1>::check(plus_q);
+
+ Map<const Quaterniond> q1m ( & q1 (0) );
+ Map<Quaterniond> plus_qm ( & plus_q(0) );
+
+ plus(p1, q1m, p2, o2, plus_p, plus_qm);
}
template<typename D1, typename D2, typename D3>
@@ -275,14 +371,14 @@ 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);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > o2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > p_p ( & d_plus(0) );
+ Map<Quaternion<typename D3::Scalar> > q_p ( & d_plus(3) );
+
+ plus(p1, q1, p2, o2, p_p, q_p);
}
template<typename D1, typename D2>
@@ -294,25 +390,42 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
return d_plus;
}
+inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
+{
+ plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O'));
+}
+
+inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
+{
+ VectorComposite res;
+
+ res['P'] = Vector3d::Zero();
+ res['O'] = Vector4d::Zero();
+
+ plus(x, tau, res);
+
+ return res;
+}
+
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 )
+inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pd, MatrixBase<D8>& od )
{
- diff_p = dp2 - dp1;
- diff_o = log_q(dq1.conjugate() * dq2);
+ pd = p2 - p1;
+ od = log_q(q1.conjugate() * q2);
}
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)
+inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D6>& pd, MatrixBase<D7>& od,
+ MatrixBase<D8>& J_do_q1, MatrixBase<D9>& J_do_q2)
{
- minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
+ minus(p1, q1, p2, q2, pd, od);
- J_do_dq1 = - jac_SO3_left_inv(diff_o);
- J_do_dq2 = jac_SO3_right_inv(diff_o);
+ J_do_q1 = - jac_SO3_left_inv(od);
+ J_do_q2 = jac_SO3_right_inv(od);
}
template<typename D1, typename D2, typename D3>
@@ -320,14 +433,14 @@ 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);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & err(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > od ( & err(3) );
+
+ minus(p1, q1, p2, q2, pd, od);
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
@@ -337,35 +450,30 @@ inline void minus(const MatrixBase<D1>& d1,
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) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & dif(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > od ( & dif(3) );
- Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+ Matrix<typename D4::Scalar, 3, 3> J_do_q1, J_do_q2;
- minus(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+ minus(p1, q1, p2, q2, pd, od, J_do_q1, J_do_q2);
/* d = minus(d1, d2) is
- * dp = dp2 - dp1
- * do = Log(dq1.conj * dq2)
- * dv = dv2 - dv1
+ * pd = p2 - p1
+ * od = Log(q1.conj * q2)
*
- * With trivial Jacobians for dp and dv, and:
- * J_do_dq1 = - J_l_inv(theta)
- * J_do_dq2 = J_r_inv(theta)
+ * With trivial Jacobians for p and dv, and:
+ * J_do_q1 = - J_l_inv(theta)
+ * J_do_q2 = 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_d1.block(3,3,3,3) = J_do_q1; // 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)
+ J_diff_d2.block(3,3,3,3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
}
template<typename D1, typename D2>
@@ -385,9 +493,9 @@ inline void interpolate(const MatrixBase<D1>& d1,
{
Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
- Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
+ Matrix<typename D1::Scalar, 6, 1> tau = t * log(dd);
- ret = compose(d1, exp_SE3(tau));
+ ret = compose(d1, exp(tau));
}
template<typename D1, typename D2>
@@ -423,9 +531,9 @@ inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const
// 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.);
+ B = double(1./6.) + double(1./120.) * th_2;
+ C = -double(1./24.) + double(1./720.) * th_2;
+ D = -double(1./60.);
}
else
{
@@ -443,7 +551,7 @@ inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const
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() !!!
+ - C * (VWW - VWW.transpose() - double(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;
diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h
new file mode 100644
index 0000000000000000000000000000000000000000..45cbdf3e1366fab06820ca54fad65cfa7e3af5fd
--- /dev/null
+++ b/include/core/math/covariance.h
@@ -0,0 +1,110 @@
+/**
+ * \file covariance.h
+ *
+ * Created on: Feb 26, 2020
+ * \author: jsola
+ */
+
+#ifndef MATH_COVARIANCE_H_
+#define MATH_COVARIANCE_H_
+
+#include <Eigen/Dense>
+
+namespace wolf{
+
+template <typename T, int N, int RC>
+inline 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>
+inline bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
+{
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly);
+
+ if (eigensolver.info() == Eigen::Success)
+ {
+ // All eigenvalues must be >= 0:
+ return (eigensolver.eigenvalues().array() >= eps).all();
+ }
+ else
+ std::cout << "eigen decomposition failed" << std::endl;
+
+ return false;
+}
+
+template <typename T, int N, int RC>
+inline 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, double(0.0)) && "Not an information matrix");
+
+template <typename T, int N, int RC>
+inline 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:
+ double 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;
+}
+
+inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd & _info)
+{
+ // impose symmetry
+ Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>();
+
+ // Normal Cholesky factorization
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(info);
+ Eigen::MatrixXd R = llt_of_info.matrixU();
+
+ // Good factorization
+ if (info.isApprox(R.transpose() * R, Constants::EPS))
+ return R;
+
+ // Not good factorization: SelfAdjointEigenSolver
+ Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info);
+ Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
+
+ R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
+
+ return R;
+}
+
+
+}
+
+
+
+#endif /* MATH_COVARIANCE_H_ */
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index 6e2630efed88721857a0c4cd70a4c7d08277d7cb..a0a86c6ee4c3ef095725a2df2230c2fdb8de751f 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -22,13 +22,15 @@ namespace wolf
*/
template<typename T>
inline T
-pi2pi(const T angle)
+pi2pi(const T _angle)
{
- using std::fmod;
+ T angle = _angle;
+ while (angle > T( M_PI ))
+ angle -= T( 2 * M_PI );
+ while (angle <= T( -M_PI ))
+ angle += T( 2 * M_PI );
- 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);
+ return angle;
}
/** \brief Conversion to radians
@@ -60,7 +62,7 @@ toDeg(const T rad)
/** \brief Skew symmetric matrix
*
- * @param _v a 3D vector
+ * @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>
@@ -203,26 +205,26 @@ 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 );
+ double w = _q(3);
+ double x = _q(0);
+ double y = _q(1);
+ double z = _q(2);
+
+ double xx = x*x;
+ double xy = x*y;
+ double xz = x*z;
+ double yy = y*y;
+ double yz = y*z;
+ double zz = z*z;
+ double wx = w*x;
+ double wy = w*y;
+ double wz = w*z;
+
+ double r00 = T(1) - T(2) * ( yy + zz );
+ double r10 = T(2) * ( xy + wz );
+ double r20 = T(2) * ( xz - wy );
+ double r21 = T(2) * ( yz + wx );
+ double r22 = T(1) - T(2) * ( xx + yy );
e(0) = atan2( r21, r22);
e(1) = atan2(-r20, sqrt(r00*r00 + r10*r10));
@@ -344,7 +346,7 @@ exp_R(const Eigen::MatrixBase<Derived>& _v)
/** \brief Rotation matrix logarithmic map
*
- * @param _R a 3D rotation matrix
+ * @param _R a 3d rotation matrix
* @return the rotation vector v such that _R = exp_R(v)
*/
template<typename Derived>
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 6b2e32db108077716f372deb7fc4b2fc4438a6c6..3d0eba08b8e234a1adcd3ab4bd00a149e10e6f65 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -10,17 +10,19 @@ class MapBase;
class ProcessorMotion;
class StateBlock;
class TimeStamp;
-struct IntrinsicsBase;
-struct ProcessorParamsBase;
+struct ParamsSensorBase;
+struct ParamsProcessorBase;
}
//wolf includes
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
-#include "core/sensor/autoconf_sensor_factory.h"
-#include "core/processor/autoconf_processor_factory.h"
+#include "core/utils/params_server.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/processor/factory_processor.h"
+#include "core/processor/is_motion.h"
+#include "core/state_block/state_composite.h"
// std includes
#include <mutex>
@@ -33,78 +35,103 @@ enum Notification
REMOVE
};
+struct PriorOptions
+{
+ std::string mode = "";
+ VectorComposite state;
+ MatrixComposite cov;
+ double time_tolerance;
+};
+WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
+
/** \brief Wolf problem node element in the Wolf Tree
*/
class Problem : public std::enable_shared_from_this<Problem>
{
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
+ friend ProcessorBase;
+ friend ProcessorMotion;
+ friend IsMotion;
protected:
+ TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
- ProcessorMotionPtr processor_motion_ptr_;
- std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
+ std::map<int, IsMotionPtr> processor_is_motion_map_;
+ std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
- mutable std::mutex mut_factor_notifications_;
- mutable std::mutex mut_state_block_notifications_;
+ mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
- bool prior_is_set_;
- std::string frame_structure_;
+ StateStructure frame_structure_;
+ PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(const std::string& _frame_structure); // USE create() below !!
Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
void setup();
public:
static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
- static ProblemPtr autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file);
+ static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
+
+
// Properties -----------------------------------------
- SizeEigen getFrameStructureSize() const;
- void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
+ public:
SizeEigen getDim() const;
- std::string getFrameStructure() const;
+ const StateStructure& getFrameStructure() const;
+
+ protected:
+ void appendToStructure(const StateStructure& _structure);
+
+
+
+
+ // Tree manager --------------------------------------
+ public:
+ void setTreeManager(TreeManagerBasePtr _gm);
+ TreeManagerBasePtr getTreeManager() const;
+
+
+
// Hardware branch ------------------------------------
- HardwareBasePtr getHardware();
- void addSensor(SensorBasePtr _sen_ptr);
+ HardwareBasePtr getHardware() const;
/** \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 _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);
+ const Eigen::VectorXd& _extrinsics, //
+ ParamsSensorBasePtr _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.
+ * \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 FactoryParamsSensor under the key _sen_type.
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
+ const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename);
- /**
- Custom installSensor using the parameters server
- */
- SensorBasePtr installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name,
- const paramsServer& _server);
+ /**
+ Custom installSensor using the parameters server
+ */
+ SensorBasePtr installSensor(const std::string& _sen_type, //
+ const std::string& _unique_sensor_name,
+ const ParamsServer& _server);
/** \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);
+ SensorBasePtr getSensor(const std::string& _sensor_name) const;
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -117,7 +144,7 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorBasePtr installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
SensorBasePtr _corresponding_sensor_ptr, //
- ProcessorParamsBasePtr _prc_params = nullptr);
+ ParamsProcessorBasePtr _prc_params = nullptr);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -134,95 +161,143 @@ class Problem : public std::enable_shared_from_this<Problem>
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename = "");
- /**
- Custom installProcessor to be used with parameters server
- */
- ProcessorBasePtr installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- const std::string& _corresponding_sensor_name, //
- const paramsServer& _server);
- /** \brief Set the processor motion
+ /**
+ Custom installProcessor to be used with parameters server
+ */
+ ProcessorBasePtr installProcessor(const std::string& _prc_type, //
+ const std::string& _unique_processor_name, //
+ const std::string& _corresponding_sensor_name, //
+ const ParamsServer& _server);
+ protected:
+ /** \brief Set the processor motion
*
- * Set the processor motion.
+ * Add a new processor of type is motion to the processor is motion list.
*/
- void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
- ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
- void clearProcessorMotion();
- ProcessorMotionPtr& getProcessorMotion();
+ void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
+ void removeProcessorIsMotion(IsMotionPtr proc);
- // Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectory();
- virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
- const Eigen::MatrixXs& _prior_cov, //
- const TimeStamp& _ts,
- const Scalar _time_tolerance);
+ public:
+ IsMotionPtr getProcessorIsMotion();
+ std::map<int,IsMotionPtr>& getProcessorIsMotionMap();
+ const std::map<int,IsMotionPtr>& getProcessorIsMotionMap() const;
- /** \brief Emplace frame from string frame_structure
+ // Trajectory branch ----------------------------------
+ TrajectoryBasePtr getTrajectory() const;
+
+ // Prior
+ bool isPriorSet() const;
+ void setPriorOptions(const std::string& _mode,
+ const double _time_tolerance = 0,
+ const VectorComposite& _state = VectorComposite(),
+ const VectorComposite& _cov = VectorComposite());
+ FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
+ FrameBasePtr setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_sigma,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+ FrameBasePtr setPriorFix(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+ FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+
+ /** \brief Emplace frame from string frame_structure, dimension and vector
+ * \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _frame_state State vector; must match the size and format of the chosen frame structure
+ *
+ * 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 TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state);
+
+ /** \brief Emplace frame from string frame_structure and state
* \param _time_stamp Time stamp of the frame
+ * \param _frame_structure String indicating the frame structure.
+ * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ *
+ * - The dimension is taken from Problem
+ *
+ * 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 TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state);
+
+ /** \brief Emplace frame from state
+ * \param _time_stamp Time stamp of the frame
+ * \param _frame_state State; must be part of the problem's frame structure
+ *
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
*
* 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, //
- const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state);
- /** \brief Emplace frame from string frame_structure without state
+ /** \brief Emplace frame from string frame_structure and dimension
+ * \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
- * \param _time_stamp Time stamp of the frame
+ *
+ * - The dimension is taken from Problem
+ * - The state is taken from Problem
*
* 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, //
- const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim);
- /** \brief Emplace frame from string frame_structure without structure
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ /** \brief Emplace frame from state vector
* \param _time_stamp Time stamp of the frame
+ * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ *
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
*
* 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);
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state);
- /** \brief Emplace frame from string frame_structure without structure nor state
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ /** \brief Emplace frame, guess all values
* \param _time_stamp Time stamp of the frame
*
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
+ * - The state is taken from Problem
+ *
* 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);
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
// Frame getters
FrameBasePtr getLastFrame( ) const;
- FrameBasePtr getLastKeyFrame( ) const;
- FrameBasePtr getLastKeyOrAuxFrame( ) const;
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
- FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
/** \brief Give the permission to a processor to create a new key Frame
*
@@ -230,7 +305,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - This decision is made at problem level, not at processor configuration level.
* - Therefore, if you want to permanently configure a processor for not creating key frames, see Processor::voting_active_ and its accessors.
*/
- bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
+ bool permitKeyFrame(ProcessorBasePtr _processor_ptr) const;
/** \brief New key frame callback
*
@@ -238,63 +313,50 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
ProcessorBasePtr _processor_ptr, //
- const Scalar& _time_tolerance);
-
- /** \brief Give the permission to a processor to create a new auxiliary Frame
- *
- * This should implement a black list of processors that have forbidden auxiliary frame creation.
- * - This decision is made at problem level, not at processor configuration level.
- * - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors.
- */
- bool permitAuxFrame(ProcessorBasePtr _processor_ptr);
-
- /** \brief New auxiliary frame callback
- *
- * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion.
- */
- void auxFrameCallback(FrameBasePtr _frame_ptr, //
- ProcessorBasePtr _processor_ptr, //
- const Scalar& _time_tolerance);
+ const double& _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);
+ TimeStamp getTimeStamp ( ) const;
+ VectorComposite getState ( const StateStructure& _structure = "" ) const;
+ VectorComposite getState ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
+ VectorComposite getOdometry ( const StateStructure& _structure = "" ) const;
+
// Zero state provider
- Eigen::VectorXs zeroState ( );
- bool priorIsSet() const;
+ VectorComposite stateZero ( const StateStructure& _structure = "" ) const;
+
+
+
// Map branch -----------------------------------------
- MapBasePtr getMap();
- LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
- void addLandmarkList(LandmarkBasePtrList& _lmk_list);
+ MapBasePtr getMap() const;
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 -------------------------------------------
+
+
+ // All branches -------------------------------------------
+ // perturb states
+ void perturb(double amplitude = 0.01);
+
+ // 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 getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& _covariance);
- bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
+ void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
+ void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
+ bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
+ bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
+ bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
+ bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
+ bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
+ bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
- // Solver management ----------------------------------------
- /** \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
+ // Solver management ----------------------------------------
+
+ /** \brief Notifies a new/removed state block to update the solver manager
*/
- void removeStateBlock(StateBlockPtr _state_ptr);
+ StateBlockPtr notifyStateBlock(StateBlockPtr _state_ptr, Notification _notif);
/** \brief Returns the size of the map of state block notification
*/
@@ -304,13 +366,9 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
- /** \brief Notifies a new factor to be added to the solver manager
+ /** \brief Notifies a new/removed factor to update the solver manager
*/
- FactorBasePtr addFactor(FactorBasePtr _factor_ptr);
-
- /** \brief Notifies a factor to be removed from the solver manager
- */
- void removeFactor(FactorBasePtr _factor_ptr);
+ FactorBasePtr notifyFactor(FactorBasePtr _factor_ptr, Notification _notif);
/** \brief Returns the size of the map of factor notification
*/
@@ -321,6 +379,11 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
protected:
+ /** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
+ */
+ void consumeNotifications(std::map<StateBlockPtr,Notification>&,
+ std::map<FactorBasePtr,Notification>&);
+
/** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
@@ -341,12 +404,10 @@ class Problem : public std::enable_shared_from_this<Problem>
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);
+ bool state_blocks = false,
+ std::ostream& stream = std::cout) const;
+ bool check(int verbose_level = 0) const;
+ bool check(bool verbose, std::ostream& stream) const;
};
@@ -357,41 +418,56 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
-inline bool Problem::priorIsSet() const
+inline TreeManagerBasePtr Problem::getTreeManager() const
{
- return prior_is_set_;
+ return tree_manager_;
}
-inline ProcessorMotionPtr& Problem::getProcessorMotion()
+inline bool Problem::isPriorSet() const
{
- return processor_motion_ptr_;
+ return prior_options_ == nullptr;
}
-inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
+inline IsMotionPtr Problem::getProcessorIsMotion()
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
- return std::move(state_block_notification_map_);
+ if (not processor_is_motion_map_.empty())
+ return processor_is_motion_map_.begin()->second;
+ return nullptr;
}
-inline SizeStd Problem::getStateBlockNotificationMapSize() const
+inline std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap()
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
- return state_block_notification_map_.size();
+ return processor_is_motion_map_;
+}
+
+inline const std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap() const
+{
+ return processor_is_motion_map_;
}
-inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
+inline SizeStd Problem::getStateBlockNotificationMapSize() const
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
- return std::move(factor_notification_map_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
+ return state_block_notification_map_.size();
}
inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
return factor_notification_map_.size();
}
+inline void Problem::consumeNotifications(std::map<StateBlockPtr,Notification>& _sb_notification_map,
+ std::map<FactorBasePtr,Notification>& _fac_notification_map)
+{
+ std::lock_guard<std::mutex> lock(mut_notifications_);
+
+ _sb_notification_map = std::move(state_block_notification_map_);
+ _fac_notification_map = std::move(factor_notification_map_);
+}
} // namespace wolf
+
+
#endif // PROBLEM_H
diff --git a/include/core/processor/autoconf_processor_factory.h b/include/core/processor/autoconf_processor_factory.h
deleted file mode 100644
index 0fc1b5de4ac4722bf204959483ab35cc392a6ac5..0000000000000000000000000000000000000000
--- a/include/core/processor/autoconf_processor_factory.h
+++ /dev/null
@@ -1,185 +0,0 @@
-/**
- * \file processor_factory.h
- *
- * Created on: May 4, 2016
- * \author: jsola
- */
-
-#ifndef AUTOCONF_PROCESSOR_FACTORY_H_
-#define AUTOCONF_PROCESSOR_FACTORY_H_
-
-namespace wolf
-{
-class ProcessorBase;
-struct ProcessorParamsBase;
-}
-
-// wolf
-#include "core/common/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 "sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory
- * #include "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 "sensor_odom_2D.h"
- * #include "processor_odom_2D.h"
- * #include "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 paramsServer&,
- const SensorBasePtr> AutoConfProcessorFactory;
-template<>
-inline std::string AutoConfProcessorFactory::getClass()
-{
- return "AutoConfProcessorFactory";
-}
-
-
-#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType, ProcessorName) \
- namespace{ const bool WOLF_UNUSED ProcessorName##AutoConf##Registered = \
- wolf::AutoConfProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::createAutoConf); }\
-
-} /* namespace wolf */
-
-#endif /* PROCESSOR_FACTORY_H_ */
diff --git a/include/core/processor/diff_drive_tools.h b/include/core/processor/diff_drive_tools.h
deleted file mode 100644
index 50c20c0a021fd98ef882fe549d366b55cc7966e8..0000000000000000000000000000000000000000
--- a/include/core/processor/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 "core/utils/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 "core/processor/diff_drive_tools.hpp"
-
-#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */
diff --git a/include/core/processor/diff_drive_tools.hpp b/include/core/processor/diff_drive_tools.hpp
deleted file mode 100644
index f70ca2c373796de8eccfff4debb6cb9ae9b7fab9..0000000000000000000000000000000000000000
--- a/include/core/processor/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/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
new file mode 100644
index 0000000000000000000000000000000000000000..e36e85a6d109572070bf2d317d81349df73f824a
--- /dev/null
+++ b/include/core/processor/factory_processor.h
@@ -0,0 +1,147 @@
+/**
+ * \file factory_processor.h
+ *
+ * Created on: May 4, 2016
+ * \author: jsola
+ */
+
+#ifndef FACTORY_PROCESSOR_H_
+#define FACTORY_PROCESSOR_H_
+
+namespace wolf
+{
+class ProcessorBase;
+struct ParamsProcessorBase;
+}
+
+// wolf
+#include "core/common/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,
+ * - "ProcessorOdom2d" for ProcessorOdom2d
+ * - "ProcessorOdom3d" for ProcessorOdom3d
+ * - "ProcessorDiffDrive" for ProcessorDiffDrive
+ *
+ * among others.
+ *
+ * Find general Factory documentation in class Factory:
+ * - Access the factory
+ * - Register/unregister creators
+ * - Invoke object creation
+ *
+ *
+ * This documentation shows you how to use the FactoryProcessor specifically:
+ * - Write a processor creator
+ * - Create processors
+ *
+ * #### Write processor creators
+ * Processor creators have the following API:
+ *
+ * \code
+ * static ProcessorBasePtr create(const std::string& _name, ParamsProcessorBasePtr _params_processor);
+ * \endcode
+ *
+ * They follow the general implementation shown below:
+ *
+ * \code
+ * static ProcessorBasePtr create(const std::string& _unique_name, ParamsProcessorBasePtr _params_processor)
+ * {
+ * // cast processor parameters to good type --- example: ParamsProcessorOdom3d
+ * auto params_processor_ptr = std::static_pointer_cast<ParamsProcessorOdom3d>(_params_processor);
+ *
+ * // Do create the Processor object --- example: ProcessorOdom3d
+ * auto prc_ptr = std::make_shared<ProcessorOdom3d>(params_processor_ptr);
+ *
+ * // Complete the processor setup with a unique name identifying the processor
+ * prc_ptr->setName(_unique_name);
+ *
+ * return prc_ptr;
+ * }
+ * \endcode
+ *
+ * #### Creating processors
+ * Note: 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
+ * auto prc_odom2d_ptr = FactoryProcessor::create("ProcessorOdom2d", "main odometry", params_ptr);
+ * \endcode
+ *
+ * #### Example 1 : Create a sensor and its processor
+ * We provide the necessary steps to create a processor of class ProcessorOdom2d in our application,
+ * and bind it to a SensorOdom2d:
+ *
+ * \code
+ * #include "core/sensor/sensor_odom_2d.h" // provides SensorOdom2d and FactorySensor
+ * #include "core/processor/processor_odom_2d.h" // provides ProcessorOdom2d and FactoryProcessor
+ *
+ * // Note: SensorOdom2d::create() is already registered, automatically.
+ * // Note: ProcessorOdom2d::create() is already registered, automatically.
+ *
+ * // First create the sensor (See FactorySensor for details)
+ * SensorBasePtr sensor_ptr = FactorySensor::create ( "SensorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
+ *
+ * // To create a odometry integrator, provide a type="ProcessorOdom2d", a name="main odometry", and a pointer to the parameters struct:
+ *
+ * auto params = make_shared<ParamsProcessorOdom2d>({...}); // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
+ *
+ * ProcessorBasePtr processor_ptr =
+ * FactoryProcessor::create ( "ProcessorOdom2d" , "main odometry" , params );
+ *
+ * // Bind processor to sensor
+ * sensor_ptr->addProcessor(processor_ptr);
+ * \endcode
+ *
+ */
+typedef Factory<ProcessorBase,
+ const std::string&,
+ const ParamsProcessorBasePtr> FactoryProcessor;
+template<>
+inline std::string FactoryProcessor::getClass() const
+{
+ return "FactoryProcessor";
+}
+
+
+// ParamsProcessor factory
+struct ParamsProcessorBase;
+typedef Factory<ParamsProcessorBase,
+ const std::string&> FactoryParamsProcessor;
+template<>
+inline std::string FactoryParamsProcessor::getClass() const
+{
+ return "FactoryParamsProcessor";
+}
+
+#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
+ namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
+ wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
+
+typedef Factory<ProcessorBase,
+ const std::string&,
+ const ParamsServer&> AutoConfFactoryProcessor;
+template<>
+inline std::string AutoConfFactoryProcessor::getClass() const
+{
+ return "AutoConfFactoryProcessor";
+}
+
+
+#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType) \
+ namespace{ const bool WOLF_UNUSED ProcessorType##AutoConfRegistered = \
+ wolf::AutoConfFactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_FACTORY_H_ */
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
new file mode 100644
index 0000000000000000000000000000000000000000..a17ac88e7f2d78bc83e5866f10e85173af380638
--- /dev/null
+++ b/include/core/processor/is_motion.h
@@ -0,0 +1,110 @@
+/**
+ * \file is_motion.h
+ *
+ * Created on: Mar 10, 2020
+ * \author: jsola
+ */
+
+#ifndef PROCESSOR_IS_MOTION_H_
+#define PROCESSOR_IS_MOTION_H_
+
+#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
+#include "core/utils/params_server.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsIsMotion);
+
+struct ParamsIsMotion
+{
+ bool state_getter = true;
+ int state_priority = 1;
+
+ ParamsIsMotion() = default;
+ ParamsIsMotion(std::string _unique_name, const ParamsServer& _server)
+ {
+ state_getter = _server.getParam<bool>("processor/" + _unique_name + "/state_getter");
+ state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority");
+ }
+ std::string print() const
+ {
+ return "state_getter: " + std::to_string(state_getter) + "\n"
+ + "state_priority: " + std::to_string(state_priority) + "\n";
+ }
+
+};
+class TimeStamp;
+
+WOLF_PTR_TYPEDEFS(IsMotion);
+
+class IsMotion
+{
+ public:
+
+ IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params);
+ virtual ~IsMotion();
+
+ // Queries to the processor:
+ virtual TimeStamp getTimeStamp() const = 0;
+ virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
+
+ VectorComposite getOdometry ( ) const;
+ void setOdometry(const VectorComposite&);
+
+ bool isStateGetter() const;
+ int getStatePriority() const;
+ void setStatePriority(int);
+
+ public:
+ const StateStructure& getStateStructure ( ) { return state_structure_; };
+ void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
+ void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
+
+ protected:
+ StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+ VectorComposite odometry_;
+ ParamsIsMotionPtr params_is_motion_;
+};
+
+inline IsMotion::IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params) :
+ state_structure_(_structure),
+ params_is_motion_(_params)
+{
+ //
+}
+
+inline wolf::VectorComposite IsMotion::getOdometry ( ) const
+{
+ return odometry_;
+}
+
+}
+
+///// IMPLEMENTATION ///////
+namespace wolf{
+
+inline IsMotion::~IsMotion()
+{
+}
+
+inline bool IsMotion::isStateGetter() const
+{
+ return params_is_motion_->state_getter;
+}
+
+inline int IsMotion::getStatePriority() const
+{
+ return params_is_motion_->state_priority;
+}
+
+inline void IsMotion::setStatePriority(int _priority)
+{
+ params_is_motion_->state_priority = _priority;
+}
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_IS_MOTION_H_ */
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index 9b64ff56252a8229c27a73cbb8880adaa7689ae9..548fcf6e1e4f99b9d020784b226f4f1df75e4b98 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -23,28 +23,28 @@ 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)
+ Eigen::VectorXd data_; ///< instantaneous motion data
+ Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data
+ Eigen::VectorXd delta_; ///< instantaneous motion delta
+ Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta
+ Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral
+ Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta
+ Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_
+ Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
+ Eigen::MatrixXd 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));
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _delta,
+ const MatrixXd& _delta_cov,
+ const VectorXd& _delta_int,
+ const MatrixXd& _delta_integr_cov,
+ const MatrixXd& _jac_delta,
+ const MatrixXd& _jac_delta_int,
+ const MatrixXd& _jacobian_calib);
~Motion();
private:
void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
@@ -72,50 +72,16 @@ struct Motion
* - 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{
+class MotionBuffer : public std::list<Motion>
+{
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);
+ MotionBuffer() ;
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/core/processor/processor_base.h b/include/core/processor/processor_base.h
index a6f8ac588195b3402ba2aebab998a8d8f6fcf1f7..bbb9d109f4ea76919f4c9ddac9c5960eb94de141 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -9,16 +9,56 @@ class SensorBase;
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
-#include "core/common/time_stamp.h"
+#include "core/processor/is_motion.h"
+#include "core/sensor/sensor_base.h"
#include "core/frame/frame_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/common/time_stamp.h"
+#include "core/common/params_base.h"
// std
#include <memory>
#include <map>
+#include <chrono>
namespace wolf {
+/*
+ * Macro for defining Autoconf processor creator for WOLF's high level API.
+ *
+ * Place a call to this macro inside your class declaration (in the processor_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived processor class, ProcessorClass,
+ * must have a constructor available with the API:
+ *
+ * ProcessorClass(const ParamsProcessorClassPtr _params);
+ */
+#define WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass) \
+static ProcessorBasePtr create(const std::string& _unique_name, \
+ const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsProcessorClass>(_unique_name, _server); \
+ \
+ auto processor = std::make_shared<ProcessorClass>(params); \
+ \
+ processor ->setName(_unique_name); \
+ \
+ return processor; \
+} \
+static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProcessorBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsProcessorClass>(_params); \
+ \
+ auto processor = std::make_shared<ProcessorClass>(params); \
+ \
+ processor ->setName(_unique_name); \
+ \
+ return processor; \
+} \
+
+
+
+
/** \brief Key frame class pack
*
* To store a key_frame with an associated time tolerance.
@@ -28,208 +68,358 @@ namespace wolf {
class PackKeyFrame
{
public:
- PackKeyFrame(const FrameBasePtr _key_frame, const Scalar _time_tolerance) : key_frame(_key_frame), time_tolerance(_time_tolerance) {};
+ PackKeyFrame(const FrameBasePtr _key_frame, const double _time_tolerance) : key_frame(_key_frame), time_tolerance(_time_tolerance) {};
~PackKeyFrame(){};
FrameBasePtr key_frame;
- Scalar time_tolerance;
+ double time_tolerance;
};
WOLF_PTR_TYPEDEFS(PackKeyFrame);
+
+/** \brief Buffer for arbitrary type objects
+ *
+ * Object and functions to manage a buffer of objects.
+ */
+template <typename T>
+class Buffer
+{
+public:
+
+ typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator
+
+ Buffer(){};
+ ~Buffer(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
+ */
+ T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
+
+ /**\brief Select a Pack iterator from the buffer
+ *
+ * Select from the buffer the iterator pointing to the closest pack (w.r.t. time stamp),
+ * respecting a defined time tolerances
+ */
+ Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
+
+ T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+
+ T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance);
+
+ T selectFirst();
+
+ T selectLast();
+
+ /**\brief Buffer size
+ *
+ */
+ SizeStd size(void);
+
+ /**\brief Add a pack to the buffer
+ *
+ */
+ void add(const TimeStamp& _time_stamp, const T& _element); //const double& _time_tolerance);
+
+ /** \brief returns the container with elements of the buffer
+ *
+ * elements are ordered from most recent to oldest
+ */
+ std::map<TimeStamp,T>& getContainer();
+
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpTo(const TimeStamp& _time_stamp);
+
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpToLower(const TimeStamp& _time_stamp);
+
+ /**\brief Clear the buffer
+ *
+ */
+ void clear();
+
+ /**\brief is the buffer empty ?
+ *
+ */
+ bool empty();
+
+ /**\brief Check time tolerance
+ *
+ * Check if the time distance between two time stamps is smaller than
+ * the time tolerance.
+ */
+ static bool simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
+
+ /**\brief Check time tolerance
+ *
+ * Check if the time distance between two time stamps is smaller than
+ * the minimum time tolerance of the two frames.
+ */
+ static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2);
+
+protected:
+
+ std::map<TimeStamp,T> container_; // Main buffer container
+};
+
+
/** \brief Buffer of Key frame class objects
*
* Object and functions to manage a buffer of KFPack objects.
*/
-class PackKeyFrameBuffer
+class BufferPackKeyFrame : public Buffer<PackKeyFramePtr>
{
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);
+ PackKeyFramePtr selectPack(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ PackKeyFramePtr selectPack(const CaptureBasePtr _capture, const double& _time_tolerance);
- /**\brief Buffer size
- *
- */
- SizeStd size(void);
+ PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const double& _time_tolerance);
/**\brief Add a pack to the buffer
*
*/
- void add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance);
+ void add(const FrameBasePtr& _key_frame, const double& _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
+ /**\brief Print buffer information
*
- * 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);
+ void print() const;
- /**\brief Clear the buffer
- *
- */
- void clear();
+ /**\brief Alias funct
+ *
+ */
+ static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2)
+ { return doubleCheckTimeTolerance(_time_stamp1, _time_tolerance1, _time_stamp2, _time_tolerance2); };
- /**\brief Empty the buffer
- *
- */
- bool empty();
+};
- /**\brief Print buffer information
- *
- */
- void print();
- private:
+/** \brief Buffer of Capture class objects
+ *
+ * Object and functions to manage a buffer of Capture objects.
+ */
+class BufferCapture : public Buffer<CaptureBasePtr> {};
- 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 : public ParamsBase
-{
- ProcessorParamsBase() = default;
- ProcessorParamsBase(bool _voting_active,
- Scalar _time_tolerance,
- bool _voting_aux_active = false) :
- voting_active(_voting_active),
- voting_aux_active(_voting_aux_active),
- time_tolerance(_time_tolerance)
- {
- //
- }
- ProcessorParamsBase(std::string _unique_name, const paramsServer& _server):
+struct ParamsProcessorBase : public ParamsBase
+{
+ std::string prefix = "processor/";
+ ParamsProcessorBase() = default;
+ ParamsProcessorBase(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
{
- voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
- time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0");
+ time_tolerance = _server.getParam<double>(prefix + _unique_name + "/time_tolerance");
+ voting_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_active");
+ apply_loss_function = _server.getParam<bool>(prefix + _unique_name + "/apply_loss_function");
}
- virtual ~ProcessorParamsBase() = default;
+ ~ParamsProcessorBase() override = default;
- bool voting_active = false; ///< Whether this processor is allowed to vote for a Key Frame or not
- bool voting_aux_active = false; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
+ /** maximum time difference between a Keyframe time stamp and
+ * a particular Capture of this processor to allow assigning
+ * this Capture to the Keyframe.
+ */
+ double time_tolerance;
+ bool voting_active; ///< Whether this processor is allowed to vote for a Key Frame or not
+ bool apply_loss_function; ///< Whether this processor emplaces factors with loss function or not
- ///< 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);
+ std::string print() const override
+ {
+ return "voting_active: " + std::to_string(voting_active) + "\n"
+ + "time_tolerance: " + std::to_string(time_tolerance) + "\n"
+ + "apply_loss_function: " + std::to_string(apply_loss_function) + "\n";
+ }
};
//class ProcessorBase
class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
{
+ friend SensorBase;
protected:
unsigned int processor_id_;
- ProcessorParamsBasePtr params_;
- PackKeyFrameBuffer kf_pack_buffer_;
+ ParamsProcessorBasePtr params_;
+ BufferPackKeyFrame buffer_pack_kf_;
+ BufferCapture buffer_capture_;
+ int dim_;
private:
SensorBaseWPtr sensor_ptr_;
-
static unsigned int processor_id_count_;
public:
- ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params);
- virtual ~ProcessorBase();
+ ProcessorBase(const std::string& _type, int _dim, ParamsProcessorBasePtr _params);
+ ~ProcessorBase() override;
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
- unsigned int id();
+ unsigned int id() const;
+
+ // PROFILING
+ unsigned int n_capture_callback_;
+ unsigned int n_kf_callback_;
+ std::chrono::microseconds acc_duration_capture_;
+ std::chrono::microseconds acc_duration_kf_;
+ std::chrono::microseconds max_duration_capture_;
+ std::chrono::microseconds max_duration_kf_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_;
+ void startCaptureProfiling();
+ void stopCaptureProfiling();
+ void startKFProfiling();
+ void stopKFProfiling();
+ void printProfiling(std::ostream& stream = std::cout) const;
- virtual void process(CaptureBasePtr _capture_ptr) = 0;
+ protected:
+ /** \brief process an incoming capture
+ *
+ * Each derived processor should implement this function. It will be called if:
+ * - A new capture arrived and triggerInCapture() returned true.
+ */
+ virtual void processCapture(CaptureBasePtr) = 0;
- /** \brief Vote for KeyFrame generation
+ /** \brief process an incoming key-frame
*
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
+ * Each derived processor should implement this function. It will be called if:
+ * - A new KF arrived and triggerInKF() returned true.
+ */
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) = 0;
+
+ /** \brief trigger in capture
*
- * WARNING! This function only votes! It does not create KeyFrames!
+ * Returns true if processCapture() should be called after the provided capture arrived.
*/
- virtual bool voteForKeyFrame() = 0;
+ virtual bool triggerInCapture(CaptureBasePtr) const = 0;
- /** \brief Vote for Auxiliary Frame generation
+ /** \brief trigger in key-frame
*
- * If a Auxiliary Frame criterion is validated, this function returns true,
- * meaning that it wants to create a Auxiliary Frame at the \b last Capture.
+ * Returns true if processKeyFrame() should be called after the provided KF arrived.
+ */
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const = 0;
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ virtual bool storeKeyFrame(FrameBasePtr) = 0;
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ virtual bool storeCapture(CaptureBasePtr) = 0;
+
+ /** \brief Vote for KeyFrame generation
*
- * WARNING! This function only votes! It does not create Auxiliary Frames!
+ * 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 voteForAuxFrame(){return false;};
+ virtual bool voteForKeyFrame() const = 0;
virtual bool permittedKeyFrame() final;
- virtual bool permittedAuxFrame() final;
+ void setProblem(ProblemPtr) override;
- /**\brief make a Frame with the provided Capture
+ public:
+ /**\brief notify a new keyframe made by another processor
+ *
+ * It stores the new KF in buffer_pack_kf_ and calls triggerInKF()
*
- * 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);
+ void keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other);
- /**\brief make a Frame with the provided Capture
+ /**\brief notify a new capture
*
- * Provide the following functionality:
- * - Construct a Frame,
- * - Set its state vector
- * - Put it in the Trajectory, and
- * - Add the provided capture on it.
+ * It stores the new capture in buffer_capture_ and calls triggerInCapture()
*/
- FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state);
+ void captureCallback(CaptureBasePtr _capture_ptr);
- virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
-
- SensorBasePtr getSensor();
- const SensorBasePtr getSensor() const;
+ SensorBasePtr getSensor() const;
+ private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
- virtual bool isMotion();
+ public:
+ bool isMotion() const;
- void setTimeTolerance(Scalar _time_tolerance);
+ void setTimeTolerance(double _time_tolerance);
bool isVotingActive() const;
- bool isVotingAuxActive() const;
-
void setVotingActive(bool _voting_active = true);
+ int getDim() const;
+
+ double getTimeTolerance() const;
+
void link(SensorBasePtr);
template<typename classType, typename... T>
- static std::shared_ptr<ProcessorBase> emplace(SensorBasePtr _sen_ptr, T&&... all);
- void setVotingAuxActive(bool _voting_active = true);
+ static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
-inline bool ProcessorBase::isVotingActive() const
+inline void ProcessorBase::startCaptureProfiling()
{
- return params_->voting_active;
+ start_capture_ = std::chrono::high_resolution_clock::now();
+}
+
+inline void ProcessorBase::stopCaptureProfiling()
+{
+ auto duration_capture = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_capture_);
+
+ max_duration_capture_ = std::max(max_duration_capture_, duration_capture);
+ acc_duration_capture_ += duration_capture;
+}
+
+inline void ProcessorBase::startKFProfiling()
+{
+ start_kf_ = std::chrono::high_resolution_clock::now();
}
-inline bool ProcessorBase::isVotingAuxActive() const
+inline void ProcessorBase::stopKFProfiling()
{
- return params_->voting_aux_active;
+ auto duration_kf = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_kf_);
+
+ max_duration_kf_ = std::max(max_duration_kf_, duration_kf);
+ acc_duration_kf_ += duration_kf;
+}
+
+inline bool ProcessorBase::isVotingActive() const
+{
+ return params_->voting_active;
}
inline void ProcessorBase::setVotingActive(bool _voting_active)
@@ -237,76 +427,225 @@ inline void ProcessorBase::setVotingActive(bool _voting_active)
params_->voting_active = _voting_active;
}
-inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
+inline bool ProcessorBase::isMotion() const
{
- params_->voting_aux_active = _voting_active;
+ // check if this inherits from IsMotion
+ return (std::dynamic_pointer_cast<const IsMotion>(shared_from_this()) != nullptr);
}
+inline unsigned int ProcessorBase::id() const
+{
+ return processor_id_;
}
-#include "core/sensor/sensor_base.h"
-#include "core/factor/factor_base.h"
+inline SensorBasePtr ProcessorBase::getSensor() const
+{
+ return sensor_ptr_.lock();
+}
-namespace wolf {
+inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
+{
+ params_->time_tolerance = _time_tolerance;
+}
+inline int ProcessorBase::getDim() const
+{
+ return dim_;
+}
+inline double ProcessorBase::getTimeTolerance() const
+{
+ return params_->time_tolerance;
+}
template<typename classType, typename... T>
-std::shared_ptr<ProcessorBase> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
+std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
{
- ProcessorBasePtr prc = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...);
prc->link(_sen_ptr);
return prc;
}
-inline bool ProcessorBase::isMotion()
+
+/////////////////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
- return false;
+ Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+ bool post_exists = (post != container_.end());
+
+ bool post_ok = post_exists && simpleCheckTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+ if (prev_exists)
+ {
+ Buffer<T>::Iterator prev = std::prev(post);
+
+ bool prev_ok = simpleCheckTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+ if (prev_ok && !post_ok)
+ return prev;
+
+ else if (!prev_ok && post_ok)
+ return post;
+
+ else if (prev_ok && post_ok)
+ {
+ if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+ return post;
+ else
+ return prev;
+ }
+ }
+ else if (post_ok)
+ return post;
+
+ return container_.end();
}
-inline unsigned int ProcessorBase::id()
+template <typename T>
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
- return processor_id_;
+ if (container_.empty())
+ return nullptr;
+
+ Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance);
+
+ // end is returned from selectIterator if an element of the buffer complying with the time stamp
+ // and time tolerance has not been found
+ if (it != container_.end()){
+ return it->second;
+ }
+
+ return nullptr;
}
-inline SensorBasePtr ProcessorBase::getSensor()
+template <typename T>
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
- return sensor_ptr_.lock();
+ // There is no element
+ 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 (simpleCheckTimeTolerance(container_.begin()->first, _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;
}
-inline const SensorBasePtr ProcessorBase::getSensor() const
+
+template <typename T>
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
- return sensor_ptr_.lock();
+ // There is no element
+ if (container_.empty())
+ return nullptr;
+
+ // Checking on rbegin() since packs are ordered in time
+ // Return last pack if is newer than time stamp
+ if (container_.rbegin()->first > _time_stamp)
+ return container_.rbegin()->second;
+
+ // Return last pack if despite being older, it is within the time tolerance
+ if (simpleCheckTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
+ return container_.rbegin()->second;
+
+ // otherwise return nullptr (no pack after the provided ts or within the tolerance was found)
+ return nullptr;
}
-inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
+template <typename T>
+T Buffer<T>::selectFirst()
{
- params_->time_tolerance = _time_tolerance;
+ // There is no element
+ if (container_.empty())
+ return nullptr;
+
+ // Returning first map element
+ return container_.begin()->second;
}
-inline PackKeyFrameBuffer::PackKeyFrameBuffer(void)
+template <typename T>
+T Buffer<T>::selectLast()
{
+ // There is no element
+ if (container_.empty())
+ return nullptr;
+ // Returning last map element
+ return container_.rbegin()->second;
}
-inline PackKeyFrameBuffer::~PackKeyFrameBuffer(void)
+template <typename T>
+void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
{
+ container_.emplace(_time_stamp, _element);
+}
+template <typename T>
+std::map<TimeStamp,T>& Buffer<T>::getContainer()
+{
+ return container_;
}
-inline void PackKeyFrameBuffer::clear()
+template <typename T>
+inline void Buffer<T>::clear()
{
container_.clear();
}
-inline bool PackKeyFrameBuffer::empty()
+template <typename T>
+inline bool Buffer<T>::empty()
{
return container_.empty();
}
-inline SizeStd PackKeyFrameBuffer::size(void)
+template <typename T>
+inline SizeStd Buffer<T>::size(void)
{
return container_.size();
}
+template <typename T>
+inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.upper_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
+template <typename T>
+inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.lower_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
+template <typename T>
+inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
+ const TimeStamp& _time_stamp2, const double& _time_tolerance2)
+{
+ double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
+ double time_tol = std::min(_time_tolerance1, _time_tolerance2);
+ bool pass = time_diff <= time_tol;
+ return pass;
+}
+
+template <typename T>
+inline bool Buffer<T>::simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2,
+ const double& _time_tolerance)
+{
+ double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
+ bool pass = time_diff <= _time_tolerance;
+ return pass;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/processor/processor_capture_holder.h b/include/core/processor/processor_capture_holder.h
deleted file mode 100644
index 1746fd548a5a7dfb0e5f9cdaa860527b720992ae..0000000000000000000000000000000000000000
--- a/include/core/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 "core/processor/processor_base.h"
-#include "core/capture/capture_base.h"
-#include "core/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/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 2b5f1a4942f662d78b3fd97acb9714f8a00ff9ee..7203ff3fd48833c5be92de7445dc174193e6a3e1 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -1,114 +1,89 @@
/**
* \file processor_diff_drive.h
*
- * Created on: Oct 15, 2016
- * \author: Jeremie Deray
+ * Created on: Jul 22, 2019
+ * \author: jsola
*/
-#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_
-#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
+#ifndef PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
+#define PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
-#include "core/processor/processor_motion.h"
-#include "core/processor/diff_drive_tools.h"
-#include "core/utils/params_server.hpp"
+#include "core/processor/processor_odom_2d.h"
-namespace wolf {
+namespace wolf
+{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorDiffDrive);
-struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
+struct ParamsProcessorDiffDrive : public ParamsProcessorOdom2d
{
- Scalar unmeasured_perturbation_std = 0.0001;
- ProcessorParamsDiffDrive() = default;
- ProcessorParamsDiffDrive(std::string _unique_name, const paramsServer& _server):
- ProcessorParamsMotion(_unique_name, _server)
- {
- unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
- }
+ ParamsProcessorDiffDrive() = default;
+ ParamsProcessorDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorOdom2d(_unique_name, _server)
+ {
+ }
+ std::string print() const override
+ {
+ return ParamsProcessorOdom2d::print();
+ }
};
-/**
- * @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
+class ProcessorDiffDrive : public ProcessorOdom2d
{
-public:
-
- ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params);
-
- virtual ~ProcessorDiffDrive() = default;
- virtual void configure(SensorBasePtr _sensor) override { }
-
- virtual bool voteForKeyFrame() override;
-
-protected:
+ public:
+ ProcessorDiffDrive(ParamsProcessorDiffDrivePtr _params_motion);
+ ~ProcessorDiffDrive() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorDiffDrive, ParamsProcessorDiffDrive);
+
+ protected:
+ // Motion integration
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin) override;
+ VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ protected:
+ ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
+ double radians_per_tick_;
- /// @brief Intrinsic params
- ProcessorParamsDiffDrivePtr params_motion_diff_drive_;
-
- 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;
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
+{
+ if (_capture)
+ return _capture->getStateBlock('I')->getState();
+ else
+ return getSensor()->getStateBlockDynamic('I')->getState();
+}
-public:
+inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ _capture->getStateBlock('I')->setState(_calibration);
+}
- /// @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_ */
+#endif /* PROCESSOR_PROCESSOR_DIFF_DRIVE_H_ */
diff --git a/include/core/processor/processor_factory.h b/include/core/processor/processor_factory.h
deleted file mode 100644
index 8b2fdb2472b99a6eb1271e63e0196a3f0459e699..0000000000000000000000000000000000000000
--- a/include/core/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 "core/common/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 "core/sensor/sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory
- * #include "core/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 "core/sensor/sensor_odom_2D.h"
- * #include "core/processor/processor_odom_2D.h"
- * #include "core/problem/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/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c23917354ca1d0e0c3723045ded8d118e3c3abc
--- /dev/null
+++ b/include/core/processor/processor_fix_wing_model.h
@@ -0,0 +1,92 @@
+/*
+ * processor_fix_wing_model.h
+ *
+ * Created on: Sep 6, 2021
+ * Author: joanvallve
+ */
+
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
+
+#include "core/processor/processor_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
+
+struct ParamsProcessorFixWingModel : public ParamsProcessorBase
+{
+ Eigen::Vector3d velocity_local;
+ double angle_stdev;
+ double min_vel_norm;
+
+ ParamsProcessorFixWingModel() = default;
+ ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
+ angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
+ min_vel_norm = _server.getParam<double> (prefix + _unique_name + "/min_vel_norm");
+
+ assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
+ }
+ std::string print() const override
+ {
+ return ParamsProcessorBase::print() + "\n"
+ + "velocity_local: print not implemented\n"
+ + "angle_stdev: " + std::to_string(angle_stdev) + "\n"
+ + "min_vel_norm: " + std::to_string(min_vel_norm) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
+
+class ProcessorFixWingModel : public ProcessorBase
+{
+ public:
+ ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params);
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
+
+ virtual ~ProcessorFixWingModel() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ protected:
+
+ /** \brief process an incoming capture NEVER CALLED
+ */
+ virtual void processCapture(CaptureBasePtr) override {};
+
+ /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
+ */
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+
+ /** \brief trigger in capture
+ */
+ virtual bool triggerInCapture(CaptureBasePtr) const override {return false;};
+
+ /** \brief trigger in key-frame
+ */
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return true;};
+
+ /** \brief store key frame
+ */
+ virtual bool storeKeyFrame(FrameBasePtr) override {return false;};
+
+ /** \brief store capture
+ */
+ virtual bool storeCapture(CaptureBasePtr) override {return false;};
+
+ /** \brief Vote for KeyFrame generation
+ */
+ virtual bool voteForKeyFrame() const override {return false;};
+
+ // ATTRIBUTES
+ ParamsProcessorFixWingModelPtr params_processor_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_ */
diff --git a/include/core/processor/processor_frame_nearest_neighbor_filter.h b/include/core/processor/processor_frame_nearest_neighbor_filter.h
deleted file mode 100644
index dffee75750a452891432aa255447ad1ffb8491b2..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_frame_nearest_neighbor_filter.h
+++ /dev/null
@@ -1,128 +0,0 @@
-#ifndef _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-#define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-
-// Wolf related headers
-#include "core/processor/processor_loopclosure_base.h"
-#include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
-
-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_)
- {
- //
- }
- ProcessorParamsFrameNearestNeighborFilter(std::string _unique_name, const paramsServer& _server):
- ProcessorParamsLoopClosure(_unique_name, _server)
- {
- buffer_size_ = _server.getParam<int>(_unique_name + "/buffer_size");
- sample_step_degree_ = _server.getParam<int>(_unique_name + "/sample_step_degree");
- auto distance_type_str = _server.getParam<std::string>(_unique_name + "/distance_type");
- if(distance_type_str.compare("LC_POINT_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSE;
- else if(distance_type_str.compare("LC_ELLIPSE_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
- else if(distance_type_str.compare("LC_POINT_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSOID;
- else if(distance_type_str.compare("LC_ELLIPSOID_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID;
- else if(distance_type_str.compare("LC_MAHALANOBIS_DISTANCE")) distance_type_ = LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE;
- else throw std::runtime_error("Failed to fetch a valid value for the enumerate LoopclosureDistanceType. Value provided: " + distance_type_str);
- probability_ = _server.getParam<Scalar>(_unique_name + "/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/core/processor/processor_logging.h b/include/core/processor/processor_logging.h
index de1d5ead89e38e28f15aa6b1c789350c3a0b0384..bc1f8399aadc6ac344973baa377b8ea8bdb85db7 100644
--- a/include/core/processor/processor_logging.h
+++ b/include/core/processor/processor_logging.h
@@ -9,7 +9,7 @@
#define _WOLF_PROCESSOR_LOGGING_H_
/// @brief un-comment for IDE highlights.
-//#include "core/utils/logging.h"
+
#define __INTERNAL_WOLF_ASSERT_PROCESSOR \
static_assert(std::is_base_of<ProcessorBase, \
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
new file mode 100644
index 0000000000000000000000000000000000000000..94af3fa1692397e533db0379a342a39edebb51e0
--- /dev/null
+++ b/include/core/processor/processor_loop_closure.h
@@ -0,0 +1,110 @@
+#ifndef _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
+#define _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
+
+// Wolf related headers
+#include "core/processor/processor_base.h"
+
+namespace wolf{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
+
+struct ParamsProcessorLoopClosure : public ParamsProcessorBase
+{
+ int max_loops=-1;
+
+ ParamsProcessorLoopClosure() = default;
+ ParamsProcessorLoopClosure(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ max_loops = _server.getParam<int>(prefix + _unique_name + "/max_loops");
+ }
+
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print()
+ + "max_loops: " + std::to_string(max_loops) + "\n";
+ }
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
+
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
+struct MatchLoopClosure
+{
+ CaptureBasePtr capture_reference; ///< Capture reference
+ CaptureBasePtr capture_target; ///< Capture target
+ double normalized_score; ///< normalized similarity score (0 is bad, 1 is good)
+};
+
+/** \brief General loop closure processor
+ *
+ * This is an abstract class.
+ * + You must define the following classes :
+ * - voteFindLoopClosures(CaptureBasePtr)
+ * - emplaceFeatures(CaptureBasePtr)
+ * - findLoopClosures(CaptureBasePtr)
+ * - validateLoop(CaptureBasePtr, CaptureBasePtr)
+ * - emplaceFactors(CaptureBasePtr, CaptureBasePtr)
+ * + You can override the following classes :
+ * - process(CaptureBasePtr)
+ */
+class ProcessorLoopClosure : public ProcessorBase
+{
+ protected:
+
+ ParamsProcessorLoopClosurePtr params_loop_closure_;
+
+ public:
+
+ ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
+
+ ~ProcessorLoopClosure() override = default;
+ void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ /** \brief Process a capture (linked to a frame)
+ * If voteFindLoopClosures() returns true, findLoopClosures() is called.
+ * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
+ */
+ virtual void process(CaptureBasePtr);
+
+ /** \brief Returns if findLoopClosures() has to be called for the given capture
+ */
+ virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
+
+ /** \brief detects and emplaces all features of the given capture
+ */
+ virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
+
+ /** \brief Find captures that correspond to loop closures with the given capture
+ */
+ virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
+
+ /** \brief validates a loop closure
+ */
+ virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
+
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
+
+ void processCapture(CaptureBasePtr) override;
+ void processKeyFrame(FrameBasePtr, const double&) override;
+
+ bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+
+ bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
+ bool storeCapture(CaptureBasePtr _cap) override { return false;};
+
+ bool voteForKeyFrame() const override { return false;};
+};
+
+} // namespace wolf
+
+#endif /* _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H */
diff --git a/include/core/processor/processor_loopclosure_base.h b/include/core/processor/processor_loopclosure_base.h
deleted file mode 100644
index a5dc8589156b6b525b0a9b551ec3dc55ab7f5af7..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_loopclosure_base.h
+++ /dev/null
@@ -1,140 +0,0 @@
-#ifndef _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-#define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-
-// Wolf related headers
-#include "core/processor/processor_base.h"
-
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure);
-
-struct ProcessorParamsLoopClosure : public ProcessorParamsBase
-{
- using ProcessorParamsBase::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/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 5b971769b72a357d2b775fa4503670ada36711c9..207bacb8eb7c9770787e00e9a811b30543708c51 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -11,8 +11,9 @@
// Wolf
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
// std
#include <iomanip>
@@ -20,25 +21,38 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion);
-struct ProcessorParamsMotion : public ProcessorParamsBase
+struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion
{
- Scalar max_time_span = 0.5;
+ double max_time_span = 0.5;
unsigned int max_buff_length = 10;
- Scalar dist_traveled = 5;
- Scalar angle_turned = 0.5;
- Scalar unmeasured_perturbation_std = 1e-4;
- ProcessorParamsMotion() = default;
- ProcessorParamsMotion(std::string _unique_name, const paramsServer& _server):
- ProcessorParamsBase(_unique_name, _server)
- {
- max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span", "0.5");
- max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length", "10");
- dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled", "5");
- angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5");
- unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4");
- }
+ double dist_traveled = 5;
+ double angle_turned = 0.5;
+ double unmeasured_perturbation_std = 1e-4;
+
+ ParamsProcessorMotion() = default;
+ ParamsProcessorMotion(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server),
+ ParamsIsMotion(_unique_name, _server)
+ {
+ max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
+ max_buff_length = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/max_buff_length");
+ dist_traveled = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/dist_traveled");
+ angle_turned = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/angle_turned");
+ unmeasured_perturbation_std = _server.getParam<double>(prefix + _unique_name + "/unmeasured_perturbation_std");
+ }
+ std::string print() const override
+ {
+ return ParamsProcessorBase::print() + "\n" +
+ ParamsIsMotion::print() + "\n"
+ + "max_time_span: " + std::to_string(max_time_span) + "\n"
+ + "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
+ + "angle_turned: " + std::to_string(angle_turned) + "\n"
+ + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
+ }
+
};
/** \brief class for Motion processors
@@ -88,7 +102,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
* - 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)
+ * void data2delta(const VectorXd _data)
* {
* delta_S = format(_data);
* delta_R = fromSensorFrame(delta_S);
@@ -118,70 +132,49 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
* // TODO: JS: review instructions up to here
*
*/
-class ProcessorMotion : public ProcessorBase
+class ProcessorMotion : public ProcessorBase, public IsMotion
{
public:
typedef enum {
- RUNNING_WITHOUT_PACK,
- RUNNING_WITH_PACK_BEFORE_ORIGIN,
- RUNNING_WITH_PACK_ON_ORIGIN,
- RUNNING_WITH_PACK_AFTER_ORIGIN
+ FIRST_TIME_WITHOUT_KF,
+ FIRST_TIME_WITH_KF_BEFORE_INCOMING,
+ FIRST_TIME_WITH_KF_ON_INCOMING,
+ FIRST_TIME_WITH_KF_AFTER_INCOMING,
+ RUNNING_WITHOUT_KF,
+ RUNNING_WITH_KF_BEFORE_ORIGIN,
+ RUNNING_WITH_KF_ON_ORIGIN,
+ RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep ;
protected:
- ProcessorParamsMotionPtr params_motion_;
+ ParamsProcessorMotionPtr params_motion_;
ProcessingStep processing_step_; ///< State machine controlling the processing step
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
+ int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
- ProcessorParamsMotionPtr _params_motion);
- virtual ~ProcessorMotion();
+ ParamsProcessorMotionPtr _params_motion);
+ ~ProcessorMotion() override;
// 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);
+ TimeStamp getTimeStamp() const override;
+ VectorComposite getState(const StateStructure& _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
*/
- const Eigen::MatrixXs getCurrentDeltaPreintCov();
+ const Eigen::MatrixXd getCurrentDeltaPreintCov() const;
/** \brief Provide the motion integrated so far
* \return the integrated motion
@@ -207,21 +200,60 @@ class ProcessorMotion : public ProcessorBase
* \param _x_origin the state at the origin
* \param _ts_origin origin timestamp.
*/
- FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
+ FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin);
+ // Helper functions:
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
- // Helper functions:
protected:
- Scalar updateDt();
+ /** \brief process an incoming capture
+ *
+ * Each derived processor should implement this function. It will be called if:
+ * - A new capture arrived and triggerInCapture() returned true.
+ */
+ void processCapture(CaptureBasePtr) override;
+
+ /** \brief process an incoming key-frame
+ *
+ * The ProcessorMotion only processes incoming captures (it is not called).
+ */
+ void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+
+ /** \brief trigger in capture
+ *
+ * Returns true if processCapture() should be called after the provided capture arrived.
+ */
+ bool triggerInCapture(CaptureBasePtr) const override {return true;}
+
+ /** \brief trigger in key-frame
+ *
+ * The ProcessorMotion only processes incoming captures, then it returns false.
+ */
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ bool storeKeyFrame(FrameBasePtr) override;
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ bool storeCapture(CaptureBasePtr) override;
+
+ bool voteForKeyFrame() const override;
+
+ double updateDt();
void integrateOneStep();
- void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
+ void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
TimeStamp ts_split,
const FrameBasePtr& keyframe_target,
- const wolf::CaptureMotionPtr& capture_target);
+ const wolf::CaptureMotionPtr& capture_target) const;
/** Pre-process incoming Capture
*
@@ -234,7 +266,7 @@ class ProcessorMotion : public ProcessorBase
* - initializing counters, flags, or any derived variables
* - initializing algorithms needed for processing the derived data
*/
- virtual void preProcess() { };
+ virtual void preProcess(){ };
/** Post-process
*
@@ -246,12 +278,12 @@ class ProcessorMotion : public ProcessorBase
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
- virtual void postProcess() { };
+ virtual void postProcess(){ };
PackKeyFramePtr computeProcessingStep();
// These are the pure virtual functions doing the mathematics
- protected:
+ public:
/** \brief convert raw CaptureMotion data to the delta-state format
*
@@ -271,7 +303,7 @@ class ProcessorMotion : public ProcessorBase
*
* 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.
+ * See the class documentation for some Eigen::VectorXd 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,
@@ -305,13 +337,13 @@ class ProcessorMotion : public ProcessorBase
*
* 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;
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const = 0;
/** \brief composes a delta-state on top of another delta-state
* \param _delta1 the first delta-state
@@ -321,10 +353,10 @@ class ProcessorMotion : public ProcessorBase
*
* 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;
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const = 0;
/** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
* \param _delta1 the first delta-state
@@ -336,12 +368,12 @@ class ProcessorMotion : public ProcessorBase
*
* 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;
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const = 0;
/** \brief composes a delta-state on top of a state
* \param _x the initial state
@@ -351,113 +383,112 @@ class ProcessorMotion : public ProcessorBase
*
* 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;
+ virtual void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const = 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]
+ * Examples (see documentation of the the class for info on Eigen::VectorXd):
+ * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::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;
+ virtual Eigen::VectorXd 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.
+
+ /** \brief correct the preintegrated delta
+ * This produces a delta correction according to the appropriate manifold.
+ * @param delta_preint : the preintegrated delta to correct
+ * @param delta_step : an increment in the tangent space of the manifold
*
- * 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.
+ * We want to do
*
- * 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
+ * delta_corr = delta_preint (+) d_step
*
- * @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).
+ * where the operator (+) is the right-plus operator on the delta's manifold.
*
- * This function interpolates a motion between two existing motions.
+ * Note: usually in motion pre-integration we have
*
- * 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.
+ * delta_step = Jac_delta_calib * (calib - calib_pre)
*
- * 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);
+ virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const = 0;
+
+ /** \brief merge two consecutive capture motions into the second one
+ * Merge two consecutive capture motions into the second one.
+ * The first capture motion is not removed.
+ * If the second capture has feature and factor emplaced, they are replaced by a new ones.
+ * @param cap_prev : the first capture motion to be merged (input)
+ * @param cap_target : the second capture motion (modified)
+ */
+ void mergeCaptures(CaptureMotionConstPtr cap_prev,
+ CaptureMotionPtr cap_target);
- /** \brief create a CaptureMotion and add it to a Frame
- * \param _ts time stamp
+ protected:
+ /** \brief emplace a CaptureMotion
+ * \param _frame_own frame owning the Capture to create
* \param _sensor Sensor that produced the Capture
+ * \param _ts time stamp
* \param _data a vector of motion data
- * \param _sata_cov covariances matrix of the motion data data
- * \param _frame_own frame owning the Capture to create
- * \param _frame_origin frame acting as the origin of motions for the MorionBuffer of the created MotionCapture
- */
- 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
+ * \param _data_cov covariances matrix of the motion data data
+ * \param _calib calibration vector
+ * \param _calib_preint calibration vector used during pre-integration
+ * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
*/
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own);
-
- /** \brief create a feature corresponding to given capture
+ virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) = 0;
+
+ /** \brief emplace a feature corresponding to given capture and add the feature to this capture
* \param _capture_motion: the parent capture
*/
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
+ virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) = 0;
- /** \brief create a factor and link it in the wolf tree
+ /** \brief emplace a factor and link it in the wolf tree
* \param _feature_motion: the parent feature
* \param _frame_origin: the frame constrained by this motion factor
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
- Motion motionZero(const TimeStamp& _ts);
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
- bool hasCalibration() {return calib_size_ > 0;}
+ Motion motionZero(const TimeStamp& _ts) const;
public:
- //getters
- CaptureMotionPtr getOrigin();
- CaptureMotionPtr getLast();
- CaptureMotionPtr getIncoming();
- Scalar getMaxTimeSpan() const;
- Scalar getMaxBuffLength() const;
- Scalar getDistTraveled() const;
- Scalar getAngleTurned() const;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0;
+ bool hasCalibration() const {return calib_size_ > 0;}
- 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);
+ //getters
+ CaptureMotionPtr getOrigin() const;
+ CaptureMotionPtr getLast() const;
+ CaptureMotionPtr getIncoming() const;
+
+ double getMaxTimeSpan() const;
+ double getMaxBuffLength() const;
+ double getDistTraveled() const;
+ double getAngleTurned() const;
+
+ void setMaxTimeSpan(const double& _max_time_span);
+ void setMaxBuffLength(const double& _max_buff_length);
+ void setDistTraveled(const double& _dist_traveled);
+ void setAngleTurned(const double& _angle_turned);
+
+ void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
protected:
// Attributes
@@ -470,21 +501,23 @@ class ProcessorMotion : public ProcessorBase
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
+ FrameBasePtr last_frame_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
- Eigen::MatrixXs unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
+ mutable double dt_; ///< Time step
+ mutable Eigen::VectorXd x_; ///< current state
+ mutable Eigen::VectorXd delta_; ///< current delta
+ mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance
+ mutable Eigen::VectorXd delta_integrated_; ///< integrated delta
+ mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
+ mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
+ mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
+ mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
+ mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
+ mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
+
+ Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
}
@@ -498,44 +531,19 @@ inline void ProcessorMotion::resetDerived()
// Blank function, to be implemented in derived classes
}
-inline bool ProcessorMotion::voteForKeyFrame()
+inline bool ProcessorMotion::voteForKeyFrame() const
{
return false;
}
-inline Eigen::VectorXs ProcessorMotion::getState(const TimeStamp& _ts)
+inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
{
- 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_;
+ return getBuffer().back().delta_integr_cov_;
}
inline Motion ProcessorMotion::getMotion() const
{
- return getBuffer().get().back();
+ return getBuffer().back();
}
inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
@@ -546,89 +554,86 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
return capture_ptr->getBuffer().getMotion(_ts);
}
-inline bool ProcessorMotion::isMotion()
-{
- return true;
-}
-
-inline Scalar ProcessorMotion::updateDt()
+inline double ProcessorMotion::updateDt()
{
- return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
+ return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().back().ts_;
}
inline const MotionBuffer& ProcessorMotion::getBuffer() const
{
+ assert(last_ptr_);
return last_ptr_->getBuffer();
}
inline MotionBuffer& ProcessorMotion::getBuffer()
{
+ assert(last_ptr_);
return last_ptr_->getBuffer();
}
-inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
+inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
{
return Motion(_ts,
- VectorXs::Zero(data_size_), // data
- Eigen::MatrixXs::Zero(data_size_, data_size_), // Cov data
+ VectorXd::Zero(data_size_), // data
+ Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
deltaZero(),
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
+ Eigen::MatrixXd::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
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
+ Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib
);
}
-inline CaptureMotionPtr ProcessorMotion::getOrigin()
+inline CaptureMotionPtr ProcessorMotion::getOrigin() const
{
return origin_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getLast()
+inline CaptureMotionPtr ProcessorMotion::getLast() const
{
return last_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getIncoming()
+inline CaptureMotionPtr ProcessorMotion::getIncoming() const
{
return incoming_ptr_;
}
-inline Scalar ProcessorMotion::getMaxTimeSpan() const
+inline double ProcessorMotion::getMaxTimeSpan() const
{
return params_motion_->max_time_span;
}
-inline Scalar ProcessorMotion::getMaxBuffLength() const
+inline double ProcessorMotion::getMaxBuffLength() const
{
return params_motion_->max_buff_length;
}
-inline Scalar ProcessorMotion::getDistTraveled() const
+inline double ProcessorMotion::getDistTraveled() const
{
return params_motion_->dist_traveled;
}
-inline Scalar ProcessorMotion::getAngleTurned() const
+inline double ProcessorMotion::getAngleTurned() const
{
return params_motion_->angle_turned;
}
-inline void ProcessorMotion::setMaxTimeSpan(const Scalar& _max_time_span)
+inline void ProcessorMotion::setMaxTimeSpan(const double& _max_time_span)
{
params_motion_->max_time_span = _max_time_span;
}
-inline void ProcessorMotion::setMaxBuffLength(const Scalar& _max_buff_length)
+inline void ProcessorMotion::setMaxBuffLength(const double& _max_buff_length)
{
params_motion_->max_buff_length = _max_buff_length;
}
-inline void ProcessorMotion::setDistTraveled(const Scalar& _dist_traveled)
+inline void ProcessorMotion::setDistTraveled(const double& _dist_traveled)
{
params_motion_->dist_traveled = _dist_traveled;
}
-inline void ProcessorMotion::setAngleTurned(const Scalar& _angle_turned)
+inline void ProcessorMotion::setAngleTurned(const double& _angle_turned)
{
params_motion_->angle_turned = _angle_turned;
}
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
deleted file mode 100644
index 90d47452d1334d731b39c6154edfef8400573b0a..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_odom_2D.h
+++ /dev/null
@@ -1,98 +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 "core/processor/processor_motion.h"
-#include "core/capture/capture_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/math/rotations.h"
-#include "core/utils/params_server.hpp"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorOdom2D);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
-
-struct ProcessorParamsOdom2D : public ProcessorParamsMotion
-{
- Scalar cov_det = 1.0; // 1 rad^2
- ProcessorParamsOdom2D() = default;
- ProcessorParamsOdom2D(std::string _unique_name, const wolf::paramsServer & _server):
- ProcessorParamsMotion(_unique_name, _server)
- {
- cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
- unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.001");
- }
-};
-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_;
-
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
- static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server, 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/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..254eded5ccc644faf5b6c20e128f55da77c92b95
--- /dev/null
+++ b/include/core/processor/processor_odom_2d.h
@@ -0,0 +1,121 @@
+/**
+ * \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 "core/processor/processor_motion.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+#include "core/utils/params_server.h"
+#include "core/math/SE2.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom2d);
+
+struct ParamsProcessorOdom2d : public ParamsProcessorMotion
+{
+ double cov_det = 1.0;
+
+ ParamsProcessorOdom2d() = default;
+ ParamsProcessorOdom2d(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorMotion(_unique_name, _server)
+ {
+ cov_det = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/cov_det");
+ }
+
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print() + "\n"
+ + "cov_det: " + std::to_string(cov_det) + "\n";
+ }
+};
+
+class ProcessorOdom2d : public ProcessorMotion
+{
+ public:
+ ProcessorOdom2d(ParamsProcessorOdom2dPtr _params);
+ ~ProcessorOdom2d() override;
+ void configure(SensorBasePtr _sensor) override { };
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ParamsProcessorOdom2d);
+
+ bool voteForKeyFrame() const override;
+
+ protected:
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
+ CaptureBasePtr _capture_origin) override;
+ VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ protected:
+ ParamsProcessorOdom2dPtr params_odom_2d_;
+
+};
+
+inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
+{
+ return Eigen::VectorXd::Zero(delta_size_);
+}
+
+inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ VectorXd delta_corrected(3);
+ SE2::plus(delta_preint, delta_step, delta_corrected);
+ return delta_corrected;
+}
+
+inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+
+inline void ProcessorOdom2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+}
+
+} // namespace wolf
+
+#endif /* SRC_PROCESSOR_ODOM_2d_H_ */
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
deleted file mode 100644
index 38683c57f04c5b6908bac19e26b5cc9ee3353e49..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_odom_3D.h
+++ /dev/null
@@ -1,134 +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 "core/processor/processor_motion.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/capture/capture_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/math/rotations.h"
-#include <cmath>
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
-
-struct ProcessorParamsOdom3D : public ProcessorParamsMotion
-{
- ProcessorParamsOdom3D() = default;
- ProcessorParamsOdom3D(std::string _unique_name, const paramsServer& _server):
- ProcessorParamsMotion(_unique_name, _server)
- {
- //
- }
-};
-
-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/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..95035e56abd313b123c61aebf6068b862b6b794e
--- /dev/null
+++ b/include/core/processor/processor_odom_3d.h
@@ -0,0 +1,126 @@
+/**
+ * \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 "core/processor/processor_motion.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/math/rotations.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include <cmath>
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom3d);
+
+struct ParamsProcessorOdom3d : public ParamsProcessorMotion
+{
+ ParamsProcessorOdom3d() = default;
+ ParamsProcessorOdom3d(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorMotion(_unique_name, _server)
+ {
+ //
+ }
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print();
+ }
+};
+
+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(ParamsProcessorOdom3dPtr _params);
+ ~ProcessorOdom3d() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorOdom3d, ParamsProcessorOdom3d);
+
+ public:
+ // Motion integration
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
+ bool voteForKeyFrame() const override;
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
+
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin) override;
+ VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ protected:
+ ParamsProcessorOdom3dPtr params_odom_3d_;
+
+};
+
+inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const
+{
+ return (Eigen::VectorXd(7) << 0,0,0, 0,0,0,1).finished(); // p, q
+}
+
+} // namespace wolf
+
+#endif /* SRC_PROCESSOR_ODOM_3d_H_ */
diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e0a68527a194bf071c38ee2ffadf561de2524ea
--- /dev/null
+++ b/include/core/processor/processor_pose.h
@@ -0,0 +1,68 @@
+#ifndef PROCESSOR_POSE_NOMOVE_H
+#define PROCESSOR_POSE_NOMOVE_H
+
+// Wolf
+#include "core/sensor/sensor_base.h"
+#include "core/processor/processor_base.h"
+
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPose);
+
+struct ParamsProcessorPose : public ParamsProcessorBase
+{
+ ParamsProcessorPose() = default;
+ ParamsProcessorPose(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ }
+ ~ParamsProcessorPose() override = default;
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print() + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorPose);
+
+//class
+class ProcessorPose : public ProcessorBase{
+ public:
+ ProcessorPose(ParamsProcessorPosePtr _params_pfnomove);
+ ~ProcessorPose() override = default;
+ WOLF_PROCESSOR_CREATE(ProcessorPose, ParamsProcessorPose);
+
+ void configure(SensorBasePtr _sensor) override;
+
+ void processCapture(CaptureBasePtr) override;
+ void processKeyFrame(FrameBasePtr, const double&) override;
+ bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+ bool storeKeyFrame(FrameBasePtr _frm) override { return true;};
+ bool storeCapture(CaptureBasePtr _cap) override { return true;};
+ bool voteForKeyFrame() const override { return false;};
+ void createFactorIfNecessary();
+
+ protected:
+ ParamsProcessorPosePtr params_pfnomove_;
+};
+
+
+
+
+
+} /* namespace wolf */
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+
+namespace wolf{
+
+} // namespace wolf
+
+#endif // PROCESSOR_POSE_NOMOVE_H
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 56be18b64f9e71b2dabe5b4fde223174711326ce..855d30ee277a718826870f150d6bc13d789b0931 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -13,20 +13,27 @@
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTracker);
-struct ProcessorParamsTracker : public ProcessorParamsBase
+struct ParamsProcessorTracker : public ParamsProcessorBase
{
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.)
- ProcessorParamsTracker() = default;
- ProcessorParamsTracker(std::string _unique_name, const wolf::paramsServer & _server):
- ProcessorParamsBase(_unique_name, _server)
+ ParamsProcessorTracker() = default;
+ ParamsProcessorTracker(std::string _unique_name, const wolf::ParamsServer & _server):
+ ParamsProcessorBase(_unique_name, _server)
{
- min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1");
- max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1");
+ min_features_for_keyframe = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/min_features_for_keyframe");
+ max_new_features = _server.getParam<int>(prefix + _unique_name + "/max_new_features");
}
+ std::string print() const override
+ {
+ return ParamsProcessorBase::print() + "\n"
+ + "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
+ + "max_new_features: " + std::to_string(max_new_features) + "\n";
+ }
+
};
WOLF_PTR_TYPEDEFS(ProcessorTracker);
@@ -89,38 +96,74 @@ class ProcessorTracker : public ProcessorBase
} ProcessingStep ;
protected:
- ProcessorParamsTrackerPtr params_tracker_; ///< parameters for the tracker
+ ParamsProcessorTrackerPtr 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.
+ FrameBasePtr last_frame_ptr_;
FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
-
- SizeStd number_of_tracks_;
+ FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
+ FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
+ StateStructure state_structure_; ///< Structure of frames created or used by this processor
public:
ProcessorTracker(const std::string& _type,
- ProcessorParamsTrackerPtr _params_tracker);
- virtual ~ProcessorTracker();
+ const StateStructure& _structure,
+ int _dim,
+ ParamsProcessorTrackerPtr _params_tracker);
+ ~ProcessorTracker() 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 const _incoming_ptr);
+ StateStructure getStateStructure() const;
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();
+ virtual CaptureBasePtr getOrigin() const;
+ virtual CaptureBasePtr getLast() const;
+ virtual CaptureBasePtr getIncoming() const;
protected:
+ /** \brief process an incoming capture
+ *
+ * Each derived processor should implement this function. It will be called if:
+ * - A new capture arrived and triggerInCapture() returned true.
+ */
+ void processCapture(CaptureBasePtr) override;
+
+ /** \brief process an incoming key-frame
+ *
+ * The ProcessorTracker only processes incoming captures (it is not called).
+ */
+ void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
+
+ /** \brief trigger in capture
+ *
+ * Returns true if processCapture() should be called after the provided capture arrived.
+ */
+ bool triggerInCapture(CaptureBasePtr) const override;
+
+ /** \brief trigger in key-frame
+ *
+ * The ProcessorTracker only processes incoming captures, then it returns false.
+ */
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return false;}
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ bool storeKeyFrame(FrameBasePtr) override;
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ bool storeCapture(CaptureBasePtr) override;
+
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
@@ -177,7 +220,7 @@ class ProcessorTracker : public ProcessorBase
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() = 0;
+ bool voteForKeyFrame() const override = 0;
/** \brief Advance the incoming Capture to become the last.
*
@@ -204,15 +247,16 @@ class ProcessorTracker : public ProcessorBase
FeatureBasePtrList& getNewFeaturesListLast();
- const SizeStd& previousNumberOfTracks() const
- {
- return number_of_tracks_;
+ std::string print() const {
+ return this->params_tracker_->print();
}
- SizeStd& previousNumberOfTracks()
- {
- return number_of_tracks_;
- }
+ void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
protected:
@@ -261,22 +305,27 @@ inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const
return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
}
+inline StateStructure ProcessorTracker::getStateStructure ( ) const
+{
+ return state_structure_;
+}
+
inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
}
-inline CaptureBasePtr ProcessorTracker::getOrigin()
+inline CaptureBasePtr ProcessorTracker::getOrigin() const
{
return origin_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getLast()
+inline CaptureBasePtr ProcessorTracker::getLast() const
{
return last_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getIncoming()
+inline CaptureBasePtr ProcessorTracker::getIncoming() const
{
return incoming_ptr_;
}
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 055e2ddf87220a65289c09e20746dd4da990541d..86fc8f35a78a78d882820f3512a6bcec12eeb193 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -18,11 +18,11 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeature);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeature);
-struct ProcessorParamsTrackerFeature : public ProcessorParamsTracker
+struct ParamsProcessorTrackerFeature : public ParamsProcessorTracker
{
- using ProcessorParamsTracker::ProcessorParamsTracker;
+ using ParamsProcessorTracker::ParamsProcessorTracker;
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
@@ -85,14 +85,15 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Constructor with type
*/
ProcessorTrackerFeature(const std::string& _type,
- ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
- virtual ~ProcessorTrackerFeature();
+ const StateStructure& _structure,
+ int _dim,
+ ParamsProcessorTrackerFeaturePtr _params_tracker_feature);
+ ~ProcessorTrackerFeature() override;
protected:
- ProcessorParamsTrackerFeaturePtr params_tracker_feature_;
+ ParamsProcessorTrackerFeaturePtr params_tracker_feature_;
TrackMatrix track_matrix_;
- FeatureBasePtrList known_features_incoming_;
FeatureMatchMap matches_last_from_incoming_;
/** \brief Process known Features
@@ -111,21 +112,34 @@ class ProcessorTrackerFeature : public ProcessorTracker
* - Create the factors, of the correct type, derived from FactorBase
* (through FactorAnalytic or FactorSparse).
*/
- virtual unsigned int processKnown();
+ unsigned int processKnown() override;
- /** \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
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
+ *
+ * \return the number of features tracked
*/
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_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;
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -134,42 +148,50 @@ class ProcessorTrackerFeature : public ProcessorTracker
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() = 0;
+ bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
- void advanceDerived();
- void resetDerived();
+ void advanceDerived() override;
+ void resetDerived() override;
/**\brief Process new Features
*
*/
- virtual unsigned int processNew(const int& _max_features);
+ unsigned int processNew(const int& _max_features) override;
/** \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.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
+ *
* 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;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) = 0;
- /** \brief Create a new factor and link it to the wolf tree
+ /** \brief Emplaces a new factor
* \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.
+ * This function emplaces a factor of the appropriate type for the derived processor.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
- /** \brief Establish factors between features in Captures \b last and \b origin
+ /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in Capture \b origin
*/
- virtual void establishFactors();
+ void establishFactors() override;
};
} // namespace wolf
diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h
deleted file mode 100644
index f16aa35f9ab1475ba5c2cc807de3fbb81dffc683..0000000000000000000000000000000000000000
--- a/include/core/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 "core/common/wolf.h"
-#include "core/processor/processor_tracker_feature.h"
-#include "core/factor/factor_feature_dummy.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_incoming_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/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index 0e7e2e0a2b7bac5161caa2d7969766a2b57f2581..23f0b89fc03432ea411227ca9de85dc40407693e 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -16,11 +16,11 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmark);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmark);
-struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker
+struct ParamsProcessorTrackerLandmark : public ParamsProcessorTracker
{
- using ProcessorParamsTracker::ProcessorParamsTracker;
+ using ParamsProcessorTracker::ParamsProcessorTracker;
//
};
@@ -81,12 +81,13 @@ class ProcessorTrackerLandmark : public ProcessorTracker
{
public:
ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
- virtual ~ProcessorTrackerLandmark();
+ const StateStructure& _structure,
+ ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark);
+ ~ProcessorTrackerLandmark() override;
protected:
- ProcessorParamsTrackerLandmarkPtr params_tracker_landmark_;
+ ParamsProcessorTrackerLandmarkPtr params_tracker_landmark_;
LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
LandmarkMatchMap matches_landmark_from_incoming_;
LandmarkMatchMap matches_landmark_from_last_;
@@ -102,17 +103,25 @@ class ProcessorTrackerLandmark : public ProcessorTracker
* 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();
+ unsigned int processKnown() override;
- /** \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
+ /** \brief Find provided landmarks as features in the provided capture
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture 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
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
+ *
* \return the number of landmarks found
*/
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences) = 0;
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -121,50 +130,58 @@ class ProcessorTrackerLandmark : public ProcessorTracker
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() = 0;
+ bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
- void advanceDerived();
- void resetDerived();
+ void advanceDerived() override;
+ void resetDerived() override;
/** \brief Process new Features
*
*/
- unsigned int processNew(const int& _max_features);
+ unsigned int processNew(const int& _max_features) override;
/** \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.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
- * This function detects Features that do not correspond to known Landmarks in the system.
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
*
* 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_incoming_out) = 0;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) = 0;
- /** \brief Creates a landmark for each of new_features_last_
+ /** \brief Emplaces a landmark for each new feature of new_features_last_
**/
- virtual void createNewLandmarks();
+ virtual void emplaceNewLandmarks();
- /** \brief Create one landmark
+ /** \brief Emplaces one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) = 0;
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
- /** \brief Create a new factor
+ /** \brief Emplaces 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;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
- /** \brief Establish factors between features in Capture \b last and landmarks
+ /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a landmark
*/
- virtual void establishFactors();
+ void establishFactors() override;
};
}// namespace wolf
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 511f22a096d830d898049fa966ad1d63ab42c624..1d8bfb343d851a7c5890a8442d844d3bc7b44e9e 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -63,12 +63,12 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat
*
* 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.
+ * add(Cap, track_id, f) will set f.capture_ptr = C and f.track_id = track_id.
*
* so e.g. given a feature 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.
+ * getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f.
*
*/
@@ -78,34 +78,43 @@ class TrackMatrix
TrackMatrix();
virtual ~TrackMatrix();
- void newTrack (CaptureBasePtr _cap, FeatureBasePtr _ftr);
- void add (size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr);
+ void newTrack (FeatureBasePtr _ftr);
+ void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
+ void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
void remove (FeatureBasePtr _ftr);
- void remove (size_t _track_id);
+ void remove (const SizeStd& _track_id);
void remove (CaptureBasePtr _cap);
- SizeStd numTracks ();
- SizeStd trackSize (size_t _track_id);
- Track track (size_t _track_id);
- Snapshot snapshot (CaptureBasePtr _capture);
+ SizeStd numTracks () const;
+ SizeStd trackSize (const SizeStd& _track_id) const;
+ Track track (const SizeStd& _track_id) const;
+ Snapshot snapshot (CaptureBasePtr _capture) const;
vector<FeatureBasePtr>
- trackAsVector(size_t _track_id);
+ trackAsVector(const SizeStd& _track_id) const;
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);
- CaptureBasePtr firstCapture(size_t _track_id);
+ snapshotAsList(CaptureBasePtr _cap) const;
+ TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const;
+ FeatureBasePtr firstFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr lastFeature (const SizeStd& _track_id) const;
+ FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap) const;
+ CaptureBasePtr firstCapture(const SizeStd& _track_id) const;
+
+ // tracks across captures that belong to keyframe
+// SizeStd numKeyframeTracks();
+ Track trackAtKeyframes(size_t _track_id) const;
+// bool markKeyframe(CaptureBasePtr _capture);
+// bool unmarkKeyframe(CaptureBasePtr _capture);
private:
static SizeStd track_id_count_;
- // Along track: maps of Feature pointers indexed by time stamp.
- map<size_t, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
+ // tracks across all Captures
+ map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
+// // tracks across captures that belong to keyframe
+// 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 ) )
+ map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
};
} /* namespace wolf */
diff --git a/include/core/sensor/autoconf_sensor_factory.h b/include/core/sensor/autoconf_sensor_factory.h
deleted file mode 100644
index e52366e334187e9f318bb9a514c1e7fdde6b3b21..0000000000000000000000000000000000000000
--- a/include/core/sensor/autoconf_sensor_factory.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/**
- * \file sensor_factory.h
- *
- * Created on: Apr 25, 2016
- * \author: jsola
- */
-
-#ifndef AUTOCONF_SENSOR_FACTORY_H_
-#define AUTOCONF_SENSOR_FACTORY_H_
-
-namespace wolf
-{
-class SensorBase;
-struct IntrinsicsBase;
-}
-
-// wolf
-#include "core/common/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 "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 paramsServer&> AutoConfSensorFactory;
-
-template<>
-inline std::string AutoConfSensorFactory::getClass()
-{
- return "AutoConfSensorFactory";
-}
-
-#define WOLF_REGISTER_SENSOR_AUTO(SensorType, SensorName) \
- namespace{ const bool WOLF_UNUSED SensorName##AutConf##Registered = \
- AutoConfSensorFactory::get().registerCreator(SensorType, SensorName::createAutoConf); } \
-
-} /* namespace wolf */
-
-#endif /* SENSOR_FACTORY_H_ */
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb75e84cd1c27c6f3681d1a6a5065ba6d29ac9f4
--- /dev/null
+++ b/include/core/sensor/factory_sensor.h
@@ -0,0 +1,237 @@
+/**
+ * \file factory_sensor.h
+ *
+ * Created on: Apr 25, 2016
+ * \author: jsola
+ */
+
+#ifndef FACTORY_SENSOR_H_
+#define FACTORY_SENSOR_H_
+
+namespace wolf
+{
+class SensorBase;
+struct ParamsSensorBase;
+}
+
+// wolf
+#include "core/common/factory.h"
+#include "core/utils/params_server.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,
+ * - "SensorOdom2d" for SensorOdom2d
+ * - "SensorOdom3d" for SensorOdom3d
+ * - "SensorDiffDrive" for SensorDiffDrive
+ * - "SensorCamera" for SensorCamera // in plugin 'vision'
+ * - "SensorLaser2d" for SensorLaser2d // in plugin 'laser'
+ *
+ * among others.
+ *
+ * Find general Factory documentation in class Factory:
+ * - Access the factory
+ * - Register/unregister creators
+ * - Invoke object creation
+ *
+ * This documentation shows you how to use the FactorySensor specifically:
+ * - Write sensor creators.
+ * - Create sensors
+ *
+ * #### Write sensor creators
+ * Sensor creators have the following API:
+ *
+ * \code
+ * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor);
+ * \endcode
+ *
+ * They follow the general implementation shown below:
+ *
+ * \code
+ * static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor)
+ * {
+ * // check extrinsics vector --- example: 3D pose
+ * assert(_params_extrinsics.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
+ *
+ * // cast sensor parameters to good type --- example: ParamsSensorCamera
+ * auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_params_sensor);
+ *
+ * // Do create the Sensor object --- example: SensorCamera
+ * auto sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
+ *
+ * // Complete the sensor setup with a unique name identifying the sensor
+ * sen_ptr->setName(_unique_name);
+ *
+ * return sen_ptr;
+ * }
+ * \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
+ * auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", params_extrinsics, params_camera);
+ * \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
+ * - FactoryParamsSensor: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
+ * - FactoryProcessor: to create processors that will be bound to sensors.
+ * - Problem::installSensor() : to install sensors in WOLF Problem.
+ *
+ * #### Example 1: writing a SensorCamera creator
+ * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp:
+ *
+ * \code
+ * static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics)
+ * {
+ * // check extrinsics vector
+ * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
+ *
+ * // cast instrinsics to good type
+ * auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_intrinsics);
+ *
+ * // Do create the SensorCamera object, and complete its setup
+ * auto sen_ptr = std::make_shared<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 = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
+ * }
+ * main () { ... }
+ * \endcode
+ * or inside your main(), where a direct call is possible:
+ * \code
+ * main () {
+ * FactorySensor::registerCreator("SensorCamera", 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 = FactorySensor::registerCreator("SensorCamera", 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 "core/sensor/factory_sensor.h"
+ * #include "vision/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 = "SensorCamera",
+ * // a name = "Front-left camera",
+ * // an extrinsics vector, and
+ * // a pointer to the intrinsics struct:
+ *
+ * Eigen::VectorXd extrinsics_1(7); // give it some values...
+ *
+ * // Create a pointer to the struct of sensor parameters stored in a YAML file ( see FactoryParamsSensor )
+ * ParamsSensorCameraPtr intrinsics_1 =
+ * FactoryParamsSensor::create("ParamsSensorCamera",
+ * camera_1.yaml);
+ *
+ * SensorBasePtr camera_1_ptr =
+ * FactorySensor::create ( "SensorCamera" ,
+ * "Front-left camera" ,
+ * extrinsics_1 ,
+ * intrinsics_1 );
+ *
+ * // A second camera... with a different name!
+ *
+ * Eigen::VectorXd extrinsics_2(7);
+ *
+ * ParamsSensorCameraPtr intrinsics_2 =
+ * FactoryParamsSensor::create("ParamsSensorCamera",
+ * camera_2.yaml);
+ *
+ * SensorBasePtr camera_2_ptr =
+ * FactorySensor::create( "SensorCamera" ,
+ * "Front-right camera" ,
+ * extrinsics_2 ,
+ * intrinsics_2 );
+ *
+ * return 0;
+ * }
+ * \endcode
+ *
+ */
+typedef Factory<SensorBase,
+ const std::string&,
+ const Eigen::VectorXd&,
+ const ParamsSensorBasePtr> FactorySensor;
+
+template<>
+inline std::string FactorySensor::getClass() const
+{
+ return "FactorySensor";
+}
+
+// ParamsSensor factory
+struct ParamsSensorBase;
+typedef Factory<ParamsSensorBase,
+ const std::string&> FactoryParamsSensor;
+template<>
+inline std::string FactoryParamsSensor::getClass() const
+{
+ return "FactoryParamsSensor";
+}
+
+#define WOLF_REGISTER_SENSOR(SensorType) \
+ namespace{ const bool WOLF_UNUSED SensorType##Registered = \
+ FactorySensor::registerCreator(#SensorType, SensorType::create); } \
+
+
+typedef Factory<SensorBase,
+ const std::string&,
+ const ParamsServer&> AutoConfFactorySensor;
+
+template<>
+inline std::string AutoConfFactorySensor::getClass() const
+{
+ return "AutoConfFactorySensor";
+}
+
+#define WOLF_REGISTER_SENSOR_AUTO(SensorType) \
+ namespace{ const bool WOLF_UNUSED SensorType##AutoConfRegistered = \
+ AutoConfFactorySensor::registerCreator(#SensorType, SensorType::create); } \
+
+
+} /* namespace wolf */
+
+#endif /* SENSOR_FACTORY_H_ */
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 2e05d2fe57dfb3494cd3b33c2d697510ff493849..fc12b568c965e2ddf3bc9ed9364ded2eee2d992b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -12,42 +12,100 @@ class StateBlock;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/common/params_base.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
namespace wolf {
+
+/*
+ * Macro for defining Autoconf sensor creator.
+ *
+ * Place a call to this macro inside your class declaration (in the sensor_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived sensor class, SensorClass,
+ * must have a constructor available with the API:
+ *
+ * SensorClass(const VectorXd & _extrinsics, const ParamsSensorClassPtr _intrinsics);
+ *
+ * We recommend writing one of such constructors in your derived sensors.
+ */
+#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass, ExtrinsicsSize) \
+static \
+SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \
+{ \
+ Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \
+ \
+ assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
+ \
+ auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
+ \
+ auto sensor = std::make_shared<SensorClass>(extrinsics, params); \
+ \
+ sensor ->setName(_unique_name); \
+ \
+ return sensor; \
+} \
+ \
+static \
+SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\
+{ \
+ assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
+ \
+ auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \
+ \
+ auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \
+ \
+ sensor ->setName(_unique_name); \
+ \
+ return sensor; \
+} \
+
+
+
+
/** \brief base struct for intrinsic sensor parameters
*
* Derive from this struct to create structs of sensor intrinsic parameters.
*/
-struct IntrinsicsBase: public ParamsBase
+struct ParamsSensorBase: public ParamsBase
{
- virtual ~IntrinsicsBase() = default;
+ std::string prefix = "sensor/";
+ ~ParamsSensorBase() override = default;
using ParamsBase::ParamsBase;
+ std::string print() const override
+ {
+ return "";
+ }
};
-class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
+class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase>
{
+ friend Problem;
+ friend ProcessorBase;
+
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
+ std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
+
+ std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+
+ protected:
+ Eigen::VectorXd noise_std_; // std of sensor noise
+ Eigen::MatrixXd noise_cov_; // cov matrix of noise
- Eigen::VectorXs noise_std_; // std of sensor noise
- Eigen::MatrixXs noise_cov_; // cov matrix of noise
+ CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
- std::map<unsigned int,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by index in state_block_vec_)
+ void setProblem(ProblemPtr _problem) override final;
public:
/** \brief Constructor with noise size
@@ -58,7 +116,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \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)
+ * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
+ * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
+ * \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
*
**/
SensorBase(const std::string& _type,
@@ -66,7 +126,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const unsigned int _noise_size,
- const bool _extr_dyn = false,
+ const bool _p_dyn = false,
+ const bool _o_dyn = false,
const bool _intr_dyn = false);
/** \brief Constructor with noise std vector
@@ -77,61 +138,70 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \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)
+ * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
+ * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
+ * \param _intr_dyn Flag indicating if intrinsics is 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 Eigen::VectorXd & _noise_std,
+ const bool _p_dyn = false,
+ const bool _o_dyn = false,
const bool _intr_dyn = false);
- virtual ~SensorBase();
+ ~SensorBase() override;
- unsigned int id();
+ unsigned int id() const;
- virtual void setProblem(ProblemPtr _problem) final;
- HardwareBasePtr getHardware();
- void setHardware(const HardwareBasePtr _hw_ptr);
+ HardwareBasePtr getHardware() const;
+ private:
+ void setHardware(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
- ProcessorBasePtrList& getProcessorList();
+ void removeProcessor(ProcessorBasePtr _proc_ptr);
+
+ public:
+ const ProcessorBasePtrList& getProcessorList() const;
- CaptureBasePtr lastKeyCapture(void);
- CaptureBasePtr lastCapture(const TimeStamp& _ts);
+ CaptureBasePtr getLastCapture() const;
+ void setLastCapture(CaptureBasePtr);
+ void updateLastCapture();
+ CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts) const;
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);
+ using HasStateBlocks::addStateBlock;
+ StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
+ StateBlockPtr getStateBlockDynamic(const char& _key) const;
+ StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
+
+ // Declare a state block as dynamic or static (with _dymanic = false)
+ void setStateBlockDynamic(const char& _key, bool _dynamic = true);
+
+ bool isStateBlockDynamic(const char& _key) const;
+ bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const;
+ bool isStateBlockInCapture(const char& _key) const;
+
+ StateBlockPtr getP(const TimeStamp _ts) const;
+ StateBlockPtr getO(const TimeStamp _ts) const;
+ StateBlockPtr getIntrinsic(const TimeStamp _ts) const;
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
+ StateBlockPtr getIntrinsic() const;
+
+ protected:
void removeStateBlocks();
+ virtual void registerNewStateBlocks() const;
+
+ public:
- void fix();
- void unfix();
void fixExtrinsics();
void unfixExtrinsics();
void fixIntrinsics();
@@ -147,49 +217,48 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \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,
+ void addPriorParameter(const char& _key,
+ const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
- void addPriorP(const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov,
+ void addPriorP(const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _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,
+ void addPriorO(const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov);
+ void addPriorIntrinsics(const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
- SizeEigen getCalibSize() const;
- Eigen::VectorXs getCalibration() const;
-
- virtual void registerNewStateBlocks();
+ void setNoiseStd(const Eigen::VectorXd & _noise_std);
+ void setNoiseCov(const Eigen::MatrixXd & _noise_std);
+ Eigen::VectorXd getNoiseStd() const;
+ Eigen::MatrixXd getNoiseCov() const;
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
- 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);
void link(HardwareBasePtr);
template<typename classType, typename... T>
- static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
-
- protected:
- SizeEigen computeCalibSize() const;
-
- private:
- void updateCalibSize();
+ static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
};
}
@@ -202,132 +271,81 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
namespace wolf{
template<typename classType, typename... T>
-std::shared_ptr<SensorBase> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all)
+std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all)
{
- SensorBasePtr sen = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> sen = std::make_shared<classType>(std::forward<T>(all)...);
sen->link(_hwd_ptr);
return sen;
}
-inline unsigned int SensorBase::id()
+inline unsigned int SensorBase::id() const
{
return sensor_id_;
}
-inline ProcessorBasePtrList& SensorBase::getProcessorList()
+inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
{
return processor_list_;
}
-inline const std::vector<StateBlockPtr>& SensorBase::getStateBlockVec() const
+inline CaptureBasePtr SensorBase::getLastCapture() const
{
- return state_block_vec_;
+ return last_capture_;
}
-inline std::vector<StateBlockPtr>& SensorBase::getStateBlockVec()
+inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
- return state_block_vec_;
+ assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
+ HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
+ setStateBlockDynamic(_key, _dynamic);
+ return _sb_ptr;
}
-inline StateBlockPtr SensorBase::getStateBlockPtrStatic(unsigned int _i) const
+inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic)
{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
+ assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway.");
+ state_block_dynamic_[_key] = _dynamic;
}
-inline void SensorBase::setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline bool SensorBase::isStateBlockDynamic(const char& _key) const
{
- 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;
+ assert(state_block_dynamic_.count(_key) != 0);
+ return state_block_dynamic_.at(_key);
}
-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()
+inline Eigen::VectorXd SensorBase::getNoiseStd() const
{
return noise_std_;
}
-inline Eigen::MatrixXs SensorBase::getNoiseCov()
+inline Eigen::MatrixXd SensorBase::getNoiseCov() const
{
return noise_cov_;
}
-inline HardwareBasePtr SensorBase::getHardware()
+inline HardwareBasePtr SensorBase::getHardware() const
{
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()
+inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
- calib_size_ = computeCalibSize();
+ addPriorParameter('P', _x, _cov, _start_idx, _size);
}
-inline void SensorBase::setExtrinsicDynamic(bool _extrinsic_dynamic)
+inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov)
{
- extrinsic_dynamic_ = _extrinsic_dynamic;
+ addPriorParameter('O', _x, _cov);
}
-inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic)
+inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
- intrinsic_dynamic_ = _intrinsic_dynamic;
+ addPriorParameter('I', _x, _cov);
}
} // namespace wolf
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index c1cbd7c382c58244e0e6a12f31b27d472b3507de..212a955b19c1a76aecebaa90f7cc39be73f38a7a 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -1,115 +1,82 @@
/**
* \file sensor_diff_drive.h
*
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
+ * Created on: Jul 22, 2019
+ * \author: jsola
*/
-#ifndef WOLF_SENSOR_DIFF_DRIVE_H_
-#define WOLF_SENSOR_DIFF_DRIVE_H_
+#ifndef SENSOR_SENSOR_DIFF_DRIVE_H_
+#define SENSOR_SENSOR_DIFF_DRIVE_H_
-//wolf includes
#include "core/sensor/sensor_base.h"
-#include "core/processor/diff_drive_tools.h"
-#include "core/utils/params_server.hpp"
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive);
-WOLF_PTR_TYPEDEFS(SensorDiffDrive);
-
-struct IntrinsicsDiffDrive : public IntrinsicsBase
+namespace wolf
{
- 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;
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDiffDrive);
- IntrinsicsDiffDrive()
+struct ParamsSensorDiffDrive : public ParamsSensorBase
+{
+ double radius_left;
+ double radius_right;
+ double wheel_separation;
+ double ticks_per_wheel_revolution;
+ bool set_intrinsics_prior;
+ Eigen::Vector3d prior_cov_diag;
+ double ticks_cov_factor;
+
+ ParamsSensorDiffDrive() = default;
+ ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server)
+ : ParamsSensorBase(_unique_name, _server)
{
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ radius_left = _server.getParam<double>(prefix + _unique_name + "/radius_left");
+ radius_right = _server.getParam<double>(prefix + _unique_name + "/radius_right");
+ wheel_separation = _server.getParam<double>(prefix + _unique_name + "/wheel_separation");
+ ticks_per_wheel_revolution = _server.getParam<double>(prefix + _unique_name + "/ticks_per_wheel_revolution");
+ set_intrinsics_prior = _server.getParam<bool>(prefix + _unique_name + "/set_intrinsics_prior");
+ prior_cov_diag = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_cov_diag");
+ ticks_cov_factor = _server.getParam<double>(prefix + _unique_name + "/ticks_cov_factor");
}
- IntrinsicsDiffDrive(std::string _unique_name, const paramsServer& _server):
- IntrinsicsBase(_unique_name, _server)
+ std::string print() const override
{
-
- left_radius_ = _server.getParam<Scalar>(_unique_name + "/left_radius_");
- right_radius_ = _server.getParam<Scalar>(_unique_name + "/right_radius_");
- separation_ = _server.getParam<Scalar>(_unique_name + "/separation_");
-
- auto model_str = _server.getParam<std::string>(_unique_name + "/model");
- if(model_str.compare("Two_Factor_Model")) model_ = DiffDriveModel::Two_Factor_Model;
- else if(model_str.compare("Three_Factor_Model")) model_ = DiffDriveModel::Three_Factor_Model;
- else if(model_str.compare("Five_Factor_Model")) model_ = DiffDriveModel::Five_Factor_Model;
- else throw std::runtime_error("Failed to fetch a valid value for the enumerate DiffDriveModel. Value provided: " + model_str);
-
- factors_ = _server.getParam<Eigen::VectorXs>(_unique_name + "/factors", "[1,1,1]");
-
- left_resolution_ = _server.getParam<Scalar>(_unique_name + "/left_resolution_");
- right_resolution_ = _server.getParam<Scalar>(_unique_name + "/right_resolution_");
-
- left_gain_ = _server.getParam<Scalar>(_unique_name + "/left_gain", "0.01");
- right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain", "0.01");
+ return ParamsSensorBase::print() + "\n"
+ + "radius_left: " + std::to_string(radius_left) + "\n"
+ + "radius_right: " + std::to_string(radius_right) + "\n"
+ + "wheel_separation: " + std::to_string(wheel_separation) + "\n"
+ + "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n"
+ + "set_intrinsics_prior: " + std::to_string(set_intrinsics_prior) + "\n"
+ + "prior_cov_diag: to_string not implemented yet! \n"
+ + "ticks_cov_factor: " + std::to_string(ticks_cov_factor) + "\n";
}
- virtual ~IntrinsicsDiffDrive() = default;
};
-typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;
+WOLF_PTR_TYPEDEFS(SensorDiffDrive);
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_;}
+ public:
+ SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
+ ParamsSensorDiffDrivePtr _intrinsics);
+ WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive, 3);
+ ~SensorDiffDrive() override;
+ ParamsSensorDiffDriveConstPtr getParams() const;
+
+ double getRadiansPerTick() const
+ {
+ return radians_per_tick;
+ }
+
+ protected:
+ ParamsSensorDiffDrivePtr params_diff_drive_;
+ double radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM.
-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
+inline wolf::ParamsSensorDiffDriveConstPtr SensorDiffDrive::getParams() const
+{
+ return params_diff_drive_;
+}
+
+} /* namespace wolf */
-#endif /* WOLF_SENSOR_DIFF_DRIVE_H_ */
+#endif /* SENSOR_SENSOR_DIFF_DRIVE_H_ */
diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h
deleted file mode 100644
index abe8b64180cdd5f0c2e95cd9eb6ccfe636e83e3a..0000000000000000000000000000000000000000
--- a/include/core/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 "core/common/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 "core/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/core/sensor/sensor_model.h b/include/core/sensor/sensor_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e2a37df70243f7d331dadde464fffd1d7f081ef
--- /dev/null
+++ b/include/core/sensor/sensor_model.h
@@ -0,0 +1,38 @@
+#ifndef SRC_SENSOR_MODEL_H_
+#define SRC_SENSOR_MODEL_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(SensorModel);
+
+class SensorModel : public SensorBase
+{
+ public:
+ SensorModel();
+ ~SensorModel() override;
+
+ static SensorBasePtr create(const std::string& _unique_name,
+ const ParamsServer& _server)
+ {
+ auto sensor = std::make_shared<SensorModel>();
+ sensor ->setName(_unique_name);
+ return sensor;
+ }
+
+ static SensorBasePtr create(const std::string& _unique_name,
+ const Eigen::VectorXd& _extrinsics,
+ const ParamsSensorBasePtr _intrinsics)
+ {
+ auto sensor = std::make_shared<SensorModel>();
+ sensor ->setName(_unique_name);
+ return sensor;
+ }
+};
+
+
+} /* namespace wolf */
+
+#endif /* SRC_SENSOR_POSE_H_ */
diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h
deleted file mode 100644
index 02218e8698790d207e61594befd403bca916862a..0000000000000000000000000000000000000000
--- a/include/core/sensor/sensor_odom_2D.h
+++ /dev/null
@@ -1,67 +0,0 @@
-#ifndef SENSOR_ODOM_2D_H_
-#define SENSOR_ODOM_2D_H_
-
-//wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
-
-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
- IntrinsicsOdom2D()
- {
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
- }
- IntrinsicsOdom2D(std::string _unique_name, const paramsServer& _server):
- IntrinsicsBase(_unique_name, _server)
- {
- k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
- k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
- }
-};
-
-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);
- static SensorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server);
-
-};
-
-} // namespace wolf
-
-#endif // SENSOR_ODOM_2D_H_
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..0859f15bcee22d3be1c1857365d03d50f0315962
--- /dev/null
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -0,0 +1,88 @@
+#ifndef SENSOR_ODOM_2d_H_
+#define SENSOR_ODOM_2d_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d);
+
+struct ParamsSensorOdom2d : public ParamsSensorBase
+{
+ ~ParamsSensorOdom2d() override = default;
+
+ double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
+ double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
+ ParamsSensorOdom2d()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
+ k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
+ }
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n"
+ + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(SensorOdom2d);
+
+class SensorOdom2d : public SensorBase
+{
+ protected:
+ double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
+ double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
+
+ public:
+ SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics);
+ SensorOdom2d(Eigen::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics);
+ WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d, 3);
+
+ ~SensorOdom2d() override;
+
+ /** \brief Returns displacement noise factor
+ *
+ * Returns displacement noise factor
+ *
+ **/
+ double getDispVarToDispNoiseFactor() const;
+
+ /** \brief Returns rotation noise factor
+ *
+ * Returns rotation noise factor
+ *
+ **/
+ double getRotVarToRotNoiseFactor() const;
+
+ /**
+ * Compute covariance of odometry given the motion data.
+ *
+ * NOTE: This is a helper function for the user, not called automatically anywhere.
+ *
+ * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+ * - a linear dependence with total displacement
+ * - a linear dependence with total rotation
+ *
+ * The formula for the variances is as follows:
+ *
+ * - disp_var = k_disp_to_disp * displacement
+ * - rot_var = k_rot_to_rot * rotation
+ *
+ * See implementation for details.
+ */
+ Matrix2d computeCovFromMotion (const VectorXd& _data) const;
+
+
+};
+
+} // namespace wolf
+
+#endif // SENSOR_ODOM_2d_H_
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h
deleted file mode 100644
index dffaf433ebd1ff29a258aec53e5478874b51f7c7..0000000000000000000000000000000000000000
--- a/include/core/sensor/sensor_odom_3D.h
+++ /dev/null
@@ -1,97 +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 "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
-
-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;
- IntrinsicsOdom3D()
- {
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
- }
- IntrinsicsOdom3D(std::string _unique_name, const paramsServer& _server):
- IntrinsicsBase(_unique_name, _server)
- {
- k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
- k_disp_to_rot = _server.getParam<Scalar>(_unique_name + "/k_disp_to_rot");
- k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
- min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
- min_rot_var = _server.getParam<Scalar>(_unique_name + "/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/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..d5f5b417eb36a0628a3425822d03b3fef494fb36
--- /dev/null
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -0,0 +1,124 @@
+/**
+ * \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 "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom3d);
+
+struct ParamsSensorOdom3d : public ParamsSensorBase
+{
+ double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
+ double k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation
+ double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
+ double min_disp_var;
+ double min_rot_var;
+ ParamsSensorOdom3d()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ParamsSensorOdom3d(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
+ k_disp_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_rot");
+ k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
+ min_disp_var = _server.getParam<double>(prefix + _unique_name + "/min_disp_var");
+ min_rot_var = _server.getParam<double>(prefix + _unique_name + "/min_rot_var");
+ }
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n"
+ + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
+ + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
+ + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
+ }
+ ~ParamsSensorOdom3d() override = default;
+};
+
+WOLF_PTR_TYPEDEFS(SensorOdom3d);
+
+class SensorOdom3d : public SensorBase
+{
+ protected:
+ double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
+ double k_disp_to_rot_; ///< ratio of displacement variance to rotation, for odometry noise calculation
+ double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
+ double min_disp_var_;
+ double min_rot_var_;
+
+ public:
+ SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& params);
+ SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr params);
+ WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom3d, 7);
+
+ ~SensorOdom3d() override;
+
+ double getDispVarToDispNoiseFactor() const;
+ double getDispVarToRotNoiseFactor() const;
+ double getRotVarToRotNoiseFactor() const;
+ double getMinDispVar() const;
+ double getMinRotVar() const;
+
+ /**
+ * Compute covariance of odometry given the motion data.
+ *
+ * NOTE: This is a helper function for the user, not called automatically anywhere.
+ *
+ * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+ * - a minimal value for displacement
+ * - a minimal value for rotation
+ * - a linear dependence with total displacement
+ * - a linear dependence with total rotation
+ *
+ * The formula for the variances is as follows:
+ *
+ * - disp_var = disp_var_min + k_disp_to_disp * displacement
+ * - rot_var = rot_var_min + k_disp_to_rot * displacement + k_rot_to_rot * rotation
+ *
+ * See implementation for details.
+ */
+ Matrix6d computeCovFromMotion (const VectorXd& _data) const;
+
+};
+
+inline double SensorOdom3d::getDispVarToDispNoiseFactor() const
+{
+ return k_disp_to_disp_;
+}
+
+inline double SensorOdom3d::getDispVarToRotNoiseFactor() const
+{
+ return k_disp_to_rot_;
+}
+
+inline double SensorOdom3d::getRotVarToRotNoiseFactor() const
+{
+ return k_rot_to_rot_;
+}
+
+inline double SensorOdom3d::getMinDispVar() const
+{
+ return min_disp_var_;
+}
+
+inline double SensorOdom3d::getMinRotVar() const
+{
+ return min_rot_var_;
+}
+
+} /* namespace wolf */
+
+#endif /* SRC_SENSOR_ODOM_3d_H_ */
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..4df6215468e6aabba07caee1b8d28af225d6fccc
--- /dev/null
+++ b/include/core/sensor/sensor_pose.h
@@ -0,0 +1,79 @@
+/**
+ * \file sensor_pose.h
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+#ifndef SRC_SENSOR_POSE_H_
+#define SRC_SENSOR_POSE_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
+
+struct ParamsSensorPose : public ParamsSensorBase
+{
+ double std_p = 1; // m
+ double std_o = 1; // rad
+ ParamsSensorPose()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ std_p = _server.getParam<double>(prefix + _unique_name + "/std_p");
+ std_o = _server.getParam<double>(prefix + _unique_name + "/std_o");
+ }
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n"
+ + "std_p: " + std::to_string(std_p) + "\n"
+ + "std_o: " + std::to_string(std_o) + "\n";
+ }
+ ~ParamsSensorPose() override = default;
+};
+
+WOLF_PTR_TYPEDEFS(SensorPose);
+
+class SensorPose : public SensorBase
+{
+ protected:
+ double std_p_; // standard deviation of translation measurements
+ double std_o_; // standard deviation of orientation measurements
+
+ public:
+ SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
+ SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr params);
+ WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose, 7);
+
+ ~SensorPose() override;
+
+ double getStdP() const;
+ double getStdO() const;
+
+};
+
+inline double SensorPose::getStdP() const
+{
+ return std_p_;
+}
+
+inline double SensorPose::getStdO() const
+{
+ return std_o_;
+}
+
+// inline Matrix6d SensorPose::computeDataCov() const
+// {
+// return (Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
+// }
+
+} /* namespace wolf */
+
+#endif /* SRC_SENSOR_POSE_H_ */
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e13e598fbd2726532333f2a71d4eabc5adcc1fb
--- /dev/null
+++ b/include/core/solver/factory_solver.h
@@ -0,0 +1,44 @@
+/**
+ * \file factory_solver.h
+ *
+ * Created on: Dec 17, 2018
+ * \author: jcasals
+ */
+
+#ifndef FACTORY_SOLVER_H_
+#define FACTORY_SOLVER_H_
+
+namespace wolf
+{
+class SensorBase;
+struct ParamsSensorBase;
+}
+
+// wolf
+#include "core/common/factory.h"
+#include "core/solver/solver_manager.h"
+
+namespace wolf
+{
+
+/** \brief Solver factory class
+ *
+ */
+
+typedef Factory<SolverManager,
+ const ProblemPtr&,
+ const ParamsServer&> FactorySolver;
+
+template<>
+inline std::string FactorySolver::getClass() const
+{
+ return "FactorySolver";
+}
+
+#define WOLF_REGISTER_SOLVER(SolverType) \
+ namespace{ const bool WOLF_UNUSED SolverType##Registered = \
+ wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
+
+} /* namespace wolf */
+
+#endif /* SENSOR_FACTORY_H_ */
diff --git a/include/core/solver/solver_factory.h b/include/core/solver/solver_factory.h
deleted file mode 100644
index 36a551b668a1f4f1c595d98ea61b1534f54c670e..0000000000000000000000000000000000000000
--- a/include/core/solver/solver_factory.h
+++ /dev/null
@@ -1,228 +0,0 @@
-/**
- * \file solver_factory.h
- *
- * Created on: Dec 17, 2018
- * \author: jcasals
- */
-
-#ifndef SOLVER_FACTORY_H_
-#define SOLVER_FACTORY_H_
-
-namespace wolf
-{
-class SensorBase;
-struct IntrinsicsBase;
-}
-
-// wolf
-#include "core/common/factory.h"
-#include "core/solver/solver_manager.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 "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<SolverManager,
- const ProblemPtr&,
- const paramsServer&> SolverFactory;
-
-template<>
-inline std::string SolverFactory::getClass()
-{
- return "SolverFactory";
-}
-
-#define WOLF_REGISTER_SOLVER(SolverName) \
- namespace{ const bool WOLF_UNUSED SolverName##Registered = \
- wolf::SolverFactory::get().registerCreator("Solver", SolverName::create); } \
-
-} /* namespace wolf */
-
-#endif /* SENSOR_FACTORY_H_ */
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 049cb951d07c5e1c0b70cdf9be9a1d9dea992261..f3fbeefb3dce65e58ad82d3c604ba3e554169624 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -5,96 +5,225 @@
#include "core/common/wolf.h"
#include "core/state_block/state_block.h"
#include "core/factor/factor_base.h"
+#include "core/common/params_base.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(SolverManager)
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver)
+
+/*
+ * Macro for defining Autoconf solver creator for WOLF's high level API.
+ *
+ * Place a call to this macro inside your class declaration (in the solver_manager_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived solver manager class, SolverManagerClass,
+ * must have a constructor available with the API:
+ *
+ * SolverManagerClass(const ProblemPtr& wolf_problem,
+ * const ParamsSolverClassPtr _params);
+ */
+#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \
+static SolverManagerPtr create(const ProblemPtr& _problem, \
+ const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsSolverClass>(#SolverClass, _server); \
+ return std::make_shared<SolverClass>(_problem, params); \
+} \
+static SolverManagerPtr create(const ProblemPtr& _problem, \
+ const ParamsSolverPtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsSolverClass>(_params); \
+ return std::make_shared<SolverClass>(_problem, params); \
+} \
+
+ struct ParamsSolver;
/**
* \brief Solver manager for WOLF
*/
class SolverManager
{
-public:
-
- /** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
- enum class CovarianceBlocksToBeComputed : std::size_t
- {
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
- };
-
- /**
- * \brief Enumeration for the verbosity of the solver report.
- */
- enum class ReportVerbosity : std::size_t
- {
- QUIET = 0,
- BRIEF,
- FULL
- };
-
-protected:
-
- ProblemPtr wolf_problem_;
-
-public:
-
- SolverManager(const ProblemPtr& wolf_problem);
-
- virtual ~SolverManager() = default;
-
- std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET);
-
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
+ public:
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
+ /** \brief Enumeration of covariance blocks to be computed
+ *
+ * Enumeration of covariance blocks to be computed
+ *
+ */
+ enum class CovarianceBlocksToBeComputed : std::size_t
+ {
+ ALL = 0, ///< All blocks and all cross-covariances
+ ALL_MARGINALS = 1, ///< All marginals
+ ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+ GAUSS = 3, ///< GAUSS: last frame P and V
+ GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T
+ GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T
+ };
- virtual bool hasConverged() = 0;
+ /**
+ * \brief Enumeration for the verbosity of the solver report.
+ */
+ enum class ReportVerbosity : std::size_t
+ {
+ QUIET = 0,
+ BRIEF,
+ FULL
+ };
- virtual SizeStd iterations() = 0;
+ // PROFILING
+ unsigned int n_solve_, n_cov_;
+ std::chrono::microseconds acc_duration_total_;
+ std::chrono::microseconds acc_duration_solver_;
+ std::chrono::microseconds acc_duration_update_;
+ std::chrono::microseconds acc_duration_state_;
+ std::chrono::microseconds acc_duration_cov_;
- virtual Scalar initialCost() = 0;
+ std::chrono::microseconds max_duration_total_;
+ std::chrono::microseconds max_duration_solver_;
+ std::chrono::microseconds max_duration_update_;
+ std::chrono::microseconds max_duration_state_;
+ std::chrono::microseconds max_duration_cov_;
- virtual Scalar finalCost() = 0;
+ void printProfiling(std::ostream& stream = std::cout) const;
- virtual void update();
+ protected:
- ProblemPtr getProblem();
+ ProblemPtr wolf_problem_;
+ ParamsSolverPtr params_;
+
+ public:
+ /**
+ * \brief Constructor with default params_
+ */
+ SolverManager(const ProblemPtr& _problem);
+ /**
+ * \brief Constructor with given params_
+ */
+ SolverManager(const ProblemPtr& _problem,
+ const ParamsSolverPtr& _params);
+
+ virtual ~SolverManager();
+
+ /**
+ * \brief Solves with the verbosity defined in params_
+ */
+ std::string solve();
+ /**
+ * \brief Solves with a given verbosity
+ */
+ std::string solve(const ReportVerbosity report_level);
+
+ virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
+
+ virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
+
+ virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
+
+ virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
+
+ virtual bool hasConverged() = 0;
+
+ virtual SizeStd iterations() = 0;
+
+ virtual double initialCost() = 0;
+
+ virtual double finalCost() = 0;
+
+ virtual double totalTime() = 0;
+
+ /**
+ * \brief Updates solver's problem according to the wolf_problem
+ */
+ virtual void update();
+
+ ProblemPtr getProblem();
+
+ double getPeriod() const;
+
+ ReportVerbosity getVerbosity() const;
+
+ virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
+
+ virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
+
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+ virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+
+ virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) final;
+
+ virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
+
+ virtual int numFactors() const final;
+ virtual int numStateBlocks() const final;
+ virtual int numStateBlocksFloating() const final;
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
+ virtual int numFactorsDerived() const = 0;
+ virtual int numStateBlocksDerived() const = 0;
+
+ virtual bool check(std::string prefix="") const final;
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr);
+ protected:
-protected:
+ std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
+ std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
+ std::set<FactorBasePtr> factors_;
+ std::set<StateBlockPtr> floating_state_blocks_;
- std::map<StateBlockPtr, Eigen::VectorXs> state_blocks_;
+ virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+ const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
+ double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
- virtual Eigen::VectorXs& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
- virtual Scalar* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+ private:
+ // SolverManager functions
+ virtual void addFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
- 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;
-
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
+ protected:
+ // Derived virtual functions
+ virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
+ virtual bool checkDerived(std::string prefix="") const = 0;
+ virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+ virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) = 0;
+ virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
+};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) = 0;
+// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
+struct ParamsSolver: public ParamsBase
+{
+ std::string prefix = "solver/";
+ double period = 0.0;
+ SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
+
+ ParamsSolver() = default;
+ ParamsSolver(std::string _unique_name, const ParamsServer& _server):
+ ParamsBase(_unique_name, _server)
+ {
+ period = _server.getParam<double>(prefix + "period");
+ verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose");
+ }
+ std::string print() const override
+ {
+ return "period: " + std::to_string(period) + "\n";
+ }
+
+ ~ParamsSolver() override = default;
};
} // namespace wolf
diff --git a/include/core/solver_suitesparse/cost_function_base.h b/include/core/solver_suitesparse/cost_function_base.h
index bf8f4ebcbee58431e0bb741eded9d3c3c799fdbb..523e2147ace0242d5b4739480a59912686fd07cd 100644
--- a/include/core/solver_suitesparse/cost_function_base.h
+++ b/include/core/solver_suitesparse/cost_function_base.h
@@ -15,18 +15,18 @@ 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_;
+ Eigen::MatrixXd J_0_;
+ Eigen::MatrixXd J_1_;
+ Eigen::MatrixXd J_2_;
+ Eigen::MatrixXd J_3_;
+ Eigen::MatrixXd J_4_;
+ Eigen::MatrixXd J_5_;
+ Eigen::MatrixXd J_6_;
+ Eigen::MatrixXd J_7_;
+ Eigen::MatrixXd J_8_;
+ Eigen::MatrixXd J_9_;
+ Eigen::VectorXd residual_;
+ std::vector<Eigen::MatrixXd*> jacobians_;
std::vector<unsigned int> block_sizes_;
public:
@@ -75,18 +75,18 @@ class CostFunctionBase
virtual void evaluateResidualJacobians() = 0;
- void getResidual(Eigen::VectorXs &residual)
+ void getResidual(Eigen::VectorXd &residual)
{
residual.resize(residual_.size());
residual = residual_;
}
- std::vector<Eigen::MatrixXs*> getJacobians()
+ std::vector<Eigen::MatrixXd*> getJacobians()
{
return jacobians_;
}
- void getJacobians(std::vector<Eigen::MatrixXs>& jacobians)
+ void getJacobians(std::vector<Eigen::MatrixXd>& jacobians)
{
jacobians.resize(n_blocks_);
for (unsigned int i = 0; i<n_blocks_; i++)
diff --git a/include/core/solver_suitesparse/cost_function_sparse_base.h b/include/core/solver_suitesparse/cost_function_sparse_base.h
index 0a1d9b3a885352fb951be84eda72f4e639085c0a..78bfd843d8d89ed05b21b7ef9cd568db2ef705fa 100644
--- a/include/core/solver_suitesparse/cost_function_sparse_base.h
+++ b/include/core/solver_suitesparse/cost_function_sparse_base.h
@@ -32,7 +32,7 @@ template <class FactorT,
unsigned int BLOCK_9_SIZE = 0>
class CostFunctionSparseBase : CostFunctionBase
{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE +
+ typedef ceres::Jet<double, BLOCK_0_SIZE +
BLOCK_1_SIZE +
BLOCK_2_SIZE +
BLOCK_3_SIZE +
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index 452971da53b36126d1b9c5b9492d99357dcd6ff0..b89d49c8cac06092aaaada411598c20514fab574 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -9,14 +9,14 @@
#define TRUNK_SRC_SOLVER_QR_SOLVER_H_
//std includes
+#include <core/factor/factor_odom_2d.h>
#include <iostream>
#include <ctime>
//Wolf includes
#include "core/state_block/state_block.h"
#include "../factor_sparse.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/factor/factor_corner_2D.h"
+#include "core/factor/factor_corner_2d.h"
#include "core/factor/factor_container.h"
#include "core/solver_suitesparse/sparse_utils.h"
@@ -28,7 +28,7 @@
#include <eigen3/Eigen/OrderingMethods>
#include <eigen3/Eigen/SparseQR>
#include <Eigen/StdVector>
-#include "core/factor/factor_pose_2D.h"
+#include "core/factor/factor_pose_2d.h"
namespace wolf
{
@@ -162,8 +162,8 @@ class SolverQR
unsigned int meas_dim = _factor_ptr->getSize();
- std::vector<Eigen::MatrixXs> jacobians(_factor_ptr->getStateBlockPtrVector().size());
- Eigen::VectorXs error(meas_dim);
+ std::vector<Eigen::MatrixXd> jacobians(_factor_ptr->getStateBlockPtrVector().size());
+ Eigen::VectorXd error(meas_dim);
cost_functions_.back()->evaluateResidualJacobians();
cost_functions_.back()->getResidual(error);
@@ -423,7 +423,7 @@ class SolverQR
// 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());
+ Eigen::Map < Eigen::VectorXd > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
}
// Zero the error
@@ -535,30 +535,30 @@ class SolverQR
switch (_fac_ptr->getTypeId())
{
- case FAC_GPS_FIX_2D:
+ case FAC_GPS_FIX_2d:
{
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_fac_ptr);
- return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2D, specific_ptr->residualSize,
+ 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:
+ case FAC_ODOM_2d:
{
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2D, specific_ptr->residualSize,
+ 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:
+ case FAC_CORNER_2d:
{
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2D, specific_ptr->residualSize,
+ 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,
diff --git a/include/core/solver_suitesparse/solver_manager.h b/include/core/solver_suitesparse/solver_manager.h
index bb3863e11d20719704c7acae1bbc88bebf597eb2..449c8eee3a7b1d7ce54782300017604c51d1843c 100644
--- a/include/core/solver_suitesparse/solver_manager.h
+++ b/include/core/solver_suitesparse/solver_manager.h
@@ -2,16 +2,16 @@
#define CERES_MANAGER_H_
//wolf includes
-#include "core/factor/factor_GPS_2D.h"
+#include "core/factor/factor_GPS_2d.h"
#include "core/common/wolf.h"
#include "core/state_block/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"
+#include "../factor_odom_2d_theta.h"
+#include "../factor_odom_2d_complex_angle.h"
+#include "../factor_corner_2d_theta.h"
/** \brief solver manager for WOLF
*
diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h
index 280aa2310c648ca360a491de6be6784826404f8a..7f5e0a0d0d2a56c9c798509c8b092eba0a35d855 100644
--- a/include/core/solver_suitesparse/sparse_utils.h
+++ b/include/core/solver_suitesparse/sparse_utils.h
@@ -14,17 +14,17 @@
namespace wolf
{
-void eraseBlockRow(Eigen::SparseMatrixs& A, const unsigned int& _row, const unsigned int& _n_rows)
+void eraseBlockRow(Eigen::SparseMatrixd& A, const unsigned int& _row, const unsigned int& _n_rows)
{
- A.prune([](int i, int, Scalar) { return i >= _row && i < _row + _n_rows; });
+ A.prune([](int i, int, double) { return i >= _row && i < _row + _n_rows; });
}
-void eraseBlockCol(Eigen::SparseMatrixs& A, const unsigned int& _col, const unsigned int& _n_cols)
+void eraseBlockCol(Eigen::SparseMatrixd& A, const unsigned int& _col, const unsigned int& _n_cols)
{
- A.prune([](int, int j, Scalar) { return j >= _col && j < _col + _n_cols; });
+ A.prune([](int, int j, double) { return j >= _col && j < _col + _n_cols; });
}
-void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& 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++)
@@ -33,7 +33,7 @@ void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original,
original.makeCompressed();
}
-void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& 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++)
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e1028bf954a7c2ac71e4d133a097700a6150283
--- /dev/null
+++ b/include/core/state_block/factory_state_block.h
@@ -0,0 +1,145 @@
+/*
+ * \file factory_state_block.h
+ *
+ * Created on: Apr 27, 2020
+ * \author: jsola
+ */
+
+#ifndef STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+#define STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+
+#include "core/common/factory.h"
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+
+/** \brief StateBlock factory class
+ *
+ * This factory can create objects of class StateBlock and classes deriving from StateBlock.
+ *
+ * Specific object creation is invoked by create(TYPE, state, fixed),
+ * and the TYPE of state block is identified with a string.
+ * For example, the following processor types are implemented,
+ * - "StateBlock" for StateBlock
+ * - "StateQuaternion" for StateQuaternion
+ * - "StateAngle" for StateAngle
+ * - "StateHomogeneous3d" for StateHomogeneous3d
+ *
+ * The factory also creates state blocks according to the block key used in to identify state blocks in each Wolf node.
+ * These keys are single-letter strings. The following letters are implemented
+ * - "O" for 2d orientation, creates StateAngle
+ * - "O" for 3d orientation, creates StateQuaternion
+ * - "H" crestes StateHomogeneous3d
+ *
+ * Any other letter creates the base StateBlock.
+ *
+ * Find general Factory documentation in class Factory:
+ * - Access the factory
+ * - Register/unregister creators
+ * - Invoke object creation
+ *
+ * This documentation shows you how to use the FactoryStateBlock specifically:
+ * - Write a state block creator
+ * - Create state blocks
+ *
+ * #### Write state block creators
+ * StateBlock creators have the following API:
+ *
+ * \code
+ * static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
+ * \endcode
+ *
+ * They follow the general implementation shown below:
+ *
+ * \code
+ * static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed)
+ * {
+ * return std::make_shared<StateBlockDerived>(_state, _fixed);
+ * }
+ * \endcode
+ *
+ * #### Creating processors
+ * Note: Prior to invoking the creation of a processor of a derived type,
+ * you must register the creator for this type into the factory.
+ *
+ * Note: State blocks of the base type do not need to be registered.
+ *
+ * To create a StateQuaternion, you type:
+ *
+ * \code
+ * auto sq_ptr = FactoryStateBlock::create("StateQuaternion", Vector4d(1,0,0,0), false);
+ * \endcode
+ *
+ * If your problem has dimension 3 (e.g. is 3D), you can use the key "O" to create a StateQuaternion,
+ *
+ * \code
+ * auto sq_ptr = FactoryStateBlock::create("O", Vector4d(1,0,0,0), false);
+ * \endcode
+ *
+ * However if your problem has dimension 2 (e.g. is 2D), the key "O" will create a StateAngle,
+ *
+ * \code
+ * auto sa_ptr = FactoryStateBlock::create("O", Vector1d(angle_in_radians), false);
+ * \endcode
+ *
+ * Note: It is an error to provide state vectors of the wrong size (4 for 3D orientation, 1 for 2D).
+ *
+ * To create StateBlocks to store 2D position and velocity, you type:
+ *
+ * \code
+ * auto sp2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false);
+ * auto sv2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false);
+ * \endcode
+ *
+ * To create StateBlocks to store 2D position and velocity, you can also use the key letters:
+ *
+ * \code
+ * auto sp2_ptr = FactoryStateBlock::create("P", Vector2d(1,2), false);
+ * auto sv2_ptr = FactoryStateBlock::create("V", Vector2d(1,2), false);
+ * \endcode
+ *
+ * To create StateBlocks to store 3D position and velocity, you type:
+ *
+ * \code
+ * auto sp3_ptr = FactoryStateBlock::create("P", Vector3d(1,2,3), false);
+ * auto sv3_ptr = FactoryStateBlock::create("V", Vector3d(1,2,3), false);
+ * \endcode
+ *
+ * Note: for base state blocks, the size is determined by the size of the provided vector parameter `VectorXd& _state`.
+ */
+typedef Factory<StateBlock, const Eigen::VectorXd&, bool> FactoryStateBlock;
+template<>
+inline std::string FactoryStateBlock::getClass() const
+{
+ return "FactoryStateBlock";
+}
+
+template<>
+inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen::VectorXd& _state, bool _fixed)
+{
+ typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
+
+ if (creator_callback_it == get().callbacks_.end())
+ // not found: return StateBlock base
+ return std::make_shared<StateBlock>(_state, _fixed);
+
+ // Invoke the creation function
+ return (creator_callback_it->second)(_state, _fixed);
+}
+
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
+ namespace{ const bool WOLF_UNUSED StateBlockType##Registered = \
+ FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); } \
+
+#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
+ namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
+ FactoryStateBlock::registerCreator(#Key, StateBlockType::create); } \
+
+
+
+
+}
+
+
+#endif /* STATE_BLOCK_FACTORY_STATE_BLOCK_H_ */
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
new file mode 100644
index 0000000000000000000000000000000000000000..418e0909fdfd7e32f42347dee4644cb6eba73c97
--- /dev/null
+++ b/include/core/state_block/has_state_blocks.h
@@ -0,0 +1,401 @@
+/**
+ * \file has_state_blocks.h
+ *
+ * Created on: Aug 27, 2019
+ * \author: jsola
+ */
+
+#ifndef STATE_BLOCK_HAS_STATE_BLOCKS_H_
+#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
+
+#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
+
+#include <map>
+
+namespace wolf
+{
+
+
+class HasStateBlocks
+{
+ public:
+ HasStateBlocks();
+ HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
+ virtual ~HasStateBlocks();
+
+ const StateStructure& getStructure() const { return structure_; }
+ void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
+ bool isInStructure(const char& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
+
+ const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() const;
+ std::vector<StateBlockPtr> getStateBlockVec() const;
+
+ // Some typical shortcuts -- not all should be coded here, see notes below.
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
+
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+ virtual void fix();
+ virtual void unfix();
+ virtual bool isFixed() const;
+
+ virtual StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
+ virtual unsigned int removeStateBlock(const char& _sb_type);
+ bool hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool hasStateBlock(const StateBlockPtr& _sb) const;
+ StateBlockPtr getStateBlock(const char& _sb_type) const;
+ bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
+ bool stateBlockKey(const StateBlockPtr& _sb, char& _key) const;
+ std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
+
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template<typename SB, typename ... Args>
+ std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
+
+ // Emplace base state blocks.
+ template<typename ... Args>
+ StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
+
+ // Register/remove state blocks to/from wolf::Problem
+ void registerNewStateBlocks(ProblemPtr _problem) const;
+ void removeStateBlocks(ProblemPtr _problem);
+
+ // States
+ VectorComposite getState(const StateStructure& structure="") const;
+ void setState(const VectorComposite& _state, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateStructure& _structure="", const bool _notify = true);
+ void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
+
+ VectorXd getStateVector(const StateStructure& structure="") const;
+ unsigned int getSize(const StateStructure& _structure="") const;
+ unsigned int getLocalSize(const StateStructure& _structure="") const;
+
+ // Perturb state
+ void perturb(double amplitude = 0.01);
+
+ protected:
+ void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+
+
+ private:
+ StateStructure structure_;
+ std::unordered_map<char, StateBlockPtr> state_block_map_;
+
+};
+
+} // namespace wolf
+
+
+//////////// IMPLEMENTATION /////////////
+
+#include "core/state_block/state_block.h"
+
+namespace wolf{
+
+inline HasStateBlocks::HasStateBlocks() :
+ structure_(std::string("")),
+ state_block_map_()
+{
+ //
+}
+
+inline HasStateBlocks::~HasStateBlocks()
+{
+ //
+}
+
+inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+{
+ return state_block_map_;
+}
+
+inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
+{
+ std::vector<StateBlockPtr> sbv;
+ for (auto& key : structure_)
+ {
+ sbv.push_back(getStateBlock(key));
+ }
+ return sbv;
+}
+
+//inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
+//{
+// return removeStateBlock(std::string(1, _sb_type));
+//}
+
+inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
+{
+ return state_block_map_.erase(_sb_type);
+}
+
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
+
+ addStateBlock(_sb_type, sb, _problem);
+
+ return sb;
+}
+
+template<typename ... Args>
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+
+ addStateBlock(_sb_type, sb, _problem);
+
+ return sb;
+}
+
+inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
+{
+ assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
+ assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
+ state_block_map_.at(_sb_type) = _sb;
+ return true; // success
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+{
+ auto it = state_block_map_.find(_sb_type);
+ if (it != state_block_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getP() const
+{
+ return getStateBlock('P');
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getO() const
+{
+ return getStateBlock('O');
+}
+
+inline void HasStateBlocks::fix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->fix();
+}
+
+inline void HasStateBlocks::unfix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->unfix();
+}
+
+inline bool HasStateBlocks::isFixed() const
+{
+ bool fixed = true;
+ for (auto pair_key_sbp : state_block_map_)
+ {
+ if (pair_key_sbp.second)
+ fixed &= pair_key_sbp.second->isFixed();
+ }
+ return fixed;
+}
+
+inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _notify)
+{
+ for (const auto& pair_key_vec : _state)
+ {
+ const auto& key = pair_key_vec.first;
+ const auto& vec = pair_key_vec.second;
+ const auto& sb = getStateBlock(key);
+ if (!sb)
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ assert (sb && "Stateblock key not in the structure");
+
+ sb->setState(vec, _notify);
+ }
+}
+
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify)
+{
+ StateStructure structure;
+ if (_structure == "")
+ structure = structure_;
+ else
+ structure = _structure;
+
+ int size = getSize(structure);
+ assert(_state.size() == size && "In FrameBase::setState wrong state size");
+
+ unsigned int index = 0;
+ for (const char key : structure)
+ {
+ const auto& sb = getStateBlock(key);
+ assert (sb && "Stateblock key not in the structure");
+
+ sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
+ index += sb->getSize();
+ }
+
+}
+
+inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
+ const StateStructure& _structure,
+ const std::list<SizeEigen>& _sizes,
+ const bool _notify)
+{
+ SizeEigen index = 0;
+ auto size_it = _sizes.begin();
+ for (const auto& key : _structure)
+ {
+ const auto& sb = getStateBlock(key);
+ assert (sb && "Stateblock key not in the structure");
+ assert(*size_it == sb->getSize() && "State block size mismatch");
+
+ sb->setState(_state.segment(index, *size_it), _notify);
+ index += *size_it;
+ size_it ++;
+ }
+}
+
+
+inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
+{
+ auto vec_it = _vectors.begin();
+ for (const auto key : _structure)
+ {
+ const auto& sb = getStateBlock(key);
+ assert (sb && "Stateblock key not in the structure");
+ assert(vec_it->size() == sb->getSize() && "State block size mismatch");
+
+ sb->setState(*vec_it, _notify);
+ vec_it ++;
+ }
+}
+
+
+//// _structure can be either stateblock structure of the node or a subset of this structure
+inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) const
+{
+ StateStructure structure;
+ if (_structure == "")
+ structure = structure_;
+ else
+ structure = _structure;
+
+ VectorXd state(getSize(structure));
+
+ unsigned int index = 0;
+ for (const char key : structure)
+ {
+ const auto& sb = getStateBlock(key);
+
+ assert(sb != nullptr && "Requested StateBlock key not in the structure");
+
+ state.segment(index,sb->getSize()) = sb->getState();
+ index += sb->getSize();
+ }
+ return state;
+}
+
+inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
+{
+ const StateStructure& structure = (_structure == "" ? structure_ : _structure);
+
+ VectorComposite state;
+
+ for (const auto key : structure)
+ {
+ auto state_it = state_block_map_.find(key);
+
+ if (state_it != state_block_map_.end())
+
+ state.emplace(key, state_it->second->getState());
+ }
+
+ return state;
+}
+
+inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) const
+{
+ const StateStructure& structure = (_structure == "" ? structure_ : _structure);
+
+ unsigned int size = 0;
+ for (const char key : structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getSize();
+ }
+
+ return size;
+}
+
+inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+{
+ const auto& it = std::find_if(state_block_map_.begin(),
+ state_block_map_.end(),
+ [_sb](const std::pair<char, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
+{
+ const auto& it = this->find(_sb);
+
+ return it != state_block_map_.end();
+}
+
+inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, char& _key) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key = '0'; // '0' = not found
+ return false;
+ }
+}
+
+inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _structure) const
+{
+ StateStructure structure;
+ if (_structure == "")
+ structure = structure_;
+ else
+ structure = _structure;
+
+ unsigned int size = 0;
+ for (const char key : structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getLocalSize();
+ }
+
+ return size;
+}
+
+} // namespace wolf
+
+#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index 5b146d84d3c46de5f4b6699fdb0efb83edb64c9d..460835cb9bf03c92cfb73f8516248816212c6571 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.h
@@ -18,16 +18,16 @@ 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);
+ ~LocalParametrizationAngle() override;
+ bool plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
+ bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
+ bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
inline LocalParametrizationAngle::LocalParametrizationAngle() :
@@ -41,29 +41,36 @@ 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
+inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _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
+inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixRowXd>& _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)
+inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1)
{
_x2_minus_x1(0) = pi2pi(_x2(0)-_x1(0));
return true;
}
+inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance)
+{
+ //Any real is a valid angle because we use the pi2pi function. Also
+ //the types don't match. In this case the argument is
+ //Eigen::Map not Eigen::VectorXd
+ return true;
+}
} /* namespace wolf */
#endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 94ed61f62d83736c27593afe151ab701e51e09e5..8494ec3a89215af8287584c8255b241af3486b45 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -12,8 +12,6 @@
namespace wolf {
-//WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
-
class LocalParametrizationBase{
protected:
unsigned int global_size_;
@@ -22,14 +20,18 @@ class LocalParametrizationBase{
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;
-
+ bool plus(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
+ Eigen::VectorXd& _x_plus_delta) const;
+ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
+ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
+ virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
};
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index 4729bb3ff1f8e4ff821b4cc87f5c7c192b2ccaf0..b8bbb981c6cec93c9e4fbfa29049a12f16d137a9 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -38,15 +38,16 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase
{
public:
LocalParametrizationHomogeneous();
- virtual ~LocalParametrizationHomogeneous();
+ ~LocalParametrizationHomogeneous() override;
- 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);
+ bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
+ bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
+ bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
+ Eigen::Map<const Eigen::VectorXd>& _h2,
+ Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 3076568d529835f12f414e0194fe2a0396e65305..c637ab6bb07a23b0e5dd5528d6a1c240327e1473 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -59,20 +59,26 @@ public:
//
}
- virtual ~LocalParametrizationQuaternion()
+ ~LocalParametrizationQuaternion() override
{
//
}
- 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;
+ bool plus(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const override;
- 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);
+ bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
+ bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
+ Eigen::Map<const Eigen::VectorXd>& _q2,
+ Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
+ // inline bool isValid(const Eigen::VectorXd& state) override;
+ // template<QuaternionDeltaReference DeltaReference>
+ bool isValid(const Eigen::VectorXd& _state, double tolerance) override
+ {
+ return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
+ }
};
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 635fd6be75e01d72793b11b4987aee75a220c8e7..1a5fda068c7befa1c563bcbfe05f938e0d4c576b 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -17,15 +17,17 @@ namespace wolf
class StateAngle : public StateBlock
{
public:
- StateAngle(Scalar _angle = 0.0, bool _fixed = false);
+ StateAngle(double _angle = 0.0, bool _fixed = false);
- virtual ~StateAngle();
+ ~StateAngle() override;
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateAngle::StateAngle(Scalar _angle, bool _fixed) :
+inline StateAngle::StateAngle(double _angle, bool _fixed) :
StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>())
{
- state_(0) = _angle;
+ state_(0) = pi2pi(_angle);
}
inline StateAngle::~StateAngle()
@@ -33,6 +35,12 @@ inline StateAngle::~StateAngle()
//
}
+inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<1,1>::check(_state);
+ return std::make_shared<StateAngle>(_state(0), _fixed);
+}
+
} /* namespace wolf */
#endif /* STATE_ANGLE_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 01fa0bc31927ccabdc2e15788e2b8966bb2740e5..5406fe0c105c67d87e4793ba86a2e52f81bbfa87 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -35,7 +35,7 @@ public:
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
+ Eigen::VectorXd 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
@@ -59,7 +59,7 @@ public:
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
**/
- StateBlock(const Eigen::VectorXs& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+ StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
///< Explicitly not copyable/movable
StateBlock(const StateBlock& o) = delete;
@@ -72,11 +72,15 @@ public:
/** \brief Returns the state vector
**/
- Eigen::VectorXs getState() const;
+ Eigen::VectorXd getState() const;
+
+ /** \brief Returns the state vector data array
+ **/
+ double* getStateData();
/** \brief Sets the state vector
**/
- void setState(const Eigen::VectorXs& _state, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const bool _notify = true);
/** \brief Returns the state size
**/
@@ -142,13 +146,23 @@ public:
**/
void resetLocalParamUpdated();
- /** \brief Add this state_block to the problem
- **/
- //void addToProblem(ProblemPtr _problem_ptr);
+ virtual void setIdentity(bool _notify = true);
+ void setZero (bool _notify = true);
+
+ virtual Eigen::VectorXd identity() const;
+ Eigen::VectorXd zero() const;
+
+ /** \brief perturb state
+ */
+ void perturb(double amplitude = 0.1);
+
+
+ void plus(const Eigen::VectorXd& _dv);
+
+ bool isValid(double tolerance = Constants::EPS);
+
+ static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
- /** \brief Remove this state_block from the problem
- **/
- //void removeFromProblem(ProblemPtr _problem_ptr);
};
} // namespace wolf
@@ -160,7 +174,7 @@ public:
namespace wolf {
-inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
+inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
// notifications_{Notification::ADD},
fixed_(_fixed),
state_size_(_state.size()),
@@ -177,7 +191,7 @@ inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametri
// notifications_{Notification::ADD},
fixed_(_fixed),
state_size_(_size),
- state_(Eigen::VectorXs::Zero(_size)),
+ state_(Eigen::VectorXd::Zero(_size)),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
@@ -192,7 +206,7 @@ inline StateBlock::~StateBlock()
// std::cout << "destructed -sb" << std::endl;
}
-inline Eigen::VectorXs StateBlock::getState() const
+inline Eigen::VectorXd StateBlock::getState() const
{
std::lock_guard<std::mutex> lock(mut_state_);
return state_;
@@ -279,6 +293,38 @@ inline void StateBlock::resetLocalParamUpdated()
local_param_updated_.store(false);
}
+inline double* StateBlock::getStateData()
+{
+ return state_.data();
+}
+
+inline void StateBlock::setIdentity(bool _notify)
+{
+ setState( Eigen::VectorXd::Zero(state_size_), _notify );
+}
+
+inline void StateBlock::setZero(bool _notify)
+{
+ setIdentity(_notify);
+}
+
+inline Eigen::VectorXd StateBlock::identity() const
+{
+ return Eigen::VectorXd::Zero(state_size_);
+}
+inline Eigen::VectorXd StateBlock::zero() const
+{
+ return identity();
+}
+
+inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ return std::make_shared<StateBlock>(_state, _fixed);
+}
+inline bool StateBlock::isValid(double tolerance)
+{
+ return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
+}
}// namespace wolf
#endif
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c675c7b747b6e15cf541c16c366d2bde7146388
--- /dev/null
+++ b/include/core/state_block/state_composite.h
@@ -0,0 +1,364 @@
+/*
+ * state_composite.h
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+#ifndef STATE_BLOCK_STATE_COMPOSITE_H_
+#define STATE_BLOCK_STATE_COMPOSITE_H_
+
+#include "core/common/wolf.h"
+
+#include <unordered_map>
+
+#include <iostream>
+
+
+namespace wolf
+{
+
+using std::string;
+using namespace Eigen;
+
+typedef std::string StateStructure;
+typedef std::unordered_map < char, StateBlockPtr > StateBlockMap;
+typedef StateBlockMap::const_iterator StateBlockMapCIter;
+
+class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
+{
+ public:
+ VectorComposite() {};
+ VectorComposite(const StateStructure& _s);
+ VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+ VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
+
+ Eigen::VectorXd vector(const StateStructure& _structure) const;
+
+ /**
+ * \brief set from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * Eigen::VectorXd vec_eigen;
+ * wolf::VectorComposite vec_comp;
+ *
+ * vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+ void setZero();
+
+ bool includesStructure(const StateStructure &_structure) const;
+
+ friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
+ friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+ friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+ friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x);
+};
+
+
+class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >
+{
+
+ private:
+ std::unordered_map < char, unsigned int> size_rows_, size_cols_;
+ unsigned int rows() const;
+ unsigned int cols() const;
+ void sizeAndIndices(const StateStructure & _struct_rows,
+ const StateStructure & _struct_cols,
+ std::unordered_map < char, unsigned int>& _indices_rows,
+ std::unordered_map < char, unsigned int>& _indices_cols,
+ unsigned int& _rows,
+ unsigned int& _cols) const;
+
+ public:
+ MatrixComposite() {};
+ MatrixComposite(const StateStructure& _row_structure,
+ const StateStructure& _col_structure);
+ MatrixComposite(const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ MatrixComposite(const MatrixXd& _m,
+ const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+ ~MatrixComposite() = default;
+
+ bool check() const;
+ static MatrixComposite zero(const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+
+ static MatrixComposite identity(const StateStructure& _structure,
+ const std::list<int>& _sizes);
+
+ unsigned int count(const char &_row,
+ const char &_col) const;
+
+ bool emplace(const char &_row,
+ const char &_col,
+ const Eigen::MatrixXd &_mat_blk);
+
+ // throw error if queried block is not present
+ bool at(const char &_row,
+ const char &_col,
+ Eigen::MatrixXd &_mat_blk) const;
+ const MatrixXd& at(const char &_row,
+ const char &_col) const;
+ MatrixXd& at(const char &_row,
+ const char &_col);
+
+ // make some shadowed API available
+ using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::at;
+ using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count;
+
+
+ MatrixXd matrix(const StateStructure & _struct_rows,
+ const StateStructure & _struct_cols) const;
+
+ MatrixComposite operator * (double scalar_right) const;
+ MatrixComposite operator + (const MatrixComposite & _second) const;
+ MatrixComposite operator - (const MatrixComposite & _second) const;
+ MatrixComposite operator - (void) const;
+
+ /**
+ * \brief Matrix product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second matrix N has blocks "PO" in vertical arrangement,
+ * and "XY" in horizontal arrangement:
+ * N["P"]["X"] N["P"]["Y"]
+ * N["O"]["X"] N["O"]["Y"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N.
+ *
+ * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY"
+ * S["V"]["X"] S["V"]["Y"]
+ * S["W"]["X"] S["W"]["Y"]
+ */
+ MatrixComposite operator *(const MatrixComposite & _second) const;
+
+ /**
+ * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix
+ *
+ * This function computes the product J * Q * J.transpose.
+ *
+ * It considers that this is a Jacobian matrix, and that the provided matrix
+ * is a proper covariance (e.g. symmmetric and semi-positive).
+ * It also considers that the Jacobian blocks are compatible with the blocks
+ * of the provided covariance, both in their structure and their individual sizes.
+ *
+ * If these conditions are not satisfied, the product will fail and throw,
+ * or give aberrant results at best.
+ *
+ * -----
+ *
+ * For example: Let us call 'this' a Jacobian matrix with the name 'J'.
+ *
+ * This Jacobian 'J' maps the "PO" blocks into "VW":
+ *
+ * J["V"]["P"] J["V"]["O"]
+ * J["W"]["P"] J["W"]["O"]
+ *
+ * The provided matrix is a covariances matrix Q.
+ * Q has blocks "P", "O" in both vertical and horizontal arrangements:
+ *
+ * Q["P"]["P"] Q["P"]["O"]
+ * Q["O"]["P"] Q["O"]["O"]
+ *
+ * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J.
+ *
+ * Now, upon a call to
+ *
+ * MatrixComposite S = J.propagate(Q);
+ *
+ * the result is a covariances matrix S with blocks "V" and "W"
+ *
+ * S["V"]["V"] S["V"]["W"]
+ * S["W"]["V"] S["W"]["W"]
+ */
+ MatrixComposite propagate(const MatrixComposite & _Cov); // This performs Jac * this * Jac.tr
+
+ /**
+ * \brief Matrix-vector product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second vector V has blocks "PO" in vertical arrangement,
+ * V["P"]
+ * V["O"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V.
+ *
+ * Then, the result is a vector S = M * V with block arrangement "VW"
+ * S["V"]
+ * S["W"]
+ */
+ VectorComposite operator *(const VectorComposite & _second) const;
+
+ friend std::ostream& operator << (std::ostream & _os, const MatrixComposite & _M);
+
+ friend MatrixComposite operator * (double scalar_left, const MatrixComposite& M);
+};
+
+
+class StateBlockComposite
+{
+ public:
+ StateBlockComposite() = default;
+ ~StateBlockComposite() = default;
+
+ const StateBlockMap& getStateBlockMap() const;
+
+ StateBlockPtr add(const char& _sb_type, const StateBlockPtr& _sb);
+
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template<typename SB, typename ... Args>
+ std::shared_ptr<SB> emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+
+ // Emplace base state blocks.
+ template<typename ... Args>
+ inline StateBlockPtr emplace(const char& _sb_type, Args&&... _args_of_base_state_block_constructor);
+
+ unsigned int remove(const char& _sb_type);
+ bool has(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool has(const StateBlockPtr& _sb) const;
+ StateBlockPtr at(const char& _sb_type) const;
+ void set(const char& _sb_type, const StateBlockPtr& _sb);
+ void set(const char& _sb_type, const VectorXd& _vec);
+ void setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors);
+ bool key(const StateBlockPtr& _sb, char& _key) const;
+ char key(const StateBlockPtr& _sb) const;
+ StateBlockMapCIter find(const StateBlockPtr& _sb) const;
+
+ // identity and zero (they are the same with different names)
+ void setIdentity(bool _notify = true);
+ void setZero(bool _notify = true);
+ VectorComposite identity() const;
+ VectorComposite zero() const;
+
+ // Plus operator
+ void plus(const VectorComposite& _dx);
+
+ // Perturb state with random noise
+ void perturb(double amplitude = 0.01);
+
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+ void fix();
+ void unfix();
+ bool isFixed() const;
+
+ // Get composite of state vectors (not blocks)
+ VectorComposite getState() const;
+ bool getState(VectorComposite& _state) const;
+ void setState(const VectorComposite& _state);
+
+ // Get compact Eigen vector and related things
+ SizeEigen stateSize() const;
+ SizeEigen stateSize(const StateStructure& _structure) const;
+ VectorXd stateVector(const StateStructure& _structure) const;
+ void stateVector(const StateStructure& _structure, VectorXd& _vector) const;
+
+ private:
+ StateBlockMap state_block_map_;
+};
+
+//////////// IMPLEMENTATION ////////////
+
+inline unsigned int MatrixComposite::count(const char &_row, const char &_col) const
+{
+ const auto& rit = this->find(_row);
+
+ if (rit == this->end())
+ return 0;
+
+ else
+ return rit->second.count(_col);
+}
+
+
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char &_sb_type,
+ Args &&... _args_of_derived_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
+
+ add(_sb_type, sb);
+
+ return sb;
+}
+
+template<typename ... Args>
+inline StateBlockPtr wolf::StateBlockComposite::emplace(const char &_sb_type,
+ Args &&... _args_of_base_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+
+ add(_sb_type, sb);
+
+ return sb;
+}
+
+}
+
+
+#endif /* STATE_BLOCK_STATE_COMPOSITE_H_ */
diff --git a/include/core/state_block/state_homogeneous_3D.h b/include/core/state_block/state_homogeneous_3D.h
deleted file mode 100644
index c71e3c7f1be920e6e144af255893d9a3efc2aafe..0000000000000000000000000000000000000000
--- a/include/core/state_block/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 "core/state_block/state_block.h"
-#include "core/state_block/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/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..ccc45424e0304ab4705e6eab9625a5d5b67ab290
--- /dev/null
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -0,0 +1,67 @@
+/*
+ * \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 "core/state_block/state_block.h"
+#include "core/state_block/local_parametrization_homogeneous.h"
+
+namespace wolf {
+
+class StateHomogeneous3d : public StateBlock
+{
+ public:
+ StateHomogeneous3d(bool _fixed = false);
+ StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
+ ~StateHomogeneous3d() override;
+
+ void setIdentity(bool _notify = true) override;
+ Eigen::VectorXd identity() const override;
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
+};
+
+inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _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
+}
+
+
+inline void StateHomogeneous3d::setIdentity(bool _notify)
+{
+ setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
+inline Eigen::VectorXd StateHomogeneous3d::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
+
+inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<4,1>::check(_state);
+ return std::make_shared<StateHomogeneous3d>(_state, _fixed);
+}
+
+} // namespace wolf
+
+#endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 66b23c0504339b98b1c9be1eeaafd9d552ecf472..d52b18c83165ac2cfe98bcebd944db4c612a4887 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -17,17 +17,22 @@ 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();
+ StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
+ StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
+ ~StateQuaternion() override;
+
+ void setIdentity(bool _notify = true) override;
+ Eigen::VectorXd identity() const override;
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateQuaternion::StateQuaternion(const Eigen::Quaternions& _quaternion, bool _fixed) :
+inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
}
-inline StateQuaternion::StateQuaternion(const Eigen::VectorXs& _state, bool _fixed) :
+inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _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");
@@ -36,7 +41,7 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXs& _state, bool _fix
inline StateQuaternion::StateQuaternion(bool _fixed) :
StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
- state_ = Eigen::Quaternions::Identity().coeffs();
+ state_ = Eigen::Quaterniond::Identity().coeffs();
//
}
@@ -45,6 +50,23 @@ inline StateQuaternion::~StateQuaternion()
// The local_param_ptr_ pointer is already removed by the base class
}
+
+inline void StateQuaternion::setIdentity(bool _notify)
+{
+ setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
+inline Eigen::VectorXd StateQuaternion::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
+
+inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<4,1>::check(_state);
+ return std::make_shared<StateQuaternion>(_state, _fixed);
+}
+
} // namespace wolf
#endif /* SRC_STATE_QUATERNION_H_ */
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 0ee9c37d53052a3da7f193b8a4e6715592d5628b..431524af94e218b17f2ce223ca47483c0d3cd058 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -6,83 +6,130 @@
namespace wolf{
class Problem;
class FrameBase;
-class TimeStamp;
}
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/common/time_stamp.h"
+#include "core/state_block/state_composite.h"
//std includes
namespace wolf {
+class TrajectoryIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
+{
+ public:
+ TrajectoryIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src)
+ : std::map<TimeStamp, FrameBasePtr>::const_iterator(std::move(src))
+ {
+
+ }
+
+ // override the indirection operator
+ const FrameBasePtr& operator*() const {
+ return std::map<TimeStamp, FrameBasePtr>::const_iterator::operator*().second;
+ }
+};
+
+class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator
+{
+ public:
+ TrajectoryRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src)
+ : std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator(std::move(src))
+ {
+
+ }
+
+ // override the indirection operator
+ const FrameBasePtr& operator*() const {
+ return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
+ }
+};
+
//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
{
+ friend FrameBase;
+
private:
- std::list<FrameBasePtr> frame_list_;
+ FrameMap frame_map_;
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
- FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
+ StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
public:
- TrajectoryBase(const std::string& _frame_sturcture);
- virtual ~TrajectoryBase();
+ TrajectoryBase();
+ ~TrajectoryBase() override;
- // Properties
- std::string getFrameStructure() const;
-
// Frames
- FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
- FrameBasePtrList& getFrameList();
- const FrameBasePtrList& getFrameList() const;
+ const FrameMap& getFrameMap() const;
FrameBasePtr getLastFrame() const;
- FrameBasePtr getLastKeyFrame() const;
- FrameBasePtr getLastKeyOrAuxFrame() const;
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
- FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
- void sortFrame(FrameBasePtr _frm_ptr);
- void updateLastFrames();
+ FrameBasePtr getFirstFrame() const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ TrajectoryIter begin() const;
+ TrajectoryIter end() const;
+ TrajectoryRevIter rbegin() const;
+ TrajectoryRevIter rend() const;
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ private:
+ FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
+ void removeFrame(FrameBasePtr _frame_ptr);
+ public:
// factors
- void getFactorList(FactorBasePtrList & _fac_list);
-
- protected:
- FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
- void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
+ void getFactorList(FactorBasePtrList & _fac_list) const;
};
-inline FrameBasePtrList& TrajectoryBase::getFrameList()
+inline const FrameMap& TrajectoryBase::getFrameMap() const
{
- return frame_list_;
+ return frame_map_;
}
-inline const FrameBasePtrList& TrajectoryBase::getFrameList() const
+inline FrameBasePtr TrajectoryBase::getFirstFrame() const
{
- return frame_list_;
+ auto it = static_cast<TrajectoryIter>(frame_map_.begin());
+ return *it;
}
-
-inline FrameBasePtr TrajectoryBase::getLastFrame() const
+inline TrajectoryIter TrajectoryBase::begin() const
{
- return frame_list_.back();
+ return static_cast<TrajectoryIter>(frame_map_.begin());
}
-
-inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
+inline TrajectoryIter TrajectoryBase::end() const
{
- return last_key_frame_ptr_;
+ return static_cast<TrajectoryIter>(frame_map_.end());
}
-
-inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const
+inline TrajectoryRevIter TrajectoryBase::rbegin() const
{
- return last_key_or_aux_frame_ptr_;
+ return static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+}
+inline TrajectoryRevIter TrajectoryBase::rend() const
+{
+ return static_cast<TrajectoryRevIter>(frame_map_.rend());
}
-inline std::string TrajectoryBase::getFrameStructure() const
+inline FrameBasePtr TrajectoryBase::getLastFrame() const
{
- return frame_structure_;
+ // return last_key_frame_ptr_;
+ auto last = this->rbegin();
+ if(last != this->rend()) return *(this->rbegin());
+ else return nullptr;
}
} // namespace wolf
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9e91935fe52e08b2ea1bf0c264b5e67ca5ef4e1
--- /dev/null
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -0,0 +1,61 @@
+#ifndef FACTORY_TREE_MANAGER_H_
+#define FACTORY_TREE_MANAGER_H_
+
+namespace wolf
+{
+class TreeManagerBase;
+struct ParamsTreeManagerBase;
+}
+
+// wolf
+#include "core/common/factory.h"
+
+// std
+
+namespace wolf
+{
+/** \brief TreeManager factory class
+ * TODO
+ */
+
+// ParamsTreeManager factory
+struct ParamsTreeManagerBase;
+typedef Factory<ParamsTreeManagerBase,
+ const std::string&> FactoryParamsTreeManager;
+template<>
+inline std::string FactoryParamsTreeManager::getClass() const
+{
+ return "FactoryParamsTreeManager";
+}
+
+// TreeManager factory
+typedef Factory<TreeManagerBase,
+ const std::string&,
+ const ParamsTreeManagerBasePtr> FactoryTreeManager;
+template<>
+inline std::string FactoryTreeManager::getClass() const
+{
+ return "FactoryTreeManager";
+}
+
+#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
+ namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \
+ wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
+
+typedef Factory<TreeManagerBase,
+ const std::string&,
+ const ParamsServer&> AutoConfFactoryTreeManager;
+template<>
+inline std::string AutoConfFactoryTreeManager::getClass() const
+{
+ return "AutoConfFactoryTreeManager";
+}
+
+
+#define WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerType) \
+ namespace{ const bool WOLF_UNUSED TreeManagerType##AutoConfRegistered = \
+ wolf::AutoConfFactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
+
+} /* namespace wolf */
+
+#endif /* FACTORY_TREE_MANAGER_H_ */
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..9fd06f2f0d78e82ee1eda5800c10707b4c84b759
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -0,0 +1,82 @@
+#ifndef INCLUDE_TREE_MANAGER_BASE_H_
+#define INCLUDE_TREE_MANAGER_BASE_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/common/node_base.h"
+#include "core/common/params_base.h"
+#include "core/problem/problem.h"
+
+namespace wolf
+{
+/*
+ * Macro for defining Autoconf tree manager creator for WOLF's high level API.
+ *
+ * Place a call to this macro inside your class declaration (in the tree_manager_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived processor class, ProcessorClass,
+ * must have a constructor available with the API:
+ *
+ * TreeManagerClass(const ParamsTreeManagerPtr _params);
+ */
+#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
+static TreeManagerBasePtr create(const std::string& _unique_name, \
+ const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsTreeManagerClass>(_unique_name, _server); \
+ \
+ auto tree_manager = std::make_shared<TreeManagerClass>(params); \
+ \
+ tree_manager ->setName(_unique_name); \
+ \
+ return tree_manager; \
+} \
+static TreeManagerBasePtr create(const std::string& _unique_name, \
+ const ParamsTreeManagerBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params); \
+ \
+ auto tree_manager = std::make_shared<TreeManagerClass>(params); \
+ \
+ tree_manager ->setName(_unique_name); \
+ \
+ return tree_manager; \
+} \
+
+struct ParamsTreeManagerBase : public ParamsBase
+{
+ std::string prefix = "problem/tree_manager";
+ ParamsTreeManagerBase() = default;
+ ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server):
+ ParamsBase(_unique_name, _server)
+ {
+ }
+
+ ~ParamsTreeManagerBase() override = default;
+
+ std::string print() const override
+ {
+ return "";
+ }
+};
+
+class TreeManagerBase : public NodeBase
+{
+ public:
+ TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) :
+ NodeBase("TREE_MANAGER", _type),
+ params_(_params)
+ {}
+
+ ~TreeManagerBase() override{}
+
+ virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
+
+ protected:
+ ParamsTreeManagerBasePtr params_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_BASE_H_ */
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
new file mode 100644
index 0000000000000000000000000000000000000000..16e58428dc7fdce89b919e22109a6595dce5afd3
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -0,0 +1,56 @@
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
+
+#include "core/tree_manager/tree_manager_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindow)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow)
+
+struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
+{
+ ParamsTreeManagerSlidingWindow() = default;
+ ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsTreeManagerBase(_unique_name, _server)
+ {
+ n_frames = _server.getParam<unsigned int>(prefix + "/n_frames");
+ n_fix_first_frames = _server.getParam<unsigned int>(prefix + "/n_fix_first_frames");
+ viral_remove_empty_parent = _server.getParam<bool> (prefix + "/viral_remove_empty_parent");
+ if (n_frames <= n_fix_first_frames)
+ throw std::runtime_error("TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than 'n_frames'!");
+ }
+ std::string print() const override
+ {
+ return ParamsTreeManagerBase::print() + "\n"
+ + "n_frames: " + std::to_string(n_frames) + "\n"
+ + "n_fix_first_frames: " + std::to_string(n_fix_first_frames) + "\n"
+ + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
+ }
+
+ unsigned int n_frames;
+ unsigned int n_fix_first_frames;
+ bool viral_remove_empty_parent;
+};
+
+class TreeManagerSlidingWindow : public TreeManagerBase
+{
+ public:
+ TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params) :
+ TreeManagerBase("TreeManagerSlidingWindow", _params),
+ params_sw_(_params)
+ {};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow)
+
+ ~TreeManagerSlidingWindow() override{}
+
+ void keyFrameCallback(FrameBasePtr _frame) override;
+
+ protected:
+ ParamsTreeManagerSlidingWindowPtr params_sw_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ */
diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
new file mode 100644
index 0000000000000000000000000000000000000000..f0e20f4b5eec08e6f322424f7353ca124b448e90
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
@@ -0,0 +1,51 @@
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
+
+struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
+{
+ ParamsTreeManagerSlidingWindowDualRate() = default;
+ ParamsTreeManagerSlidingWindowDualRate(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsTreeManagerSlidingWindow(_unique_name, _server)
+ {
+ n_frames_recent = _server.getParam<unsigned int>(prefix + "/n_frames_recent");
+ assert(n_frames_recent <= n_frames);
+ rate_old_frames = _server.getParam<unsigned int>(prefix + "/rate_old_frames");
+ }
+ std::string print() const override
+ {
+ return ParamsTreeManagerBase::print() + "\n"
+ + "n_frames_recent: " + std::to_string(n_frames_recent) + "\n"
+ + "rate_old_frames: " + std::to_string(rate_old_frames) + "\n";
+ }
+
+ unsigned int n_frames_recent, rate_old_frames;
+};
+
+class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
+{
+ public:
+ TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
+ ;
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
+
+ ~TreeManagerSlidingWindowDualRate() override{}
+
+ void keyFrameCallback(FrameBasePtr _frame) override;
+
+ protected:
+ ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
+ unsigned int count_frames_;
+ //TrajectoryIter first_recent_frame_it_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ */
diff --git a/include/core/utils/check_log.h b/include/core/utils/check_log.h
new file mode 100644
index 0000000000000000000000000000000000000000..035bd0aa0e2e9fa4989c95509800d1fee517bd46
--- /dev/null
+++ b/include/core/utils/check_log.h
@@ -0,0 +1,22 @@
+#ifndef CHECK_LOG_H
+#define CHECK_LOG_H
+#include <iostream>
+#include <string>
+#include <sstream>
+
+namespace wolf
+{
+class CheckLog
+{
+ public:
+ bool is_consistent_;
+ std::string explanation_;
+
+ CheckLog();
+ CheckLog(bool _consistent, std::string _explanation);
+ ~CheckLog(){};
+ void compose(CheckLog l);
+ void assertTrue(bool _condition, std::stringstream& _stream);
+};
+} // namespace wolf
+#endif
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 1824bbf870095023d17789374d4e244bf9e2434a..7a266d50b2e26eb387ff03ac9ba1f94ab1c39437 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -1,88 +1,82 @@
#ifndef CONVERTER_H
#define CONVERTER_H
-#include <vector>
+
+// wolf
+#include "core/common/wolf.h"
+#include "core/utils/converter_utils.h"
+#include "core/state_block/state_composite.h"
+
// Eigen
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
+
+// std
#include <regex>
#include <iostream>
+#include <array>
+#include <vector>
+#include <stack>
+#include <list>
+#include <math.h>
-namespace utils{
- template <typename A>
- using list = std::vector<A>;
- // template <typename A>
- // class toString<A>{};
- static inline std::vector<std::string> splitter(std::string val){
- std::vector<std::string> cont = std::vector<std::string>();
- std::stringstream ss(val);
- std::string token;
- while (std::getline(ss, token, ',')) {
- cont.push_back(token);
- }
- return cont;
- }
- static inline std::vector<std::string> getMatches(std::string val, std::regex exp){
- std::smatch res;
- auto v = std::vector<std::string>();
- std::string::const_iterator searchStart( val.cbegin() );
- while ( std::regex_search( searchStart, val.cend(), res, exp ) ) {
- v.push_back(res[0]);
- searchStart = res.suffix().first;
- }
- return v;
- }
- static inline std::vector<std::string> pairSplitter(std::string val){
- std::regex exp("(\\{[^\\{:]:.*?\\})");
- return getMatches(val, exp);
- }
- static inline std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix){
- std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]");
- std::regex rgxStatic("\\[((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]");
- std::smatch matches;
- std::smatch matchesStatic;
- std::array<std::string,2> values = {{"[]","[]"}};
- if(std::regex_match(matrix, matches, rgx)) {
- values[0] = "[" + matches[1].str() + "]";
- values[1] = "[" + matches[2].str() + "]";
- }else if(std::regex_match(matrix, matchesStatic, rgxStatic)){
- values[1] = "[" + matchesStatic[1].str() + "]";
- }else{
- throw std::runtime_error("Invalid string representation of a Matrix. Correct format is [([num,num],)?(num(,num)*)?]. String provided: " + matrix);
- }
- return values;
- }
- static inline std::string splitMapStringRepresentation(std::string str_map){
- std::smatch mmatches;
- std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
- // auto v = std::vector<std::string>();
- std::string result = "";
- if(std::regex_match(str_map, mmatches, rgxM)) {
- // v = splitter(mmatches[1].str());
- result = mmatches[1].str();
- } else {
- throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + str_map);
- }
- return result;
- }
-}
+/**
+ @file
+ converter.h
+ @brief This file implements a set of simple functions that deal with the correspondence between
+ classes and their string representation. The YAML autosetup framework makes heavy use of this file.
+ */
+
+//Note: In order to deal with string representations we make heavy use of regexps.
+// We use the standard C++11 regular expressions capabilities.
+
+/*
+** This file is structured essentially in two parts:
+** in the first part (which starts after the CONVERTERS ~~~~ STRING ----> TYPE line)
+** we have functions to convert from string to C++ class. For example:
+ string v1 = "[3,4,5,6,7,8,9,10,11]";
+ vector<int> v = converter<vector<int>>::convert(v1);
+ This code snippet transforms the string v1 which represents an std::vector into an actual std::vector value.
+ The second part (which starts after the TYPES ----> ToSTRING line) has the functions to
+ transform from C++ classes to strings. For instance, if we wanted to convert from a C++ class
+ to a string we would do something like this:
+ vector<int> vect{ 10, 20, 30 };
+ string str = converter<std::string>::convert(vect);
+ //str == "[10,20,30]"
+ */
namespace wolf{
+ //This definition is a bit of a "hack". The reason of redefining the pair class is to be able
+ //to have two string representations of a pair, namely
+ //"(·,·)" -> typical pair/tuple representation
+ //"{·,·}" -> key-value pair representation used to represent maps.
+ //This is purely an aesthetic reason and could be removed without problems.
+template<class A, class B>
+struct Wpair : std::pair<A,B>
+{
+ Wpair(A first, B second): std::pair<A,B>(first, second)
+ {
+
+ }
+};
+ //// CONVERTERS ~~~~ STRING ----> TYPE
template<typename T>
struct converter{
static T convert(std::string val){
- assert(1 == 0 && "There is no general convert for arbitrary T !!!");
+ throw std::runtime_error("There is no general convert for arbitrary T !!! String provided: " + val);
}
};
template<typename A>
struct converter<utils::list<A>>{
- static utils::list<A> convert(std::string val){
+ static utils::list<A> convert(std::string val){
std::regex rgxP("\\[([^,]+)?(,[^,]+)*\\]");
utils::list<A> result = utils::list<A>();
if(std::regex_match(val, rgxP)) {
- std::string aux = val.substr(1,val.size()-2);
- auto l = utils::getMatches(aux, std::regex("([^,]+),?"));
+ // std::string aux = val.substr(1,val.size()-2);
+ // auto l = utils::getMatches(aux, std::regex("([^,]+)"));
+ auto l = utils::parseList(val);
for(auto it : l){
- result.push_back(converter<A>::convert(it));
+ // WOLF_DEBUG("Asking to convert in list ", it);
+ result.push_back(converter<A>::convert(it));
}
} else throw std::runtime_error("Invalid string format representing a list-like structure. Correct format is [(value)?(,value)*]. String provided: " + val);
return result;
@@ -91,11 +85,11 @@ struct converter<utils::list<A>>{
std::string aux = "";
bool first = true;
for(auto it : val){
- if(not first) aux += "," + it;
- else{
- first = false;
- aux = it;
- }
+ if(not first) aux += "," + converter<A>::convert(it);
+ else{
+ first = false;
+ aux = converter<A>::convert(it);
+ }
}
return "[" + aux + "]";
}
@@ -103,13 +97,21 @@ struct converter<utils::list<A>>{
template<>
struct converter<int>{
static int convert(std::string val){
- return stod(val);
+ double res;
+ if (modf(stod(val),&res) > 0)
+ throw std::runtime_error("Invalid conversion to int: The number contains decimals: " + val);
+ return res;
}
};
template<>
struct converter<unsigned int>{
static unsigned int convert(std::string val){
- return stod(val);
+ double res;
+ if (modf(stod(val),&res) > 0)
+ throw std::runtime_error("Invalid conversion to unsigned int: The number contains decimals: " + val);
+ if (res < 0)
+ throw std::runtime_error("Invalid conversion to unsigned int: The number is negative: " + val);
+ return res;
}
};
template<>
@@ -121,20 +123,30 @@ struct converter<double>{
template<>
struct converter<bool>{
static bool convert(std::string val){
- if(val == "true") return true;
- else if (val == "false") return false;
+ if(val == "true" or val=="True" or val=="TRUE") return true;
+ else if (val == "false" or val=="False" or val=="FALSE") return false;
else throw std::runtime_error("Invalid conversion to bool (Must be either \"true\" or \"false\"). String provided: " + val);
}
};
template<>
+struct converter<char>{
+ static char convert(std::string val){
+ //Here we should check that val.length() == 1 and get val[0] into a char variable
+ if(val.length() == 1) return val.at(0);
+ else throw std::runtime_error("Invalid converstion to char. String too long. String provided: " + val);
+ throw std::runtime_error("Invalid char conversion. String provided: " + val);
+ }
+};
+ //// TYPES ----> ToSTRING
+template<>
struct converter<std::string>{
static std::string convert(std::string val){
return val;
}
template<typename T>
static std::string convert(T val){
- assert(1 == 0 && "There is no general convert to string for arbitrary T !!!");
- return "";
+ // throw std::runtime_error("There is no general convert to string for arbitrary T !!! String provided: " + val);
+ throw std::runtime_error("There is no general convert to string for arbitrary T !!!");
}
static std::string convert(int val){
return std::to_string(val);
@@ -142,6 +154,9 @@ struct converter<std::string>{
static std::string convert(double val){
return std::to_string(val);
}
+ static std::string convert(char val){
+ return std::to_string(val);
+ }
template<typename A>
static std::string convert(utils::list<A> val){
std::string result = "";
@@ -151,10 +166,23 @@ struct converter<std::string>{
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
+ template <typename A> static std::string convert(std::list<A> val) {
+ std::string result = "";
+ for (auto it : val) {
+ result += "," + converter<std::string>::convert(it);
+ }
+ if (result.size() > 0) result = result.substr(1, result.size());
+ return "[" + result + "]";
+ }
template<typename A, typename B>
static std::string convert(std::pair<A,B> val){
return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
}
+ //NEW GUY
+ template<typename A, typename B>
+ static std::string convert(Wpair<A,B> val){
+ return "(" + converter<std::string>::convert(val.first) + "," + converter<std::string>::convert(val.second) + ")";
+ }
template<typename A, typename B>
static std::string convert(std::map<A,B> val){
std::string result = "";
@@ -164,6 +192,15 @@ struct converter<std::string>{
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
+ template<typename A, typename B>
+ static std::string convert(std::unordered_map<A,B> val){
+ std::string result = "";
+ for(auto it : val){
+ result += "," + converter<std::string>::convert(it);
+ }
+ if(result.size() > 0) result = result.substr(1,result.size());
+ return "[" + result + "]";
+ }
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
static std::string convert(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> val){
std::string result = "";
@@ -178,7 +215,25 @@ struct converter<std::string>{
}
return "[" + result + "]";
}
+ //// WOLF DEFINED TYPES -----> TO STRING
+ static std::string convert(VectorComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
+ auto helper = std::unordered_map<char, Eigen::VectorXd>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<char, Eigen::VectorXd>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
+ static std::string convert(MatrixComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
+ auto helper = std::unordered_map< char, std::unordered_map<char, Eigen::MatrixXd>>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
};
+ //// CONVERTERS ~~~~ TYPE ----> STRING
template<typename A, typename B>
struct converter<std::pair<A,B>>{
static std::pair<A,B> convert(std::string val){
@@ -188,6 +243,17 @@ struct converter<std::pair<A,B>>{
return std::pair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
} else throw std::runtime_error("Invalid string format representing a pair. Correct format is {identifier:value}. String provided: " + val);
}
+};
+ //NEW GUY
+template<typename A, typename B>
+struct converter<Wpair<A,B>>{
+ static Wpair<A,B> convert(std::string val){
+ std::regex rgxP("\\(([^\\(,]+),([^\\)]+)\\)");
+ std::smatch matches;
+ if(std::regex_match(val, matches, rgxP)) {
+ return Wpair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
+ } else throw std::runtime_error("Invalid string format representing a Wpair. Correct format is (first,second). String provided: " + val);
+ }
};
// TODO: WARNING!! For some reason when trying to specialize converter to std::array
// it defaults to the generic T type, thus causing an error!
@@ -215,7 +281,7 @@ struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCo
auto values = converter<std::vector<_Scalar>>::convert(splittedValues[1]);
Matrix m = Matrix();
if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic) {
- if(dimensions.size() != 2) throw std::runtime_error("Invalid string representing Eigen Matrix. Missing dimensions. String provided: " + val);
+ if(dimensions.size() != 2) throw std::runtime_error("Invalid string representing a dynamic Eigen Matrix. Missing dimensions. String provided: " + val);
m.resize(dimensions[0],dimensions[1]);
}else if(_Rows == Eigen::Dynamic){
int nrows = (int) values.size() / _Cols;
@@ -224,7 +290,7 @@ struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCo
int ncols = (int) values.size() / _Rows;
m.resize(_Rows, ncols);
}
- if(m.rows()*m.cols() != values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
+ if(m.rows()*m.cols() != (int) values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
+ "the Eigen matrix is of dimensions "
+ std::to_string(m.rows()) + "x" + std::to_string(m.cols()));
else{
@@ -238,8 +304,11 @@ struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCo
template<typename A>
struct converter<std::map<std::string,A>>{
static std::map<std::string,A> convert(std::string val){
- auto str_map = utils::splitMapStringRepresentation(val);
- auto v = utils::pairSplitter(str_map);
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
auto map = std::map<std::string, A>();
for(auto it : v){
auto p = converter<std::pair<std::string, A>>::convert(it);
@@ -248,5 +317,67 @@ struct converter<std::map<std::string,A>>{
return map;
}
};
+template<typename A, typename B>
+struct converter<std::map<A,B>>{
+ static std::map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+template<typename A, typename B>
+struct converter<std::unordered_map<A,B>>{
+ static std::unordered_map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of an unordered Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::unordered_map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+//// FROM STRING -----> WOLF DEFINED TYPES
+template<>
+struct converter<VectorComposite>{
+ static VectorComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<char, Eigen::VectorXd>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = VectorComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<char, Eigen::VectorXd>(it.first, it.second));
+ }
+ return helper;
+ }
+};
+template<>
+struct converter<MatrixComposite>{
+ static MatrixComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<char,
+ std::unordered_map<char, Eigen::MatrixXd>>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = MatrixComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(it.first, it.second));
+ }
+ return helper;
+ }
+};
}
-#endif
\ No newline at end of file
+#endif
diff --git a/include/core/utils/converter_utils.h b/include/core/utils/converter_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..1765c3f9091edebb4db92e66790d333e0116548a
--- /dev/null
+++ b/include/core/utils/converter_utils.h
@@ -0,0 +1,46 @@
+#ifndef CONVERTER_UTILS_H
+#define CONVERTER_UTILS_H
+
+// wolf
+#include "core/common/wolf.h"
+
+
+// std
+#include <regex>
+namespace utils{
+ //Typically we want to convert from/to list-type structures. In order to be general
+ //we define a list type which is used throughout the converter. In this case this type
+ //is implemented with std::vector
+ template <typename A>
+ using list = std::vector<A>;
+ // template <typename A>
+ // class toString<A>{};
+ /** @Brief Splits a comma-separated string into its pieces
+ * @param val is just the string of comma separated values
+ * @return <b>{std::vector<std::string>}</b> vector whose i-th component is the i-th comma separated value
+ */
+ std::vector<std::string> splitter(std::string val);
+ /** @Brief Returns all the substrings of @val that match @exp
+ * @param val String to be matched
+ * @param exp Regular expression
+ * @return <b>{std::vector<std::string>}</b> Collection of matching substrings
+ */
+ std::vector<std::string> getMatches(std::string val, std::regex exp);
+ /** @Brief Given a string representation of a matrix extracts the dimensions and the values
+ * @param matrix is a string either of the form "[[N,M],[a1,a2,a3,...]]" or "[a1,a2,a3,...]"
+ * @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...])
+ */
+ std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix);
+ /** @Brief Splits a dictionary-like string of the form {k1:v1},{k2:v2},... It is tightly coupled with splitMapStringRepresentation
+ * @param val is just a dictionary-like string
+ * @return <b>{std::vector<std::string>}</b> Collection of the strings of the form {k_i:v_i}
+ */
+ std::vector<std::string> pairSplitter(std::string val);
+ /** @Brief Splits a dictionary-like string of the form [{k1:v1},{k2:v2},...]
+ * @param str_map just a dictionary-like string
+ * @return <b>{std::string}</b> String {k1:v1},{k2:v2},... (notice the removed brackets)
+ */
+ std::string splitMapStringRepresentation(std::string str_map);
+ std::vector<std::string> parseList(std::string val);
+}
+#endif
\ No newline at end of file
diff --git a/include/core/utils/eigen_assert.h b/include/core/utils/eigen_assert.h
index 5c079da7aab383eed93578869b219c65854419d4..7873b792cf89c4af8e015776798e381f0315776f 100644
--- a/include/core/utils/eigen_assert.h
+++ b/include/core/utils/eigen_assert.h
@@ -10,7 +10,7 @@ namespace Eigen {
*
* Help:
*
- * The WOLF project implements many template functions using Eigen Matrix and Quaternions, in different versions
+ * The WOLF project implements many template functions using Eigen Matrix and Quaterniond, 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):
diff --git a/include/core/utils/eigen_predicates.h b/include/core/utils/eigen_predicates.h
index 41dddac1d50dc2d230fb749a959d3a015974a07f..1f319f3f81df939415a688fd6e7206918f5d1825 100644
--- a/include/core/utils/eigen_predicates.h
+++ b/include/core/utils/eigen_predicates.h
@@ -18,10 +18,10 @@ namespace wolf {
/// 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)
+auto is_constant = [](const Eigen::MatrixXd& lhs, const double val, const double precision)
{
- for (Eigen::MatrixXs::Index j = 0; j < lhs.cols(); ++j)
- for (Eigen::MatrixXs::Index i = 0; i < lhs.rows(); ++i)
+ for (Eigen::MatrixXd::Index j = 0; j < lhs.cols(); ++j)
+ for (Eigen::MatrixXd::Index i = 0; i < lhs.rows(); ++i)
if (std::abs(lhs.coeff(i, j) - val) > precision)
return false;
return true;
@@ -29,38 +29,38 @@ auto is_constant = [](const Eigen::MatrixXs& lhs, const Scalar val, const Scalar
/// @brief check that each element of the Matrix/Vector difference
/// is approx 0 +- precision
-auto pred_zero = [](const Eigen::MatrixXs& lhs, const Scalar precision)
+auto pred_zero = [](const Eigen::MatrixXd& lhs, const double 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)
+auto pred_diff_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double 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)
+auto pred_diff_norm_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double 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)
+auto pred_is_approx = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double 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)
+auto pred_quat_is_approx = [](const Eigen::Quaterniond& lhs, const Eigen::Quaterniond& rhs, const double 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); };
+auto pred_quat_identity = [](const Eigen::Quaterniond& lhs, const double precision)
+ { return pred_quat_is_approx(lhs, Eigen::Quaterniond::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)
+auto pred_angle_is_approx = [](const double lhs, const double rhs, const double 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)
+auto pred_angle_zero = [](const double lhs, const double precision)
{ return pred_angle_is_approx(lhs, 0, precision); };
/// @brief check that the lhs pose is approx rhs +- precision
@@ -81,13 +81,13 @@ auto pred_angle_zero = [](const Scalar lhs, const Scalar precision)
/// @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)
+//auto pred_pose_is_approx = [](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs, const double precision)
// {
-// const Eigen::MatrixXs d = lhs.inverse() * rhs;
+// const Eigen::MatrixXd 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)),
+// pred_quat_identity(Eigen::Quaterniond(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 !");
diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf14ff844d981922848645774799f2b18655e06f
--- /dev/null
+++ b/include/core/utils/graph_search.h
@@ -0,0 +1,44 @@
+#ifndef GRAPH_SEARCH_H
+#define GRAPH_SEARCH_H
+
+#include "core/common/wolf.h"
+#include "core/frame/frame_base.h"
+#include "core/factor/factor_base.h"
+
+#include <map>
+
+namespace wolf
+{
+
+class GraphSearch
+{
+ private:
+
+ std::map<FrameBasePtr, std::pair<FactorBasePtr,FrameBasePtr>> parents_;
+
+ public:
+
+ GraphSearch();
+
+ ~GraphSearch();
+
+ FactorBasePtrList computeShortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist = 0);
+
+ std::set<FrameBasePtr> getNeighborFrames(const std::set<FrameBasePtr>& frms);
+
+ static FactorBasePtrList shortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist = 0)
+ {
+ GraphSearch graph_search;
+
+ return graph_search.computeShortestPath(frm1, frm2, max_graph_dist);
+ }
+
+};
+
+
+} // namespace wolf
+#endif
diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..7da10d043ebaf5f64f28520d11fe2b114a3c1284
--- /dev/null
+++ b/include/core/utils/loader.h
@@ -0,0 +1,32 @@
+#ifndef LOADER_H
+#define LOADER_H
+
+#include <string>
+
+class Loader{
+protected:
+ std::string path_;
+public:
+ Loader(std::string _file);
+ virtual void load() = 0;
+ virtual void close() = 0;
+};
+
+class LoaderRaw: public Loader{
+ void* resource_;
+public:
+ LoaderRaw(std::string _file);
+ ~LoaderRaw();
+ void load() override;
+ void close() override;
+};
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// this->resource_ = new ClassLoader(this->path_);
+// }
+// void close(){
+// delete this->resource_;
+// }
+// };
+#endif
diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp
deleted file mode 100644
index 4f36eeecb9cda277e16bb495349ea047b546b6ce..0000000000000000000000000000000000000000
--- a/include/core/utils/loader.hpp
+++ /dev/null
@@ -1,43 +0,0 @@
-#ifndef LOADER_HPP
-#define LOADER_HPP
-#include <dlfcn.h>
-class Loader{
-protected:
- std::string path_;
-public:
- Loader(std::string _file){
- this->path_ = _file;
- }
- virtual void load() = 0;
- virtual void close() = 0;
-};
-class LoaderRaw: public Loader{
- void* resource_;
-public:
- LoaderRaw(std::string _file):
- Loader(_file)
- {
- //
- }
- ~LoaderRaw(){
- this->close();
- }
- void load(){
- this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY);
- if(not this->resource_)
- throw std::runtime_error("Couldn't load resource with path " + this->path_);
- }
- void close(){
- if(this->resource_) dlclose(this->resource_);
- }
-};
-// class LoaderPlugin: public Loader{
-// ClassLoader* resource_;
-// void load(){
-// this->resource_ = new ClassLoader(this->path_);
-// }
-// void close(){
-// delete this->resource_;
-// }
-// };
-#endif
\ No newline at end of file
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8b473ca26703f17ea932cca4d1e913717108026
--- /dev/null
+++ b/include/core/utils/params_server.h
@@ -0,0 +1,55 @@
+#ifndef PARAMS_SERVER_H
+#define PARAMS_SERVER_H
+
+#include "core/utils/converter.h"
+
+#include <map>
+#include <exception>
+
+namespace wolf{
+
+class MissingValueException : public std::runtime_error
+{
+public:
+ MissingValueException(std::string _msg) : std::runtime_error(_msg) {}
+};
+
+class ParamsServer{
+ std::map<std::string, std::string> params_;
+public:
+ ParamsServer();
+ ParamsServer(std::map<std::string, std::string> _params);
+ ~ParamsServer(){
+ //
+ }
+
+ void print();
+
+
+ void addParam(std::string _key, std::string _value);
+
+ void addParams(std::map<std::string, std::string> _params);
+
+ // template<typename T>
+ // T getParam(std::string key, std::string def_value) const {
+ // if(params_.find(key) != params_.end()){
+ // return converter<T>::convert(params_.find(key)->second);
+ // }else{
+ // return converter<T>::convert(def_value);
+ // }
+ // }
+
+ template<typename T>
+ T getParam(std::string _key) const {
+ if(params_.find(_key) != params_.end()){
+ return converter<T>::convert(params_.find(_key)->second);
+ }else{
+ throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
+ }
+ }
+
+};
+
+}
+
+#endif
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
deleted file mode 100644
index 74fb8f3e7c5f9dc12ea19cc7e85eb43dfbf011fd..0000000000000000000000000000000000000000
--- a/include/core/utils/params_server.hpp
+++ /dev/null
@@ -1,88 +0,0 @@
-#ifndef PARAMS_SERVER_HPP
-#define PARAMS_SERVER_HPP
-#include <vector>
-#include <regex>
-#include <map>
-#include "core/utils/converter.h"
-namespace wolf{
-class paramsServer{
- struct ParamsInitSensor{
- std::string _type;
- std::string _name;
- };
- struct ParamsInitProcessor{
- std::string _type;
- std::string _name;
- std::string _name_assoc_sensor;
- };
- std::map<std::string, std::string> _params;
- std::map<std::string,ParamsInitSensor> _paramsSens;
- std::map<std::string,ParamsInitProcessor> _paramsProc;
-public:
- paramsServer(){
- _params = std::map<std::string, std::string>();
- _paramsSens = std::map<std::string,ParamsInitSensor>();
- _paramsProc = std::map<std::string,ParamsInitProcessor>();
- }
- paramsServer(std::map<std::string, std::string> params,
- std::vector<std::array<std::string,2>> sensors,
- std::vector<std::array<std::string,3>> procs){
- _params = params;
- _paramsSens = std::map<std::string,ParamsInitSensor>();
- _paramsProc = std::map<std::string,ParamsInitProcessor>();
- for(auto it : sensors) {
- ParamsInitSensor pSensor = {it.at(0), it.at(1)};
- _paramsSens.insert(std::pair<std::string,ParamsInitSensor>(it.at(1), pSensor));
- }
- for(auto it : procs) {
- ParamsInitProcessor pProcs = {it.at(0), it.at(1), it.at(2)};
- _paramsProc.insert(std::pair<std::string,ParamsInitProcessor>(it.at(1), pProcs));
- }
- }
- ~paramsServer(){
- //
- }
- void print(){
- for(auto it : _params)
- std::cout << it.first << "~~" << it.second << std::endl;
- }
- void addInitParamsSensor(std::string type, std::string name){
- ParamsInitSensor params = {type, name};
- _paramsSens.insert(std::pair<std::string, ParamsInitSensor>(type + "/" + name + "/", params));
- }
- void addInitParamsProcessor(std::string type, std::string name, std::string name_assoc_sensor){
- ParamsInitProcessor params = {type, name, name_assoc_sensor};
- _paramsProc.insert(std::pair<std::string, ParamsInitProcessor>(type + "/" + name + "/", params));
- }
- void addParam(std::string key, std::string value){
- _params.insert(std::pair<std::string, std::string>(key, value));
- }
- template<typename T>
- T getParam(std::string key, std::string def_value) const {
- if(_params.find(key) != _params.end()){
- return converter<T>::convert(_params.find(key)->second);
- }else{
- return converter<T>::convert(def_value);
- }
- }
- template<typename T>
- T getParam(std::string key) const {
- if(_params.find(key) != _params.end()){
- return converter<T>::convert(_params.find(key)->second);
- }else{
- throw std::runtime_error("The following key: '" + key + "' has not been found in the parameters server and no default value was provided.");
- }
- }
- std::vector<ParamsInitSensor> getSensors(){
- std::vector<ParamsInitSensor> rtn = std::vector<ParamsInitSensor>();
- std::transform(this->_paramsSens.begin(), this->_paramsSens.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitSensor> v){return v.second;});
- return rtn;
- }
- std::vector<ParamsInitProcessor> getProcessors(){
- std::vector<ParamsInitProcessor> rtn = std::vector<ParamsInitProcessor>();
- std::transform(this->_paramsProc.begin(), this->_paramsProc.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitProcessor> v){return v.second;});
- return rtn;
- }
-};
-}
-#endif
\ No newline at end of file
diff --git a/test/utils_gtest.h b/include/core/utils/utils_gtest.h
similarity index 87%
rename from test/utils_gtest.h
rename to include/core/utils/utils_gtest.h
index 9cb2166e8429cd236f77c502891c14880e57ec30..31564f92f125ec81cbc2273e56dc032d057b6250 100644
--- a/test/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -81,7 +81,7 @@ extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
class TestCout : public std::stringstream
{
public:
- ~TestCout()
+ ~TestCout() override
{
PRINTF("%s\n", str().c_str());
}
@@ -113,12 +113,12 @@ TEST(Test, Foo)
/* Macros related to testing Eigen classes:
*/
-#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
return (lhs - rhs).isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
return (lhs - rhs).isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
@@ -127,16 +127,16 @@ TEST(Test, Foo)
#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- MatrixXs er = lhs - rhs; \
- er(2) = pi2pi((Scalar)er(2)); \
+#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- MatrixXs er = lhs - rhs; \
- er(2) = pi2pi((Scalar)er(2)); \
+#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
new file mode 100644
index 0000000000000000000000000000000000000000..74473f695b2c7078aea77111cefe34ec6977efd2
--- /dev/null
+++ b/include/core/yaml/parser_yaml.h
@@ -0,0 +1,82 @@
+#ifndef PARSER_YAML_H
+#define PARSER_YAML_H
+
+#include "core/utils/converter.h"
+#include "core/common/wolf.h"
+
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+class ParserYaml {
+ struct ParamsInitSensor{
+ std::string type_;
+ std::string name_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct ParamsInitProcessor{
+ std::string type_;
+ std::string name_;
+ std::string name_assoc_sensor_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct SubscriberManager{
+ std::string package_;
+ std::string type_;
+ std::string topic_;
+ std::string sensor_name_;
+ YAML::Node n_;
+ };
+ struct PublisherManager{
+ std::string package_;
+ std::string type_;
+ std::string topic_;
+ std::string period_;
+ YAML::Node n_;
+ };
+ std::map<std::string, std::string> params_;
+ std::string active_name_;
+ std::vector<ParamsInitSensor> paramsSens_;
+ std::vector<ParamsInitProcessor> paramsProc_;
+ std::stack<std::string> parsing_file_;
+ std::string file_;
+ std::string path_root_;
+ std::vector<SubscriberManager> subscriber_managers_;
+ std::vector<PublisherManager> publisher_managers_;
+ YAML::Node problem;
+ std::string generatePath(std::string);
+ YAML::Node loadYaml(std::string);
+ void insert_register(std::string, std::string);
+public:
+ ParserYaml(std::string _file, std::string _path_root = "",
+ bool _freely_parse = false);
+ ~ParserYaml()
+ {
+ //
+ }
+ void parse_freely();
+ std::map<std::string, std::string> getParams();
+
+ private:
+ void walkTree(std::string _file);
+ void walkTree(std::string _file, std::vector<std::string>& _tags);
+ void walkTree(std::string _file, std::vector<std::string>& _tags, std::string _hdr);
+/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
+ * @param YAML node to be parsed
+ * @param tags represents the path from the root of the YAML tree to the current node
+ * @param hdr is the name of the current YAML node
+ */
+ void walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string _hdr);
+ void updateActiveName(std::string _tag);
+/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
+ are defined at the top level of the YAML file.
+ * @param file is the path to the YAML file
+ * @param _n is the top YAML::Node of file*/
+ void parseFirstLevel(YAML::Node _n, std::string _file);
+ std::string tagsToString(std::vector<std::string>& _tags);
+ void parse();
+};
+}
+#endif
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
deleted file mode 100644
index 9b678e1b5fe2823877695c02dedaaa3784f67e98..0000000000000000000000000000000000000000
--- a/include/core/yaml/parser_yaml.hpp
+++ /dev/null
@@ -1,324 +0,0 @@
-#ifndef PARSER_YAML_HPP
-#define PARSER_YAML_HPP
-#include "yaml-cpp/yaml.h"
-#include <vector>
-#include <regex>
-#include <map>
-#include <iostream>
-#include <algorithm>
-
-using namespace std;
-namespace {
- // std::string remove_ws( const std::string& str ){
- // std::string str_no_ws ;
- // for( char c : str ) if( !std::isspace(c) ) str_no_ws += c ;
- // return str_no_ws ;
- // }
- string parseSequence(YAML::Node n){
- assert(n.Type() != YAML::NodeType::Map && "Trying to parse as a Sequence a Map node");
- if(n.Type() == YAML::NodeType::Scalar) return n.Scalar();
- string aux = "[";
- bool first = true;
- for(auto it : n){
- if(first) {
- aux = aux + parseSequence(it);
- first = false;
- }else{
- aux = aux + "," + parseSequence(it);
- }
- }
- aux = aux + "]";
- return aux;
- }
- std::string mapToString(std::map<std::string,std::string> _map){
- std::string result = "";
- auto v = std::vector<string>();
- std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<string,string> p){return "{" + p.first + ":" + p.second + "}";});
- auto concat = [](string ac, string str)-> string {
- return ac + str + ",";
- };
- string aux = "";
- string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
- if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
- else accumulated = "";
- return "[" + accumulated + "]";
- }
-}
-class parserYAML {
- struct ParamsInitSensor{
- string _type;
- string _name;
- YAML::Node n;
- };
- struct ParamsInitProcessor{
- string _type;
- string _name;
- string _name_assoc_sensor;
- YAML::Node n;
- };
- map<string, string> _params;
- string _active_name;
- vector<ParamsInitSensor> _paramsSens;
- vector<ParamsInitProcessor> _paramsProc;
- vector<string> _files;
- string _file;
- bool _relative_path;
- string _path_root;
- vector<array<string, 3>> _callbacks;
-public:
- parserYAML(){
- _params = map<string, string>();
- _active_name = "";
- _paramsSens = vector<ParamsInitSensor>();
- _paramsProc = vector<ParamsInitProcessor>();
- _file = "";
- _files = vector<string>();
- _path_root = "";
- _relative_path = false;
- _callbacks = vector<array<string, 3>>();
- }
- parserYAML(string file){
- _params = map<string, string>();
- _active_name = "";
- _paramsSens = vector<ParamsInitSensor>();
- _paramsProc = vector<ParamsInitProcessor>();
- _files = vector<string>();
- _file = file;
- _path_root = "";
- _relative_path = false;
- _callbacks = vector<array<string, 3>>();
- }
- parserYAML(string file, string path_root){
- _params = map<string, string>();
- _active_name = "";
- _paramsSens = vector<ParamsInitSensor>();
- _paramsProc = vector<ParamsInitProcessor>();
- _files = vector<string>();
- _file = file;
- _path_root = path_root;
- _relative_path = true;
- _callbacks = vector<array<string, 3>>();
- }
- ~parserYAML(){
- //
- }
- void walkTree(string file);
- void walkTree(string file, vector<string>& tags);
- void walkTree(string file, vector<string>& tags, string hdr);
- void walkTreeR(YAML::Node n, vector<string>& tags, string hdr);
- void updateActiveName(string tag);
- void parseFirstLevel(string file);
- string tagsToString(vector<string>& tags);
- vector<array<string, 2>> sensorsSerialization();
- vector<array<string, 3>> processorsSerialization();
- vector<string> getFiles();
- vector<array<string, 3>> getCallbacks();
- map<string,string> getParams();
- void parse();
- map<string, string> fetchAsMap(YAML::Node);
-};
-string parserYAML::tagsToString(vector<std::string> &tags){
- string hdr = "";
- for(auto it : tags){
- hdr = hdr + "/" + it;
- }
- return hdr;
-}
-void parserYAML::walkTree(string file){
- YAML::Node n;
- // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
- if(not _relative_path) n = YAML::LoadFile(file);
- else n = YAML::LoadFile(_path_root + file);
- vector<string> hdrs = vector<string>();
- walkTreeR(n, hdrs, "");
-}
-void parserYAML::walkTree(string file, vector<string>& tags){
- YAML::Node n;
- // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
- if(not _relative_path) n = YAML::LoadFile(file);
- else n = YAML::LoadFile(_path_root + file);
- walkTreeR(n, tags, "");
-}
-void parserYAML::walkTree(string file, vector<string>& tags, string hdr){
- YAML::Node n;
- // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
- if(not _relative_path) n = YAML::LoadFile(file);
- else n = YAML::LoadFile(_path_root + file);
- walkTreeR(n, tags, hdr);
-}
-void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
- switch (n.Type()) {
- case YAML::NodeType::Scalar : {
- regex r("^@.*");
- if(regex_match(n.Scalar(), r)){
- string str = n.Scalar();
- // cout << "SUBSTR " << str.substr(1,str.size() - 1);
- walkTree(str.substr(1,str.size() - 1), tags, hdr);
- }else{
- // std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬"));
- // cout << "«»" << n.Scalar() << endl;
- _params.insert(pair<string,string>(hdr, n.Scalar()));
- }
- break;
- }
- case YAML::NodeType::Sequence : {
- // cout << tags[tags.size() - 1] << "«»" << kv << endl;
- // std::vector<double> vi = n.as<std::vector<double>>();
- string aux = parseSequence(n);
- _params.insert(pair<string,string>(hdr, aux));
- break;
- }
- case YAML::NodeType::Map : {
- for(const auto& kv : n){
- //If the key's value starts with a $ (i.e. $key) then its value is parsed as a literal map,
- //otherwise the parser recursively parses the map.
- regex r("^\\$.*");
- if(not regex_match(kv.first.as<string>(), r)){
- /*
- If key=="follow" then the parser will assume that the value is a path and will parse
- the (expected) yaml file at the specified path. Note that this does not increase the header depth.
- The following example shows how the header remains unafected:
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - follow: "@some_path" |
- - var: 1.2 |
- Resulting map:
- cov_det -> 1
- my_value-> 23
- var: 1.2
- Instead of:
- cov_det -> 1
- follow/my_value-> 23
- var: 1.2
- Which would result from the following yaml files
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - $follow: "@some_path" |
- - var: 1.2 |
- */
- regex rr("follow");
- if(not regex_match(kv.first.as<string>(), rr)) {
- tags.push_back(kv.first.as<string>());
- if(tags.size() == 2) this->updateActiveName(kv.first.as<string>());
- walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<string>());
- tags.pop_back();
- if(tags.size() == 1) this->updateActiveName("");
- }else{
- walkTree(kv.second.as<string>(), tags, hdr);
- }
- }else{
- string key = kv.first.as<string>();
- key = key.substr(1,key.size() - 1);
- auto fm = fetchAsMap(kv.second);
- _params.insert(pair<string,string>(hdr + "/" + key, mapToString(fm)));
- }
- }
- break;
- }
- default:
- assert(1 == 0 && "Unsupported node Type at walkTreeR");
- break;
- }
-}
-void parserYAML::updateActiveName(string tag){
- this->_active_name = tag;
-}
-void parserYAML::parseFirstLevel(string file){
- YAML::Node n;
- // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
- if(not _relative_path) n = YAML::LoadFile(file);
- else n = YAML::LoadFile(_path_root + file);
- YAML::Node n_config = n["config"];
- assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
- for(const auto& kv : n_config["sensors"]){
- ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv};
- _paramsSens.push_back(pSensor);
- }
- for(const auto& kv : n_config["processors"]){
- ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor name"].Scalar(), kv};
- _paramsProc.push_back(pProc);
- }
- for(const auto& kv : n_config["callbacks"]){
- _callbacks.push_back({{kv[0].as<std::string>(), kv[1].as<std::string>(), kv[2].as<std::string>()}});
- }
- YAML::Node n_files = n["files"];
- assert(n_files.Type() == YAML::NodeType::Sequence && "trying to parse files node but found a non-Sequence node");
- for(const auto& kv : n_files){
- _files.push_back(kv.Scalar());
- }
-}
-vector<array<string, 2>> parserYAML::sensorsSerialization(){
- vector<array<string, 2>> aux = vector<array<string, 2>>();
- for(auto it : this->_paramsSens)
- aux.push_back({{it._type,it._name}});
- return aux;
-}
-vector<array<string, 3>> parserYAML::processorsSerialization(){
- vector<array<string, 3>> aux = vector<array<string, 3>>();
- for(auto it : this->_paramsProc)
- aux.push_back({{it._type,it._name,it._name_assoc_sensor}});
- return aux;
-}
-vector<string> parserYAML::getFiles(){
- return this->_files;
-}
-vector<array<string, 3>> parserYAML::getCallbacks(){
- return this->_callbacks;
-}
-map<string,string> parserYAML::getParams(){
- map<string,string> rtn = _params;
- return rtn;
-}
-void parserYAML::parse(){
- this->parseFirstLevel(this->_file);
- for(auto it : _paramsSens){
- vector<string> tags = vector<string>();
- // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
- this->walkTreeR(it.n , tags , it._name);
- }
- for(auto it : _paramsProc){
- vector<string> tags = vector<string>();
- // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
- this->walkTreeR(it.n , tags , it._name);
- }
-}
-map<string, string> parserYAML::fetchAsMap(YAML::Node n){
- assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
- auto m = map<string, string>();
- for(const auto& kv : n){
- string key = kv.first.as<string>();
- switch (kv.second.Type()) {
- case YAML::NodeType::Scalar : {
- string value = kv.second.Scalar();
- m.insert(pair<string,string>(key, value));
- break;
- }
- case YAML::NodeType::Sequence : {
- // cout << tags[tags.size() - 1] << "«»" << kv << endl;
- // std::vector<double> vi = n.as<std::vector<double>>();
- string aux = parseSequence(kv.second);
- m.insert(pair<string,string>(key, aux));
- break;
- }
- case YAML::NodeType::Map : {
- m = fetchAsMap(kv.second);
- auto rtn = vector<string>();
- std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<string,string> v){return "{" + v.first + ":" + v.second + "}";});
- auto concat = [](string ac, string str)-> string {
- return ac + str + ",";
- };
- string aux = "";
- string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat);
- if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
- else accumulated = "";
- m.insert(pair<string,string>(key, "[" + accumulated + "]"));
- break;
- }
- default:
- assert(1 == 0 && "Unsupported node Type at fetchAsMap");
- break;
- }
- }
- return m;
-}
-#endif
\ No newline at end of file
diff --git a/scilab/corner_detector.sce b/scilab/corner_detector.sce
index 18035de71787e22dd3d53adc8002cbbac8f873f6..dfeae1945084531266c5ffd2019a17ec09e6c809 100644
--- a/scilab/corner_detector.sce
+++ b/scilab/corner_detector.sce
@@ -1,4 +1,4 @@
-//info about 2D homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
+//info about 2d homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
// clear all
xdel(winsid());
diff --git a/serialization/CMakeLists.txt b/serialization/CMakeLists.txt
deleted file mode 100644
index a853ba8ec5158727fcd4bfd9752d6de398c5aefa..0000000000000000000000000000000000000000
--- a/serialization/CMakeLists.txt
+++ /dev/null
@@ -1 +0,0 @@
-add_subdirectory(cereal)
diff --git a/serialization/cereal/CMakeLists.txt b/serialization/cereal/CMakeLists.txt
deleted file mode 100644
index 1ae16bcd213401f6558a4ee80c2c5a8ef8c5f171..0000000000000000000000000000000000000000
--- a/serialization/cereal/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-SET(HDRS_SERIALIZATION ${HDRS_SERIALIZATION}
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_core.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_geometry.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_sparse.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_homogeneous.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_quaternion.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_node_base.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_sensor_intrinsic_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_sensor_odom2d_intrinsic.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_params_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_odom2d_params.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_odom3d_params.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_time_stamp.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/archives.h
- ${CMAKE_CURRENT_SOURCE_DIR}/io.h
-
- PARENT_SCOPE)
diff --git a/serialization/cereal/archives.h b/serialization/cereal/archives.h
deleted file mode 100644
index d8c16fd180ef04049ceedaf268fa33782790d27a..0000000000000000000000000000000000000000
--- a/serialization/cereal/archives.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_ARCHIVE_H_
-#define _WOLF_IO_CEREAL_ARCHIVE_H_
-
-#include <cereal/archives/binary.hpp>
-#include <cereal/archives/json.hpp>
-#include <cereal/archives/portable_binary.hpp>
-#include <cereal/archives/xml.hpp>
-
-#endif /* _WOLF_IO_CEREAL_ARCHIVE_H_ */
diff --git a/serialization/cereal/io.h b/serialization/cereal/io.h
deleted file mode 100644
index 8a8e58c2d8a873b4d327e1c8639fc007cb8604aa..0000000000000000000000000000000000000000
--- a/serialization/cereal/io.h
+++ /dev/null
@@ -1,229 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_IO_H_
-#define _WOLF_SERIALIZATION_CEREAL_IO_H_
-
-#include <stdexcept>
-#include "archives.h"
-
-//#include <cereal/types/tuple.hpp>
-
-namespace wolf {
-namespace serialization {
-
-/// @todo demangle typeid.name ?
-template <typename T>
-inline const std::string& type_name(const T&)
-{
- static const std::string typeid_name = typeid(T).name();
- return typeid_name;
-}
-
-inline std::string extension(const std::string& file)
-{
- const std::size_t p = file.find_last_of(".");
- return (p != std::string::npos) ? file.substr(p) : "";
-}
-
-//struct Extensions
-//{
-// constexpr static const char* bin = ".bin";
-// constexpr static const char* json = ".json";
-// constexpr static const char* xml = ".xml";
-
-// constexpr static const char* fall_back = json;
-//};
-
-//enum class Extensions2 : std::size_t
-//{
-// BIN = 0,
-//// CBIN,
-// JSON,
-//// TEXT,
-// XML,
-//};
-
-//template <char... Chars>
-//struct constexp_str
-//{
-// using type = constexp_str<Chars...>;
-
-// virtual ~constexp_str() = default;
-
-// constexpr static const char value[sizeof...(Chars)+1] = {Chars..., '\0'};
-
-// constexpr static std::size_t size() { return sizeof...(Chars); }
-
-// constexpr static const char* c_str() { return &value[0]; }
-
-//// constexpr static bool comp(const std::string& s) { return s == value; }
-
-// /*constexpr*/ bool operator == (const std::string& s) { return s == value; }
-
-// constexpr /*static*/ operator const char* () { return c_str(); }
-// constexpr /*static*/ operator std::string& () { return c_str(); }
-//};
-
-struct Extensions
-{
-// template <char... Chars>
-// struct EXT : constexp_str<Chars...>
-// {
-// //
-// };
-
-// struct BIN : EXT<'.','b','i','n'> { };
-// struct XML : EXT<'.','x','m','l'> { };
-// struct JSON : EXT<'.','j','s','o','n'> { };
-
- struct EXT { virtual ~EXT() = default; };
-
- struct BIN : EXT
- {
- constexpr static const char* value = ".bin";
- bool operator == (const std::string& s) { return value == s; }
- };
-
- struct XML : EXT
- {
- constexpr static const char* value = ".xml";
- bool operator == (const std::string& s) { return value == s; }
- };
-
- struct JSON : EXT
- {
- constexpr static const char* value = ".json";
- bool operator == (const std::string& s) { return value == s; }
- };
-};
-
-template <typename Ar>
-void serialize_pack(Ar&&)
-{
- // end of expansion
-}
-
-template <typename Ar, typename T, typename... Args>
-void serialize_pack(Ar&& archive, T&& object, Args&&... args)
-{
- archive( cereal::make_nvp(type_name(object), std::forward<T>(object)) );
- serialize_pack(archive, std::forward<Args>(args)...);
-}
-
-template <typename Ar, typename S, typename T, typename... Args>
-void serialize(S& stream, T&& object, Args&&... args)
-{
- Ar archive(stream);
- archive( cereal::make_nvp(type_name(object), std::forward<T>(object)) );
-
- serialize_pack(archive, std::forward<Args>(args)...);
-}
-
-template <typename EXT, typename InAr, typename OutAr>
-struct Serializer
-{
- template <typename S, typename... T>
- static void serialize_in(S& stream, T&... object)
- {
- serialize<InAr>(stream, object...);
- }
-
- template <typename S, typename... T>
- static void serialize_out(S& stream, T&&... object)
- {
- serialize<OutAr>(stream, std::forward<T>(object)...);
- }
-
- template <typename... T>
- static void save(std::string filename, T&&... o)
- {
- const std::string ext = serialization::extension(filename);
-
- if (ext != EXT::value) filename += EXT::value;
-
- std::ofstream os(filename);
- serialize_out(os, std::forward<T>(o)...);
- }
-
- template <typename... T>
- static void load(std::string filename, T&... o)
- {
- const std::string ext = serialization::extension(filename);
-
- if (ext != EXT::value) filename += EXT::value;
-
- std::ifstream is(filename);
- serialize_in(is, o...);
- }
-};
-
-using SerializerBin = Serializer<Extensions::BIN,
- cereal::BinaryInputArchive,
- cereal::BinaryOutputArchive>;
-
-using SerializerXML = Serializer<Extensions::XML,
- cereal::XMLInputArchive,
- cereal::XMLOutputArchive>;
-
-using SerializerJSON = Serializer<Extensions::JSON,
- cereal::JSONInputArchive,
- cereal::JSONOutputArchive>;
-
-} /* namespace serialization */
-
-template <typename... T>
-void save(const std::string& filename, T&&... o)
-throw(std::runtime_error)
-{
- const std::string ext = serialization::extension(filename);
-
- if (ext == serialization::Extensions::BIN::value)
- {
- serialization::SerializerBin::save(filename, std::forward<T>(o)...);
- }
- else if (ext == serialization::Extensions::JSON::value)
- {
- serialization::SerializerJSON::save(filename, std::forward<T>(o)...);
- }
- else if (ext == serialization::Extensions::XML::value)
- {
- serialization::SerializerXML::save(filename, std::forward<T>(o)...);
- }
- else if (ext == "") // falback is json
- {
- serialization::SerializerJSON::save(filename, std::forward<T>(o)...);
- }
- else
- {
- throw std::runtime_error("Unknown file extension : " + filename);
- }
-}
-
-template <typename... T>
-void load(const std::string& filename, T&... o)
-{
- const std::string ext = serialization::extension(filename);
-
- if (ext == serialization::Extensions::BIN::value)
- {
- serialization::SerializerBin::load(filename, o...);
- }
- else if (ext == serialization::Extensions::XML::value)
- {
- serialization::SerializerXML::load(filename, o...);
- }
- else if (ext == serialization::Extensions::JSON::value)
- {
- serialization::SerializerJSON::load(filename, o...);
- }
- else if (ext == "") // falback is json
- {
- serialization::SerializerJSON::load(filename, o...);
- }
- else
- {
- throw std::runtime_error("Unknown file extension : " + filename);
- }
-}
-
-} /* namespace wolf */
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_IO_H_ */
diff --git a/serialization/cereal/serialization_eigen_core.h b/serialization/cereal/serialization_eigen_core.h
deleted file mode 100644
index 8fb5b9b0c99637e7b4aed9efb3aaf68007aafb79..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_eigen_core.h
+++ /dev/null
@@ -1,125 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_H_
-#define _WOLF_IO_CEREAL_EIGEN_H_
-
-// Wolf includes
-#include <Eigen/Dense>
-#include <cereal/cereal.hpp>
-
-namespace cereal {
-
-/**
- * @brief Save Eigen::Matrix<...> to text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value,
-void>::type save(Archive& ar,
- const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows = mat.rows();
- decltype(mat.cols()) cols = mat.cols();
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-}
-
-/**
- * @brief Save Eigen::Matrix<...> to binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value,
-void>::type save(Archive& ar,
- const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows = mat.rows();
- decltype(mat.cols()) cols = mat.cols();
-
- ar(rows);
- ar(cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-/**
- * @brief Load compile-time sized Eigen::Matrix from text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- _Rows != Eigen::Dynamic and _Cols != Eigen::Dynamic,
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-}
-
-/**
- * @brief Load dynamic sized Eigen::Matrix from text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- (_Rows == Eigen::Dynamic or _Cols == Eigen::Dynamic),
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-
- //mat.resize(rows, cols);
-}
-
-/**
- * @brief Load compile-time sized Eigen::Matrix from binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- _Rows != Eigen::Dynamic and _Cols != Eigen::Dynamic,
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(rows);
- ar(cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-/**
- * @brief Load dynamic sized Eigen::Matrix from binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- (_Rows == Eigen::Dynamic or _Cols == Eigen::Dynamic),
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(rows);
- ar(cols);
-
- mat.resize(rows, cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_H_ */
diff --git a/serialization/cereal/serialization_eigen_geometry.h b/serialization/cereal/serialization_eigen_geometry.h
deleted file mode 100644
index f5d3ac61fa92c85d5c2b98670e86e5c6d3d68c24..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_eigen_geometry.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-#define _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-
-// Wolf includes
-#include <Eigen/Geometry>
-
-#include "serialization_eigen_core.h"
-
-namespace cereal {
-
-template<class Archive, typename _Scalar, int _Dim, int _Mode, int _Options>
-inline void save(Archive& ar,
- const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t)
-{
- save(ar, t.matrix());
-}
-
-template<class Archive, typename _Scalar, int _Dim, int _Mode, int _Options>
-inline void load(Archive& ar,
- Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t)
-{
- load(ar, t.matrix());
-}
-
-template<class Archive, typename _Scalar>
-void serialize(Archive & ar,
- Eigen::Quaternion<_Scalar>& q,
- const std::uint32_t /*version*/)
-{
- ar(cereal::make_nvp("w", q.w()));
- ar(cereal::make_nvp("x", q.x()));
- ar(cereal::make_nvp("y", q.y()));
- ar(cereal::make_nvp("z", q.z()));
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_ */
diff --git a/serialization/cereal/serialization_eigen_sparse.h b/serialization/cereal/serialization_eigen_sparse.h
deleted file mode 100644
index 9039b6920bc74abb3b4ff9efa4e0b935674ca53f..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_eigen_sparse.h
+++ /dev/null
@@ -1,79 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-#define _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-
-// Wolf includes
-#include <Eigen/Sparse>
-
-#include "serialization_eigen_core.h"
-#include <cereal/types/vector.hpp>
-
-namespace cereal {
-
-template<class Archive, typename Scalar, typename Index>
-inline void save(Archive& ar,
- const Eigen::Triplet<Scalar, Index>& t)
-{
- ar(cereal::make_nvp("row", t.row()));
- ar(cereal::make_nvp("col", t.col()));
- ar(cereal::make_nvp("value", t.value()));
-}
-
-template<class Archive, typename Scalar, typename Index>
-inline void load(Archive& ar,
- Eigen::Triplet<Scalar, Index>& t)
-{
- Index row, col;
- Scalar value;
-
- ar(cereal::make_nvp("row", row));
- ar(cereal::make_nvp("col", col));
- ar(cereal::make_nvp("value", value));
-
- t = Eigen::Triplet<Scalar, Index>(row, col, value);
-}
-
-template <class Archive, typename _Scalar, int _Options, typename _Index>
-void save(Archive& ar,
- const Eigen::SparseMatrix<_Scalar, _Options, _Index>& m)
-{
- _Index inner_size = m.innerSize();
- _Index outer_size = m.outerSize();
-
- using Triplet = Eigen::Triplet<_Scalar>;
- std::vector<Triplet> triplets;
-
- for (_Index i=0; i < outer_size; ++i)
- for (typename Eigen::SparseMatrix<_Scalar, _Options, _Index>::InnerIterator it(m,i); it; ++it)
- triplets.emplace_back( it.row(), it.col(), it.value() );
-
- ar(cereal::make_nvp("inner_size", inner_size));
- ar(cereal::make_nvp("outer_size", outer_size));
- ar(cereal::make_nvp("triplets", triplets));
-}
-
-template <class Archive, typename _Scalar, int _Options, typename _Index>
-void load(Archive& ar,
- Eigen::SparseMatrix<_Scalar, _Options, _Index>& m)
-{
- _Index inner_size;
- _Index outer_size;
-
- ar(cereal::make_nvp("inner_size", inner_size));
- ar(cereal::make_nvp("outer_size", outer_size));
-
- _Index rows = (m.IsRowMajor)? outer_size : inner_size;
- _Index cols = (m.IsRowMajor)? inner_size : outer_size;
-
- m.resize(rows, cols);
-
- using Triplet = Eigen::Triplet<_Scalar>;
- std::vector<Triplet> triplets;
-
- ar(cereal::make_nvp("triplets", triplets));
-
- m.setFromTriplets(triplets.begin(), triplets.end());
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h
deleted file mode 100644
index b43c35673ae05e9088c045c167fa9a02a3139c8a..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_local_parametrization_base.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
-#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
-
-#include "core/state_block/local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-// Since classes deriving from LocalParametrizationBase
-// have default constructor calling the non-default
-// LocalParametrizationBase constructor with pre-defined
-// arguments, there is nothing to save here.
-template<class Archive>
-inline void serialize(
- Archive& /*ar*/,
- wolf::LocalParametrizationBase& /*lpb*/,
- std::uint32_t const /*file_version*/)
-{
- //
-}
-
-} //namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_homogeneous.h b/serialization/cereal/serialization_local_parametrization_homogeneous.h
deleted file mode 100644
index 9fcc656d5b86c51eb3995d1a11fbdcc80b1e98c4..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_local_parametrization_homogeneous.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
-#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
-
-#include "core/local_parametrization_homogeneous.h"
-
-#include "serialization_local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationHomogeneous,
- "LocalParametrizationHomogeneous");
-
-namespace cereal {
-
-template<class Archive>
-inline void serialize(
- Archive& ar,
- wolf::LocalParametrizationHomogeneous& lp,
- std::uint32_t const /*file_version*/)
-{
- ar( cereal::make_nvp("LocalParametrizationBase",
- cereal::base_class<wolf::LocalParametrizationBase>(&lp)) );
-}
-
-} //namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_quaternion.h b/serialization/cereal/serialization_local_parametrization_quaternion.h
deleted file mode 100644
index 66fe30a361f0f1878a9444d9fb487ad2a068ad0d..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_local_parametrization_quaternion.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
-#define WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
-
-#include "core/local_parametrization_quaternion.h"
-
-#include "serialization_local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationQuaternion<wolf::DQ_LOCAL>,
- "wolf_LocalParametrizationQuaternion_DQ_LOCAL")
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationQuaternion<wolf::DQ_GLOBAL>,
- "wolf_LocalParametrizationQuaternion_DQ_GLOBAL")
-
-namespace cereal {
-
-template<class Archive, unsigned int DeltaReference>
-inline void serialize(
- Archive &ar,
- wolf::LocalParametrizationQuaternion<DeltaReference> &lp,
- const unsigned int /*file_version*/)
-{
- ar( cereal::make_nvp("LocalParametrizationBase",
- cereal::base_class<wolf::LocalParametrizationBase>(&lp)) );
-}
-
-} //namespace boost
-
-#endif /* WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_ */
diff --git a/serialization/cereal/serialization_node_base.h b/serialization/cereal/serialization_node_base.h
deleted file mode 100644
index a2c592d6982485de2aebf189e09e764b68022783..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_node_base.h
+++ /dev/null
@@ -1,77 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_NODE_BASE_H_
-#define _WOLF_IO_CEREAL_NODE_BASE_H_
-
-// Wolf includes
-#include "core/node_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace wolf {
-
-struct NodeBase::Serializer {
-
- template <class Archive>
- static void serialize(Archive& ar, NodeBase& o, std::uint32_t const /*version*/)
- {
- ar( cereal::make_nvp("node_class_", o.node_category_) );
- ar( cereal::make_nvp("node_type_", o.node_type_) );
- ar( cereal::make_nvp("node_name_", o.node_name_) );
- ar( cereal::make_nvp("node_id_", o.node_id_) );
-
-// ar( cereal::make_nvp("problem_ptr_", o.problem_ptr_) );
-
- // Not sure what to do with this guy ...
- //ar( cereal::make_nvp("node_id_count_", o.node_id_count_) );
- }
-
- template <class Archive>
- static void load_and_construct( Archive& ar, cereal::construct<wolf::NodeBase>& construct,
- std::uint32_t const /*version*/ )
- {
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_class;
- decltype(std::declval<wolf::NodeBase>().getType()) nb_type;
- decltype(std::declval<wolf::NodeBase>().getName()) nb_name;
-
- ar( cereal::make_nvp("node_class_", nb_class) );
- ar( cereal::make_nvp("node_type_", nb_type) );
- ar( cereal::make_nvp("node_name_", nb_name) );
-
- construct( nb_class, nb_type, nb_name );
-
- ar( cereal::make_nvp("node_id_", construct->node_id_) );
-
-// ar( cereal::make_nvp("problem_ptr_", construct->problem_ptr_) );
-
- // Not sure what to do with this guy ...
- //ar( cereal::make_nvp("node_id_count_", construct->node_id_count_) );
- }
-};
-
-} // namespace wolf
-
-namespace cereal {
-
-/// @note No default constructor thus the need
-/// for these specializations
-template <>
-struct LoadAndConstruct<wolf::NodeBase>
-{
- template <class Archive>
- static void load_and_construct( Archive& ar,
- cereal::construct<wolf::NodeBase>& construct,
- std::uint32_t const version )
- {
- wolf::NodeBase::Serializer::load_and_construct(ar, construct, version);
- }
-};
-
-template <class Archive>
-void serialize(Archive& ar, wolf::NodeBase& o, std::uint32_t const version)
-{
- wolf::NodeBase::Serializer::serialize(ar, o, version);
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_NODE_BASE_H_ */
diff --git a/serialization/cereal/serialization_processor_odom2d_params.h b/serialization/cereal/serialization_processor_odom2d_params.h
deleted file mode 100644
index dc0416b94634ef8415919c26fa972c86edb99a00..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_processor_odom2d_params.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
-
-// Wolf includes
-#include "core/processor/processor_odom_2D.h"
-#include "serialization_processor_params_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsOdom2D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("ProcessorParamsBase",
- cereal::base_class<wolf::ProcessorParamsBase>(&o)) );
-
- ar( cereal::make_nvp("cov_det_th_", o.cov_det) );
- ar( cereal::make_nvp("dist_traveled_th_", o.dist_traveled_th_) );
- ar( cereal::make_nvp("elapsed_time_th_", o.elapsed_time_th_) );
- ar( cereal::make_nvp("theta_traveled_th_", o.theta_traveled_th_) );
- ar( cereal::make_nvp("unmeasured_perturbation_std_",
- o.unmeasured_perturbation_std) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::ProcessorParamsOdom2D, "ProcessorParamsOdom2D")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_ */
diff --git a/serialization/cereal/serialization_processor_odom3d_params.h b/serialization/cereal/serialization_processor_odom3d_params.h
deleted file mode 100644
index d2fd7c077d868e4dafcfa209c10767415f092ab8..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_processor_odom3d_params.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
-
-// Wolf includes
-#include "core/processor/processor_odom_3D.h"
-#include "serialization_processor_params_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsOdom3D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("ProcessorParamsBase",
- cereal::base_class<wolf::ProcessorParamsBase>(&o)) );
-
- ar( cereal::make_nvp("angle_turned", o.angle_turned) );
- ar( cereal::make_nvp("dist_traveled", o.dist_traveled) );
- ar( cereal::make_nvp("max_buff_length", o.max_buff_length) );
- ar( cereal::make_nvp("max_time_span", o.max_time_span) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::ProcessorParamsOdom3D, "ProcessorOdom3DParams")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_ */
diff --git a/serialization/cereal/serialization_processor_params_base.h b/serialization/cereal/serialization_processor_params_base.h
deleted file mode 100644
index 03ea158c07ea0c18516400a6c51618998efb9473..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_processor_params_base.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
-
-// Wolf includes
-#include "core/processor/processor_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsBase& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("type", o.type) );
- ar( cereal::make_nvp("name", o.name) );
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_ */
diff --git a/serialization/cereal/serialization_sensor_intrinsic_base.h b/serialization/cereal/serialization_sensor_intrinsic_base.h
deleted file mode 100644
index 86b2a9b482eb61dd290dd2d2723c41415ca11dae..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_sensor_intrinsic_base.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
-#define _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
-
-// Wolf includes
-#include "core/sensor/sensor_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::IntrinsicsBase& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("type", o.type) );
- ar( cereal::make_nvp("name", o.name) );
-}
-
-} // namespace cereal
-
-// No need to register base
-//CEREAL_REGISTER_TYPE_WITH_NAME(wolf::IntrinsicsBase, "IntrinsicsBase");
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_ */
diff --git a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
deleted file mode 100644
index 17d4160b3751d0a13f0f28836234b500e6674400..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
-#define _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
-
-// Wolf includes
-#include "core/sensor/sensor_odom_2D.h"
-
-#include "serialization_sensor_intrinsic_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::IntrinsicsOdom2D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("IntrinsicsBase",
- cereal::base_class<wolf::IntrinsicsBase>(&o)) );
-
- ar( cereal::make_nvp("k_disp_to_disp", o.k_disp_to_disp) );
- ar( cereal::make_nvp("k_rot_to_rot", o.k_rot_to_rot) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::IntrinsicsOdom2D, "IntrinsicsOdom2D")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_ */
diff --git a/serialization/cereal/serialization_time_stamp.h b/serialization/cereal/serialization_time_stamp.h
deleted file mode 100644
index f0c978d36415be7598f2ba6eb5b83bd9a5cb0a61..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_time_stamp.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_TIME_STAMP_H_
-#define _WOLF_IO_CEREAL_TIME_STAMP_H_
-
-// Wolf includes
-#include "core/time_stamp.h"
-
-#include <cereal/cereal.hpp>
-
-namespace cereal {
-
-/// @note serialization versionning raise
-/// a compile error here...
-template <class Archive>
-void save(Archive& ar, const wolf::TimeStamp& o/*, std::uint32_t const version*/)
-{
- ar( cereal::make_nvp("value", o.get()) );
-}
-
-template <class Archive>
-void load(Archive& ar, wolf::TimeStamp& o/*, std::uint32_t const version*/)
-{
- auto val = o.get();
-
- ar( cereal::make_nvp("value", val) );
-
- o.set(val);
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_TIME_STAMP_H_ */
diff --git a/src/association/association_nnls.cpp b/src/association/association_nnls.cpp
deleted file mode 100644
index c1f9d97e7021901928b743270ff565c2fc2c5f3c..0000000000000000000000000000000000000000
--- a/src/association/association_nnls.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "core/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 cf8cd45243fab361d5053cfde96f0719cd082c45..0000000000000000000000000000000000000000
--- a/src/association/association_node.cpp
+++ /dev/null
@@ -1,195 +0,0 @@
-
-#include "core/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 8b277602114a453a3483db6bd110a9dd7e369430..0000000000000000000000000000000000000000
--- a/src/association/association_solver.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-
-#include "core/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 94d02cab70b693fd194f700893314559c5257dba..0000000000000000000000000000000000000000
--- a/src/association/association_tree.cpp
+++ /dev/null
@@ -1,186 +0,0 @@
-
-#include "core/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/capture_base.cpp b/src/capture/capture_base.cpp
index 78904aa8baac91d47d2300454b8a77b2f1160001..546fafeec33538915a23ed5e96a98400cebd58c4 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -14,87 +14,99 @@ CaptureBase::CaptureBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
NodeBase("CAPTURE", _type),
+ HasStateBlocks(""),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
- state_block_vec_(3),
- calib_size_(0),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
{
+ assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!");
if (_sensor_ptr)
{
-
- if (_sensor_ptr->isExtrinsicDynamic())
+ if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
{
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;
+ addStateBlock('P', _p_ptr, nullptr);
}
- else if (_p_ptr || _o_ptr)
+ else
+ assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
+
+ if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
{
- WOLF_ERROR("Provided dynamic sensor extrinsics but the sensor extrinsics are static");
+ assert(_o_ptr && "Pointer to dynamic orientation params is null!");
+ addStateBlock('O', _o_ptr, nullptr);
}
+ else
+ assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
- if (_sensor_ptr->isIntrinsicDynamic())
+ if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
{
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");
+ addStateBlock('I', _intr_ptr, nullptr);
}
+ else
+ assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
- getSensor()->setHasCapture();
}
else if (_p_ptr || _o_ptr || _intr_ptr)
{
WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
- updateCalibSize();
-
-// WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
}
CaptureBase::~CaptureBase()
{
- removeStateBlocks();
}
-void CaptureBase::remove()
+void CaptureBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
- // Remove State Blocks
- removeStateBlocks();
+ // SensorBase::last_capture_
+ if (getSensor() and getSensor()->getLastCapture() == this_C)
+ getSensor()->updateLastCapture();
- // remove from upstream
- FrameBasePtr F = frame_ptr_.lock();
- if (F)
+ // remove downstream
+ while (!constrained_by_list_.empty())
{
- F->getCaptureList().remove(this_C);
- if (F->getCaptureList().empty() && F->getConstrainedByList().empty())
- F->remove(); // remove upstream
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
}
-
- // remove downstream
while (!feature_list_.empty())
{
- feature_list_.front()->remove(); // remove downstream
+ feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
- while (!constrained_by_list_.empty())
+
+ // Remove State Blocks
+ removeStateBlocks(getProblem());
+
+ // remove from upstream
+ FrameBasePtr F = frame_ptr_.lock();
+ if (F)
{
- constrained_by_list_.front()->remove(); // remove constrained by
+ F->removeCapture(this_C);
+ if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
+ F->remove(viral_remove_empty_parent); // remove upstream
}
}
}
+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());
+}
+
+
+
FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
@@ -102,19 +114,19 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
return _ft_ptr;
}
-void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
+void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
{
- for (FeatureBasePtr feature_ptr : _new_ft_list)
- {
- feature_ptr->setCapture(shared_from_this());
- if (getProblem() != nullptr)
- feature_ptr->setProblem(getProblem());
- // feature_list_.push_back(feature_ptr);
- }
- feature_list_.splice(feature_list_.end(), _new_ft_list);
+ feature_list_.remove(_ft_ptr);
+}
+
+FactorBasePtrList CaptureBase::getFactorList() const
+{
+ FactorBasePtrList fac_list;
+ getFactorList(fac_list);
+ return fac_list;
}
-void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
+void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const
{
for (auto f_ptr : getFeatureList())
f_ptr->getFactorList(_fac_list);
@@ -123,189 +135,266 @@ void CaptureBase::getFactorList(FactorBasePtrList& _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
+void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
- 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);
+ constrained_by_list_.remove(_fac_ptr);
+}
- 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];
- }
+bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
}
-void CaptureBase::removeStateBlocks()
+
+StateBlockPtr CaptureBase::getStateBlock(const char& _key) const
{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ if (getSensor() and getSensor()->hasStateBlock(_key))
{
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlock(i, nullptr);
- }
+ if (getSensor()->isStateBlockDynamic(_key))
+ return HasStateBlocks::getStateBlock(_key);
+ else
+ return getSensor()->getStateBlock(_key);
}
+ else // No sensor associated, or sensor without this state block: assume sensor params are here
+ return HasStateBlocks::getStateBlock(_key);
}
void CaptureBase::fix()
{
- for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
+ HasStateBlocks::fix();
}
void CaptureBase::unfix()
{
- for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
+ HasStateBlocks::unfix();
}
-void CaptureBase::fixExtrinsics()
+void CaptureBase::move(FrameBasePtr _frm_ptr)
{
- for (unsigned int i = 0; i < 2; i++)
+ WOLF_WARN_COND(this->getFrame() == nullptr, "Moving Capture ", id(), " at ts=", getTimeStamp(), " not linked to any frame. Consider just linking it with link() instead of move()!");
+
+ assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!");
+
+ // Unlink
+ if (this->getFrame())
{
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
+ // unlink from previous non-key frame
+ this->getFrame()->removeCapture(shared_from_this());
+ this->setFrame(nullptr);
}
- updateCalibSize();
+
+ // link
+ link(_frm_ptr);
}
-void CaptureBase::unfixExtrinsics()
+
+void CaptureBase::link(FrameBasePtr _frm_ptr)
{
- for (unsigned int i = 0; i < 2; i++)
+ assert(!is_removing_ && "linking a removed capture");
+ assert(this->getFrame() == nullptr && "linking a capture already linked");
+
+ if(_frm_ptr)
+ {
+
+ _frm_ptr->addCapture(shared_from_this());
+ this->setFrame(_frm_ptr);
+ this->setProblem(_frm_ptr->getProblem());
+ }
+ else
{
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
+ WOLF_WARN("Linking Capture ", id(), " to a nullptr");
}
- updateCalibSize();
}
-void CaptureBase::fixIntrinsics()
+void CaptureBase::setProblem(ProblemPtr _problem)
{
- for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ assert(getFrame() && "Cannot set problem: Capture has no Frame!");
+
+ NodeBase::setProblem(_problem);
+ registerNewStateBlocks(_problem);
+
+ // update SensorBase::last_capture_
+ if (getSensor() and
+ (not getSensor()->getLastCapture() or
+ getSensor()->getLastCapture()->getTimeStamp() < time_stamp_))
+ getSensor()->setLastCapture(shared_from_this());
+
+ for (auto ft : feature_list_)
+ ft->setProblem(_problem);
}
-void CaptureBase::unfixIntrinsics()
+void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
+ _stream << _tabs << "Cap" << id()
+ << " " << getType()
+ << " ts = " << std::setprecision(3) << getTimeStamp();
+
+ if(getSensor() != nullptr)
{
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
+ _stream << " -> Sen" << getSensor()->id();
}
- updateCalibSize();
-}
+ else
+ _stream << " -> Sen-";
-void CaptureBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
+ _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ if (_constr_by)
{
- for (auto sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
+ _stream << std::endl;
+
+ if (getStateBlockMap().size() > 0)
+ printState(_metric, _state_blocks, _stream, _tabs);
}
-SizeEigen CaptureBase::computeCalibSize() const
+void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) 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;
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 3)
+ for (auto f : getFeatureList())
+ if (f)
+ f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-Eigen::VectorXs CaptureBase::getCalibration() const
+CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
{
- Eigen::VectorXs calib(calib_size_);
- SizeEigen index = 0;
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
{
- auto sb = getStateBlock(i);
- if (sb && !sb->isFixed())
+ _stream << _tabs << "Cap" << id() << " @ " << _cap_ptr.get() << " -> Sen";
+ if (getSensor()) _stream << getSensor()->id();
+ else _stream << "-";
+ _stream << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Frm" << getFrame()->id() << " @ " << getFrame().get() << std::endl;
+ }
+ for (auto pair: getStateBlockMap())
+ {
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Capture " << id() << " @ " << _cap_ptr.get() << " has State block pointer "
+ << sb.get() << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose)
{
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
}
}
- 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 frm_ptr = getFrame();
+ // check problem and frame pointers
+ inconsistency_explanation << "Capture problem pointer " << getProblem().get()
+ << " different from frame problem pointer " << frm_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == frm_ptr->getProblem()), inconsistency_explanation);
+
+
+ // check contrained_by
+ for (const auto& cby : getConstrainedByList())
{
- auto sb = getStateBlock(i);
- if (sb && !sb->isFixed())
+ if (_verbose)
{
- sb->setState(_calib.segment(index, sb->getSize()));
- index += sb->getSize();
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& Cow : cby->getCaptureOtherList())
+ _stream << " Cap" << Cow.lock()->id();
+ _stream << std::endl;
}
- }
-}
-void CaptureBase::link(FrameBasePtr _frm_ptr)
-{
- if(_frm_ptr)
- {
- _frm_ptr->addCapture(shared_from_this());
- this->setFrame(_frm_ptr);
- this->setProblem(_frm_ptr->getProblem());
- this->registerNewStateBlocks();
- }
- else
- {
- WOLF_WARN("Linking with a nullptr");
+ // check constrained_by pointer to this capture
+ inconsistency_explanation << "constrained by capture " << id() << " @ " << _cap_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasCaptureOther(_cap_ptr)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
}
+
+ auto frm_cap = _cap_ptr->getFrame();
+ inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
+ << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
+ << " -X-> Frm" << id() << "\n";
+ auto frm_cap_list = frm_cap->getCaptureList();
+ auto frame_has_cap = std::find_if(frm_cap_list.begin(), frm_cap_list.end(), [&_cap_ptr](CaptureBasePtr cap){ return cap == _cap_ptr;});
+ log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
+
+ for(auto f : getFeatureList())
+ {
+ inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+ << " ---> Ftr" << f->id() << " @ " << f
+ << " -X-> Cap" << id();
+ log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
+ }
+ //Check that the capture is within time tolerance of some processor
+ auto frame = getFrame();
+ double time_diff = fabs(getTimeStamp() - frame->getTimeStamp());
+
+ //It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
+ //This inicialization is needed because if the list empty the execution does not go into the loop and the
+ //assertion fails
+ bool match_any_prc_timetolerance;
+ if(getSensor() != nullptr )
+ {
+ match_any_prc_timetolerance = getSensor()->getProcessorList().empty();
+ for(auto const& prc : getSensor()->getProcessorList())
+ {
+ match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
+ }
+ inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+ << " ts =" << getTimeStamp() << "Frm" << frame->id()
+ << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of"
+ << " any processor in sensor " << getSensor()->id() << "\n";
+ log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation);
+ }
+
+ return log;
}
+bool CaptureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto sen_ptr = std::static_pointer_cast<CaptureBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for(auto f : getFeatureList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
-
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7a09336470bfefeaf304cfbb817a8793c4c9a82d
--- /dev/null
+++ b/src/capture/capture_diff_drive.cpp
@@ -0,0 +1,29 @@
+
+
+#include "core/capture/capture_diff_drive.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr) :
+ CaptureMotion("CaptureDiffDrive",
+ _ts,
+ _sensor_ptr,
+ _data,
+ _data_cov,
+ _capture_origin_ptr,
+ _p_ptr,
+ _o_ptr,
+ _intr_ptr)
+{
+ //
+}
+
+} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index be77087388afc8a3ae05eb85333b54934bb2e90e..b87e4fb10c37d79ede036e72983faea5d7d12b7c 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -1,5 +1,5 @@
/**
- * \file capture_motion2.cpp
+ * \file capture_motion.cpp
*
* Created on: Oct 14, 2017
* \author: jsola
@@ -13,15 +13,13 @@ 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) :
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_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)
+ data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXd::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
+ buffer_(),
+ capture_origin_ptr_(_capture_origin_ptr)
{
//
}
@@ -29,19 +27,17 @@ CaptureMotion::CaptureMotion(const std::string & _type,
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,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_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)
+ buffer_(),
+ capture_origin_ptr_(_capture_origin_ptr)
{
//
}
@@ -51,25 +47,76 @@ CaptureMotion::~CaptureMotion()
//
}
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
+bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance)
{
- 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;
+ assert(_ts.ok() and this->time_stamp_.ok());
+
+ // the same capture is within tolerance
+ if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance)
+ return true;
+
+ // buffer is empty, and the capture is not within tolerance
+ if (this->getBuffer().empty())
+ return false;
+
+ // buffer encloses timestamp, if ts is:
+ // from : buffer.first.ts - tt
+ // to : buffer.last.ts + tt
+ if (_ts >= this->getBuffer().front().ts_ - _time_tolerance and
+ _ts <= this->getBuffer().back().ts_ + _time_tolerance)
+ return true;
+
+ // not found anywhere
+ return false;
}
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts)
+
+
+
+void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- 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;
-}
+ _stream << _tabs << "CapM" << id()
+ << " " << getType()
+ << " ts = " << std::setprecision(3) << getTimeStamp();
+
+ if(getSensor() != nullptr)
+ {
+ _stream << " -> Sen" << getSensor()->id();
+ }
+ else
+ _stream << " -> Sen-";
+
+ if (auto OC = getOriginCapture())
+ {
+ _stream << " -> Origin: Cap" << OC->id();
+ if (auto OF = OC->getFrame())
+ _stream << " ; Frm" << OF->id();
+ }
+
+ _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ if (_constr_by)
+ {
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+ if (getStateBlockMap().size() > 0)
+ printState(_metric, _state_blocks, _stream, _tabs);
+
+ _stream << _tabs << " " << "buffer size : " << getBuffer().size() << std::endl;
+ if ( _metric && ! getBuffer().empty())
+ {
+ _stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
+// if (hasCalibration())
+// {
+// _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
+// _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
+// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
+// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+// }
+ }
+
+}
}
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
deleted file mode 100644
index 1e073b2810e92eaa7c19affe4dc0bb4ad9ef74fe..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 "core/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_2d.cpp b/src/capture/capture_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4755b6fe067fcbcd81c3ae66ecb4be6b562078e7
--- /dev/null
+++ b/src/capture/capture_odom_2d.cpp
@@ -0,0 +1,37 @@
+/*
+ * capture_odom_2d.cpp
+ *
+ * Created on: Oct 16, 2017
+ * Author: jsola
+ */
+
+#include "core/capture/capture_odom_2d.h"
+
+namespace wolf
+{
+
+CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_ptr):
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
+{
+ //
+}
+
+CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr):
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_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 f456b992b2b4136d74690d1cafb8676dabe5762f..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 "core/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_odom_3d.cpp b/src/capture/capture_odom_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a692011ffc2b61c0d46e9f024d39b354106feeb7
--- /dev/null
+++ b/src/capture/capture_odom_3d.cpp
@@ -0,0 +1,38 @@
+/*
+ * capture_odom_3d.cpp
+ *
+ * Created on: Oct 16, 2017
+ * Author: jsola
+ */
+
+#include "core/capture/capture_odom_3d.h"
+
+namespace wolf
+{
+
+CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_ptr):
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
+{
+ //
+}
+
+CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr):
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
+{
+ //
+}
+
+CaptureOdom3d::~CaptureOdom3d()
+{
+ //
+}
+
+} /* namespace wolf */
+
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 46177687a1cbaab1a507ba3ac4e091aec7d9b7fd..190fa7090c698b4a613d0ebba2594e391043f925 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -2,8 +2,8 @@
namespace wolf{
-CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase("POSE", _ts, _sensor_ptr),
+CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance) :
+ CaptureBase("CapturePose", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
{
@@ -25,11 +25,11 @@ void CapturePose::emplaceFeatureAndFactor()
// std::cout << data_.size() << " ~~ " << data_covariance_.rows() << "x" << data_covariance_.cols() << std::endl;
// Emplace factor
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
- FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose);
+ FactorBase::emplace<FactorPose2d>(feature_pose, feature_pose, nullptr, false);
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
- FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose);
+ FactorBase::emplace<FactorPose3d>(feature_pose, feature_pose, nullptr, false);
else
- throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
+ 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 0785e8bae4918c1f8406bc379123dd56f4120730..0000000000000000000000000000000000000000
--- a/src/capture/capture_velocity.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-#include "core/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
index cdc8ce156ab6f7309a28cc3e5c8551dd7bea1d12..dcce03c9dc66e1dc38c62fa87dcdfdd4a25b528e 100644
--- a/src/capture/capture_void.cpp
+++ b/src/capture/capture_void.cpp
@@ -3,7 +3,7 @@
namespace wolf {
CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) :
- CaptureBase("VOID", _ts, _sensor_ptr)
+ CaptureBase("CaptureVoid", _ts, _sensor_ptr)
{
//
}
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
deleted file mode 100644
index 10532e397c53455e8d9893cc286ec3ef5797f803..0000000000000000000000000000000000000000
--- a/src/capture/capture_wheel_joint_position.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-#include "core/capture/capture_wheel_joint_position.h"
-#include "core/sensor/sensor_diff_drive.h"
-#include "core/math/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 681f3d59ee5e3f674d277182790d9247ac41aa4c..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ /dev/null
@@ -1,449 +0,0 @@
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
-#include "core/trajectory/trajectory_base.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_base.h"
-#include "core/utils/make_unique.h"
-
-namespace wolf {
-
-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);
-}
-
-SolverManagerPtr CeresManager::create(const ProblemPtr &wolf_problem, const paramsServer &_server)
-{
- ceres::Solver::Options opt;
- opt.max_num_iterations = _server.getParam<int>("max_num_iterations","1000");
- // CeresManager m = CeresManager(wolf_problem, opt);
- return std::make_shared<CeresManager>(wolf_problem, opt);
-}
-
-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->isKeyOrAux())
- 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->isKeyOrAux())
- 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->getLocalSize(),state_block_pairs[i].second->getLocalSize());
- covariance_->GetCovarianceBlockInTangentSpace(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 << "covariance got switch: " << std::endl << cov << std::endl;
- }
- }
- else
- std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
-}
-void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& 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++){
- if (*st_it1 == nullptr){
- continue;
- }
- for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
- {
- if (*st_it2 == nullptr){
- continue;
- }
- 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->getLocalSize(),state_block_pairs[i].second->getLocalSize());
- covariance_->GetCovarianceBlockInTangentSpace(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 << "covariance got from st_list: " << 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)
-{
- 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);
- 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
-#include "core/solver/solver_factory.h"
-namespace wolf {
- WOLF_REGISTER_SOLVER(CeresManager)
-} // namespace wolf
-
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp
index 4e92e85a4f6400b36b94c82d30955a6cb4b23222..1cb1425c08c761a56ad615a63f9e26786c37600a 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.cpp
+++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp
@@ -4,16 +4,16 @@ 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());
+ Eigen::Map<const Eigen::VectorXd> x_raw_map((double*)x_raw, GlobalSize());
+ Eigen::Map<const Eigen::VectorXd> delta_raw_map((double*)delta_raw, LocalSize());
+ Eigen::Map<Eigen::VectorXd> x_plus_map((double*)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());
+ Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize());
+ Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize());
return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map);
};
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index f2e40fac0122c6eb1599cbe27a62659f439a89f0..0d7746f929f427d3e2bc9f6bcce53e4147b7924d 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -37,7 +37,7 @@ std::string QRManager::solve(const unsigned int& _report_level)
return std::string("decomposition failed\n");
// Solve
- Eigen::VectorXs x_incr = solver_.solve(-b_);
+ Eigen::VectorXd x_incr = solver_.solve(-b_);
b_.setZero();
// update state blocks
@@ -64,18 +64,18 @@ void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
computeDecomposition();
// std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
-// std::cout << Eigen::MatrixXs(solver_.matrixR()) << std::endl;
+// std::cout << Eigen::MatrixXd(solver_.matrixR()) << std::endl;
covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols()));
- Eigen::SparseMatrix<Scalar, Eigen::ColMajor> I(A_.cols(),A_.cols());
+ Eigen::SparseMatrix<double, 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();
+ Eigen::SparseMatrix<double, Eigen::ColMajor> iR = covariance_solver_.solve(I);
+ Eigen::MatrixXd 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 << "A' A = \n" << Eigen::MatrixXd(A_.transpose() * A_)<< std::endl;
+// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXd(P * iR * iR.transpose() * P.transpose()) << std::endl;
+// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXd(Sigma_full * A_.transpose() * A_) << std::endl;
// std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl;
// std::cout << "Sigma = \n" << Sigma_full << std::endl;
@@ -205,15 +205,15 @@ void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
{
// evaluate factor
- std::vector<const Scalar*> fac_states_ptr;
+ std::vector<const double*> 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;
+ Eigen::VectorXd residual(_fac_ptr->getSize());
+ std::vector<Eigen::MatrixXd> jacobians;
_fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
// Fill jacobians
- Eigen::SparseMatrixs A_block_row(_fac_ptr->getSize(), A_.cols());
+ Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols());
for (auto i = 0; i < jacobians.size(); i++)
if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
{
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5914b40a8bc67e44bbc392b146766331e344ced7
--- /dev/null
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -0,0 +1,804 @@
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
+#include "core/ceres_wrapper/cost_function_wrapper.h"
+#include "core/ceres_wrapper/iteration_update_callback.h"
+#include "core/ceres_wrapper/local_parametrization_wrapper.h"
+#include "core/trajectory/trajectory_base.h"
+#include "core/map/map_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/utils/make_unique.h"
+
+namespace wolf
+{
+
+SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) :
+ SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>())
+{
+}
+
+SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
+ const ParamsCeresPtr& _params)
+ : SolverManager(_wolf_problem, _params)
+ , params_ceres_(_params)
+{
+ covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
+ ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
+
+ if (params_ceres_->update_immediately)
+ getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
+ params_ceres_->min_num_iterations,
+ params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
+}
+
+SolverCeres::~SolverCeres()
+{
+ while (!fac_2_residual_idx_.empty())
+ removeFactorDerived(fac_2_residual_idx_.begin()->first);
+
+ while (!state_blocks_.empty())
+ {
+ removeStateBlockDerived(state_blocks_.begin()->first);
+ state_blocks_.erase(state_blocks_.begin());
+ }
+
+ while (!getSolverOptions().callbacks.empty())
+ {
+ delete getSolverOptions().callbacks.back();
+ getSolverOptions().callbacks.pop_back();
+ }
+}
+
+std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
+{
+ // run Ceres Solver
+ ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
+
+ /*/ if termination by user (Iteration callback, update and solve again)
+ // solve until max iterations reached
+ if (params_ceres_->update_immediately)
+ {
+ auto max_num_iterations = getSolverOptions().max_num_iterations;
+ while (summary_.termination_type == ceres::USER_SUCCESS)
+ {
+ // decrease wasted iterations
+ getSolverOptions().max_num_iterations -= summary_.iterations.size();
+ if (getSolverOptions().max_num_iterations <= 0)
+ break;
+
+ // update and solve again
+ update();
+ ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
+ }
+ getSolverOptions().max_num_iterations = max_num_iterations;
+ }*/
+ std::string report;
+
+ //return report
+ if (report_level == ReportVerbosity::BRIEF)
+ report = summary_.BriefReport();
+ else if (report_level == ReportVerbosity::FULL)
+ report = summary_.FullReport();
+
+ return report;
+}
+
+bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks)
+{
+ // update problem
+ update();
+
+ // 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())
+ for (const auto& key : fr_ptr->getStructure())
+ {
+ const auto& sb = fr_ptr->getStateBlock(key);
+ all_state_blocks.push_back(sb);
+ }
+
+ // landmark state blocks
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
+ {
+ 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++)
+ {
+ assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
+ for (unsigned int j = i; j < all_state_blocks.size(); j++)
+ {
+ assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
+
+ state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
+ getAssociatedMemBlockPtr(all_state_blocks[j]));
+ }
+ }
+ break;
+ }
+ case CovarianceBlocksToBeComputed::ALL_MARGINALS:
+ {
+ // first create a vector containing all state blocks
+ for(auto fr_ptr : *wolf_problem_->getTrajectory())
+ for (const auto& key1 : wolf_problem_->getFrameStructure())
+ {
+ const auto& sb1 = fr_ptr->getStateBlock(key1);
+ assert(isStateBlockRegisteredDerived(sb1));
+
+ for (const auto& key2 : wolf_problem_->getFrameStructure())
+ {
+ const auto& sb2 = fr_ptr->getStateBlock(key2);
+ assert(isStateBlockRegisteredDerived(sb2));
+
+ state_block_pairs.emplace_back(sb1, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+ if (sb1 == sb2)
+ break;
+ }
+ }
+
+ // landmark state blocks
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
+ for (auto sb : l_ptr->getUsedStateBlockVec())
+ {
+ assert(isStateBlockRegisteredDerived(sb));
+ for(auto sb2 : l_ptr->getUsedStateBlockVec())
+ {
+ assert(isStateBlockRegisteredDerived(sb2));
+ state_block_pairs.emplace_back(sb, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+ if (sb == sb2)
+ break;
+ }
+ }
+ // // loop all marginals (PO marginals)
+ // for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
+ // {
+ // 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_->getLastFrame();
+
+ assert(isStateBlockRegisteredDerived(last_key_frame->getP()));
+ assert(isStateBlockRegisteredDerived(last_key_frame->getO()));
+
+ state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
+ state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
+ 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++)
+ {
+ assert(isStateBlockRegisteredDerived(*state_it));
+
+ // robot - landmark
+ state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
+ state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
+ 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++)
+ {
+ assert(isStateBlockRegisteredDerived(*next_state_it));
+
+ state_block_pairs.emplace_back(*state_it, *next_state_it);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
+ getAssociatedMemBlockPtr((*next_state_it)));
+ }
+ }
+ }
+ break;
+ }
+ case CovarianceBlocksToBeComputed::GAUSS:
+ {
+ // State blocks:
+ // - Last KF: PV
+
+ // last KF
+ FrameBasePtr frame = wolf_problem_->getLastFrame();
+ if (not frame)
+ return false;
+
+ while (frame)
+ {
+ if (frame->hasStateBlock('P') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ frame->hasStateBlock('V') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('V')))
+ {
+ break;
+ }
+ //else
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ frame = frame->getPreviousFrame();
+ }
+ if (not frame)
+ {
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and V, returning...");
+ return false;
+ }
+
+ // only marginals of P and V
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
+ "\nP @", frame->getStateBlock('P'),
+ "\nV @", frame->getStateBlock('V'));
+ state_block_pairs.emplace_back(frame->getStateBlock('P'),
+ frame->getStateBlock('P'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('P')));
+ state_block_pairs.emplace_back(frame->getStateBlock('V'),
+ frame->getStateBlock('V'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('V')));
+
+ break;
+ }
+ case CovarianceBlocksToBeComputed::GAUSS_TUCAN:
+ {
+ // State blocks:
+ // - Last KF: PV
+
+ // last KF
+ FrameBasePtr frame = wolf_problem_->getLastFrame();
+ if (not frame)
+ return false;
+
+ StateBlockPtr sb_gnss_T;
+ while (frame)
+ {
+ // get capture gnss
+ for (CaptureBasePtr cap : frame->getCaptureList())
+ if (cap->hasStateBlock('T'))
+ {
+ sb_gnss_T = cap->getStateBlock('T');
+ break;
+ }
+
+ if (frame->hasStateBlock('P') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ frame->hasStateBlock('O') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('O')) and
+ frame->hasStateBlock('V') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('V')) and
+ sb_gnss_T and
+ isStateBlockRegisteredDerived(sb_gnss_T))
+ {
+ break;
+ }
+ // else
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ frame = frame->getPreviousFrame();
+ }
+ if (not frame)
+ {
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, O, V and CaptureGnss with T, returning...");
+ return false;
+ }
+
+ // only marginals of P, O, V and T
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
+ "\nP @", frame->getStateBlock('P'),
+ "\nO @", frame->getStateBlock('O'),
+ "\nV @", frame->getStateBlock('V'),
+ "\nT @", sb_gnss_T);
+ state_block_pairs.emplace_back(frame->getStateBlock('P'),
+ frame->getStateBlock('P'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('P')));
+ state_block_pairs.emplace_back(frame->getStateBlock('O'),
+ frame->getStateBlock('O'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('O')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('O')));
+ state_block_pairs.emplace_back(frame->getStateBlock('V'),
+ frame->getStateBlock('V'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('V')));
+ state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
+ getAssociatedMemBlockPtr(sb_gnss_T));
+
+ break;
+ }
+ case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION:
+ {
+ // State blocks:
+ // - Last KF: PV
+
+ // last KF
+ FrameBasePtr frame = wolf_problem_->getLastFrame();
+ if (not frame)
+ return false;
+
+ StateBlockPtr sb_gnss_T;
+ while (frame)
+ {
+ // get capture gnss
+ for (CaptureBasePtr cap : frame->getCaptureList())
+ if (cap->hasStateBlock('T'))
+ {
+ sb_gnss_T = cap->getStateBlock('T');
+ break;
+ }
+
+ if (frame->hasStateBlock('P') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ sb_gnss_T and
+ isStateBlockRegisteredDerived(sb_gnss_T))
+ {
+ break;
+ }
+ // else
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ frame = frame->getPreviousFrame();
+ }
+ if (not frame)
+ {
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and CaptureGnss with T, returning...");
+ return false;
+ }
+
+ // only marginals of P and T
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
+ "\nP @", frame->getStateBlock('P'),
+ "\nT @", sb_gnss_T);
+ state_block_pairs.emplace_back(frame->getStateBlock('P'),
+ frame->getStateBlock('P'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('P')));
+ state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
+ getAssociatedMemBlockPtr(sb_gnss_T));
+
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
+
+ }
+ //std::cout << "pairs... " << double_pairs.size() << std::endl;
+
+ // COMPUTE DESIRED COVARIANCES
+ if (covariance_->Compute(double_pairs, ceres_problem_.get()))
+ {
+ // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+ wolf_problem_->clearCovariance();
+
+ // STORE DESIRED COVARIANCES
+ for (unsigned int i = 0; i < double_pairs.size(); i++)
+ {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+ covariance_->GetCovarianceBlockInTangentSpace(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 << "covariance got switch: " << std::endl << cov << std::endl;
+ }
+ return true;
+ }
+
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
+ return false;
+}
+
+bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list)
+{
+ //std::cout << "SolverCeres: computing covariances..." << std::endl;
+
+ // update problem
+ update();
+
+ // 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++)
+ {
+ if (*st_it1 == nullptr){
+ continue;
+ }
+ assert(isStateBlockRegisteredDerived(*st_it1));
+
+ for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
+ {
+ if (*st_it2 == nullptr){
+ continue;
+ }
+ assert(isStateBlockRegisteredDerived(*st_it2));
+
+ state_block_pairs.emplace_back(*st_it1, *st_it2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
+ getAssociatedMemBlockPtr((*st_it2)));
+ }
+ }
+
+ //std::cout << "pairs... " << double_pairs.size() << std::endl;
+
+ // COMPUTE DESIRED COVARIANCES
+ if (covariance_->Compute(double_pairs, ceres_problem_.get()))
+ {
+ // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+ wolf_problem_->clearCovariance();
+
+ // STORE DESIRED COVARIANCES
+ for (unsigned int i = 0; i < double_pairs.size(); i++)
+ {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+ covariance_->GetCovarianceBlockInTangentSpace(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 << "covariance got from st_list: " << std::endl << cov << std::endl;
+ }
+ return true;
+ }
+
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
+ return false;
+}
+
+void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
+{
+ assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map");
+
+ auto cost_func_ptr = createCostFunction(fac_ptr);
+ fac_2_costfunction_[fac_ptr] = cost_func_ptr;
+
+ std::vector<double*> res_block_mem;
+ res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
+ for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
+ {
+ assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb");
+ res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
+ }
+
+ 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 SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr)
+{
+ assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
+
+ 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 SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
+{
+ assert(state_ptr);
+ assert(!isStateBlockRegisteredDerived(state_ptr) && "adding a state block already added");
+ assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block");
+
+ ceres::LocalParameterization* local_parametrization_ptr = nullptr;
+
+ if (state_ptr->hasLocalParametrization())
+ {
+ 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));
+
+ updateStateBlockStatusDerived(state_ptr);
+}
+
+void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
+{
+ //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
+ assert(state_ptr);
+ assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres");
+
+ ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+ state_blocks_local_param_.erase(state_ptr);
+}
+
+void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
+{
+ assert(state_ptr != nullptr);
+ assert(isStateBlockRegisteredDerived(state_ptr) && "updating status of a state block that is not in ceres");
+
+ if (state_ptr->isFixed())
+ ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
+ else
+ ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
+}
+
+void SolverCeres::updateStateBlockLocalParametrizationDerived(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)
+ removeFactorDerived(fac);
+
+ // Remove state block (it removes all involved residual blocks but they just were removed)
+ removeStateBlockDerived(state_ptr);
+
+ // Add state block
+ addStateBlockDerived(state_ptr);
+
+ // Add all involved factors
+ for (auto fac : involved_factors)
+ addFactorDerived(fac);
+}
+
+bool SolverCeres::hasConverged()
+{
+ return summary_.termination_type == ceres::CONVERGENCE;
+}
+
+SizeStd SolverCeres::iterations()
+{
+ return summary_.iterations.size();
+}
+
+double SolverCeres::initialCost()
+{
+ return double(summary_.initial_cost);
+}
+
+double SolverCeres::finalCost()
+{
+ return double(summary_.final_cost);
+}
+
+double SolverCeres::totalTime()
+{
+ return double(summary_.total_time_in_seconds);
+}
+
+ceres::CostFunctionPtr SolverCeres::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!" );
+}
+
+bool SolverCeres::checkDerived(std::string prefix) const
+{
+ bool ok = true;
+
+ // Check numbers
+ if (ceres_problem_->NumResidualBlocks() != factors_.size() or
+ ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or
+ ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
+ {
+ WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size());
+ ok = false;
+ }
+ if (ceres_problem_->NumParameterBlocks() != state_blocks_.size())
+ {
+ WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix);
+ ok = false;
+ }
+
+ // Check parameter blocks
+ //for (auto&& state_block_pair : state_blocks_)
+ for (const auto& state_block_pair : state_blocks_)
+ {
+ if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data()))
+ {
+ WOLF_ERROR("SolverCeres::check: any state block is missing in ceres problem - in ", prefix);
+ ok = false;
+ }
+ }
+
+ // Check residual blocks
+ for (const auto& fac_res_pair : fac_2_residual_idx_)
+ {
+ // SolverManager::factors_
+ if (factors_.count(fac_res_pair.first) == 0)
+ {
+ WOLF_ERROR("SolverCeres::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
+ ok = false;
+ }
+
+ // costfunction - residual
+ if (fac_2_costfunction_.count(fac_res_pair.first) == 0)
+ {
+ WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
+ ok = false;
+ }
+ //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ {
+ WOLF_ERROR("SolverCeres::check: fac_2_costfunction_ and ceres mismatch - in ", prefix);
+ ok = false;
+ }
+
+ // factor - residual
+ if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor())
+ {
+ WOLF_ERROR("SolverCeres::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix);
+ ok = false;
+ }
+
+ // parameter blocks - state blocks
+ std::vector<double*> param_blocks;
+ ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks);
+ if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size())
+ {
+ WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix);
+ ok = false;
+ }
+ else
+ {
+ auto i = 0;
+ for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
+ {
+ if (getAssociatedMemBlockPtr(st) != param_blocks[i])
+ {
+ WOLF_ERROR("SolverCeres::check: parameter", i, " mismatch - in ", prefix);
+ ok = false;
+ }
+ i++;
+ }
+ }
+ }
+ return ok;
+}
+
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& 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();
+}
+
+const Eigen::SparseMatrixd SolverCeres::computeHessian() const
+{
+ Eigen::SparseMatrixd H;
+ Eigen::SparseMatrixd A;
+ // fac_2_residual_idx_
+ // fac_2_costfunction_
+ // state_blocks_local_param_
+ int ix_row = 0; // index of the factor/measurement in the
+ for (auto fac_res_pair: fac_2_residual_idx_){
+ FactorBasePtr fac_ptr = fac_res_pair.first;
+ ix_row += fac_ptr->getSize();
+
+ // retrieve all stateblocks data related to this factor
+ std::vector<const double*> fac_states_ptr;
+ for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){
+ fac_states_ptr.push_back(sb->getStateData());
+ }
+
+ // retrieve residual value, not used afterwards in this case since we just care about jacobians
+ Eigen::VectorXd residual(fac_ptr->getSize());
+ // retrieve jacobian in group size, not local size
+ std::vector<Eigen::MatrixXd> jacobians;
+ fac_ptr->evaluate(fac_states_ptr, residual, jacobians);
+
+ // Retrieve the block row sparse matrix of this factor
+ Eigen::SparseMatrixd A_block_row(fac_ptr->getSize(), A.cols());
+ for (auto i = 0; i < jacobians.size(); i++)
+ {
+ StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
+ if (!sb->isFixed())
+ {
+ assert(state_blocks_.count(sb) != 0 && "factor involving a state block not added");
+ assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols");
+
+ // insert since A_block_row has just been created so it's empty for sure
+ if (sb->hasLocalParametrization()){
+ // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
+ Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
+ Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
+ Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
+
+ sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
+ insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
+ }
+ else {
+ insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize());
+ }
+ }
+ }
+
+ // fill the weighted jacobian matrix block row
+ A.block(ix_row, 0, fac_ptr->getSize(), A.cols());
+
+ }
+
+ // compute the hessian matrix from the weighted jacobian
+ H = A.transpose() * A;
+
+ return H;
+}
+
+bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param)
+{
+ return state_blocks_local_param_.count(st) == 1
+ && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param
+ && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st))
+ == state_blocks_local_param_.at(st).get();
+}
+
+} // namespace wolf
+#include "core/solver/factory_solver.h"
+namespace wolf {
+ WOLF_REGISTER_SOLVER(SolverCeres)
+} // namespace wolf
+
diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp
index 36b9bb017954f818ffb7bbd1c235d1f1b88b599b..0421efa9b06aaf3f2f75796910485aa687217bba 100644
--- a/src/common/time_stamp.cpp
+++ b/src/common/time_stamp.cpp
@@ -3,8 +3,17 @@
namespace wolf {
+TimeStamp TimeStamp::Now ( )
+{
+ TimeStamp t(0);
+ t.setToNow();
+ return t;
+}
+
std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
{
+ if (!_ts.ok())
+ os << "TimeStamp is invalid! ";
os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream
os << std::setfill(' ');
return os;
@@ -13,24 +22,29 @@ std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
TimeStamp::TimeStamp() :
//time_stamp_(0)
time_stamp_nano_(0)
+ ,
+ is_valid_(false)
{
- setToNow();
+// setToNow();
}
TimeStamp::TimeStamp(const TimeStamp& _ts) :
- time_stamp_nano_(_ts.time_stamp_nano_)
+ time_stamp_nano_(_ts.time_stamp_nano_),
+ is_valid_(_ts.is_valid_)
{
//
}
-TimeStamp::TimeStamp(const Scalar& _ts) :
- time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0)
+TimeStamp::TimeStamp(const double& _ts) :
+ time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0),
+ is_valid_(_ts >= 0)
{
//
}
TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) :
- time_stamp_nano_(_sec*NANOSECS+_nsec)
+ time_stamp_nano_(_sec*NANOSECS+_nsec),
+ is_valid_(true)
{
//
}
@@ -45,25 +59,27 @@ void TimeStamp::setToNow()
timeval ts;
gettimeofday(&ts, NULL);
set(ts);
+ setOk();
}
-TimeStamp TimeStamp::operator +(const Scalar& dt) const
+TimeStamp TimeStamp::operator +(const double& dt) const
{
TimeStamp ts(*this);
ts += dt;
return ts;
}
-TimeStamp TimeStamp::operator -(const Scalar& dt) const
+TimeStamp TimeStamp::operator -(const double& dt) const
{
TimeStamp ts(*this);
ts -= dt;
return ts;
}
-inline void TimeStamp::operator -=(const Scalar& dt)
+void TimeStamp::operator -=(const double& dt)
{
unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
+ is_valid_ = (time_stamp_nano_ >= dt_nano);
time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
}
diff --git a/src/factor/CMakeLists.txt b/src/factor/CMakeLists.txt
deleted file mode 100644
index d910e757557c66ff81a85c5c8397c1aff9bb1fff..0000000000000000000000000000000000000000
--- a/src/factor/CMakeLists.txt
+++ /dev/null
@@ -1,31 +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 (OPENCV_FOUND AND Apriltag_FOUND)
- SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT}
- ${CMAKE_CURRENT_SOURCE_DIR}/constraint_autodiff_apriltag.h
- )
-ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
-
-#=========================================
-#=========================================
-
-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
index 9c8ba1ab334629db2e3dd78652884d0e54df9860..f3c300baa90528cfcca7fc604c77facf214d7a3e 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -4,10 +4,17 @@
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),
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, FactorStatus _status,
+ StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
@@ -24,43 +31,6 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
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_;
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 44e1826d0c01432018fd83366e0b54b8233a54f6..df80bf00bc0f38d08d2b9c6806a7d0ef9a01a8c7 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -7,114 +7,171 @@ 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
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ NodeBase("FACTOR", _tp),
+ feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
+ topology_(_top),
status_(_status),
apply_loss_function_(_apply_loss_function),
- frame_other_ptr_(), // nullptr
- feature_other_ptr_(), // nullptr
- landmark_other_ptr_(), // nullptr
- processor_ptr_() // nullptr
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
+ processor_ptr_(_processor_ptr)
{
-// std::cout << "constructed +c" << id() << std::endl;
+ if (_frame_other_ptr)
+ frame_other_list_.push_back(_frame_other_ptr);
+ if (_capture_other_ptr)
+ capture_other_list_.push_back(_capture_other_ptr);
+ if (_feature_other_ptr)
+ feature_other_list_.push_back(_feature_other_ptr);
+ if (_landmark_other_ptr)
+ landmark_other_list_.push_back(_landmark_other_ptr);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
FactorBase::FactorBase(const std::string& _tp,
- const 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)
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ NodeBase("FACTOR", _tp),
+ feature_ptr_(), // will be filled in link()
+ factor_id_(++factor_id_count_),
+ topology_(_top),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function),
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
+ processor_ptr_(_processor_ptr)
{
-// std::cout << "constructed +c" << id() << std::endl;
+ for (auto& Fo : _frame_other_list)
+ frame_other_list_.push_back(Fo);
+ for (auto& Co : _capture_other_list)
+ capture_other_list_.push_back(Co);
+ for (auto& fo : _feature_other_list)
+ feature_other_list_.push_back(fo);
+ for (auto& Lo : landmark_other_list_)
+ landmark_other_list_.push_back(Lo);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
-void FactorBase::remove()
+
+void FactorBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
- FactorBasePtr this_c = shared_from_this(); // keep this alive while removing it
+ FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
+
+ // add factor to be removed from solver
+ if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
+ getProblem()->notifyFactor(this_fac,REMOVE);
+
+ // remove from upstream
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
+ f->removeFactor(this_fac);
+ if (viral_remove_empty_parent && f->getFactorList().empty() && f->getConstrainedByList().empty())
+ f->remove(viral_remove_empty_parent); // 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)
+ for (auto& frm_ow : frame_other_list_)
{
- frm_o->getConstrainedByList().remove(shared_from_this());
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
- frm_o->remove();
+ auto frm_o = frm_ow.lock();
+ if (frm_o)
+ {
+ frm_o->removeConstrainedBy(this_fac);
+ if (viral_remove_empty_parent && frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
+ frm_o->remove(viral_remove_empty_parent);
+ }
}
- CaptureBasePtr cap_o = capture_other_ptr_.lock();
- if (cap_o)
+ for (auto& cap_ow : capture_other_list_)
{
- cap_o->getConstrainedByList().remove(shared_from_this());
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
- cap_o->remove();
+ auto cap_o = cap_ow.lock();
+ if (cap_o)
+ {
+ cap_o->removeConstrainedBy(this_fac);
+ if (viral_remove_empty_parent && cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ cap_o->remove(viral_remove_empty_parent);
+ }
}
- FeatureBasePtr ftr_o = feature_other_ptr_.lock();
- if (ftr_o)
+ for (auto& ftr_ow : feature_other_list_)
{
- ftr_o->getConstrainedByList().remove(shared_from_this());
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
- ftr_o->remove();
+ auto ftr_o = ftr_ow.lock();
+ if (ftr_o)
+ {
+ ftr_o->removeConstrainedBy(this_fac);
+ if (viral_remove_empty_parent && ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
+ ftr_o->remove(viral_remove_empty_parent);
+ }
}
- LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
- if (lmk_o)
+ for (auto& lmk_ow : landmark_other_list_)
{
- lmk_o->getConstrainedByList().remove(shared_from_this());
- if (lmk_o->getConstrainedByList().empty())
- lmk_o->remove();
+ auto lmk_o = lmk_ow.lock();
+ if (lmk_o)
+ {
+ lmk_o->removeConstrainedBy(this_fac);
+ if (viral_remove_empty_parent && lmk_o->getConstrainedByList().empty())
+ lmk_o->remove(viral_remove_empty_parent);
+ }
}
// std::cout << "Removed c" << id() << std::endl;
}
}
-const Eigen::VectorXs& FactorBase::getMeasurement() const
-{
- return getFeature()->getMeasurement();
-}
-
-const Eigen::MatrixXs& FactorBase::getMeasurementCovariance() const
+CaptureBasePtr FactorBase::getCapture() const
{
- return getFeature()->getMeasurementCovariance();
+ auto ftr = getFeature();
+ if (ftr != nullptr) return ftr->getCapture();
+ else return nullptr;
}
-const Eigen::MatrixXs& FactorBase::getMeasurementSquareRootInformationUpper() const
+FrameBasePtr FactorBase::getFrame() const
{
- return getFeature()->getMeasurementSquareRootInformationUpper();
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getFrame();
+ else return nullptr;
}
-CaptureBasePtr FactorBase::getCapture() const
+SensorBasePtr FactorBase::getSensor() const
{
- return getFeature()->getCapture();
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getSensor();
+ else return nullptr;
}
void FactorBase::setStatus(FactorStatus _status)
@@ -124,54 +181,436 @@ void FactorBase::setStatus(FactorStatus _status)
else if (_status != status_)
{
if (_status == FAC_ACTIVE)
- getProblem()->addFactor(shared_from_this());
+ getProblem()->notifyFactor(shared_from_this(),ADD);
else if (_status == FAC_INACTIVE)
- getProblem()->removeFactor(shared_from_this());
+ getProblem()->notifyFactor(shared_from_this(),REMOVE);
}
status_ = _status;
}
-void FactorBase::setApplyLossFunction(const bool _apply)
+bool FactorBase::hasFrameOther(const FrameBasePtr &_frm_other) const
+{
+ FrameBaseWConstIter frm_it = find_if(frame_other_list_.begin(),
+ frame_other_list_.end(),
+ [_frm_other](const FrameBaseWPtr &frm_ow)
+ {
+ return frm_ow.lock() == _frm_other;
+ }
+ );
+ if (frm_it != frame_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasCaptureOther(const CaptureBasePtr &_cap_other) const
+{
+ CaptureBaseWConstIter cap_it = find_if(capture_other_list_.begin(),
+ capture_other_list_.end(),
+ [_cap_other](const CaptureBaseWPtr &cap_ow)
+ {
+ return cap_ow.lock() == _cap_other;
+ }
+ );
+ if (cap_it != capture_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasFeatureOther(const FeatureBasePtr &_ftr_other) const
+{
+ FeatureBaseWConstIter ftr_it = find_if(feature_other_list_.begin(),
+ feature_other_list_.end(),
+ [_ftr_other](const FeatureBaseWPtr &ftr_ow)
+ {
+ return ftr_ow.lock() == _ftr_other;
+ }
+ );
+ if (ftr_it != feature_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const
+{
+ LandmarkBaseWConstIter lmk_it = find_if(landmark_other_list_.begin(),
+ landmark_other_list_.end(),
+ [_lmk_other](const LandmarkBaseWPtr &lmk_ow)
+ {
+ return lmk_ow.lock() == _lmk_other;
+ }
+ );
+ if (lmk_it != landmark_other_list_.end())
+ return true;
+
+ return false;
+}
+
+void FactorBase::link(FeatureBasePtr _ftr_ptr)
{
- if (apply_loss_function_ != _apply)
+ assert(!is_removing_ && "linking a removed factor");
+ assert(this->getFeature() == nullptr && "linking an already linked factor");
+
+ // not link if nullptr
+ if(_ftr_ptr == nullptr)
+ {
+ WOLF_WARN("Linking with nullptr");
+ return;
+ }
+
+ // link with feature
+ _ftr_ptr->addFactor(shared_from_this());
+ this->setFeature(_ftr_ptr);
+
+ // set problem ( and register factor )
+ WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
+ this->setProblem(_ftr_ptr->getProblem());
+
+ // constrained by
+ for (auto& frm_ow : frame_other_list_)
{
- if (getProblem() == nullptr)
- std::cout << "factor not linked with Problem, apply loss function change not notified" << std::endl;
- else
+ auto frame_other = frm_ow.lock();
+ if(frame_other != nullptr)
{
- FactorBasePtr this_c = shared_from_this();
- getProblem()->removeFactor(this_c);
- getProblem()->addFactor(this_c);
+ assert(frame_other->getProblem() && "Forbidden: linking a factor with a floating frame_other.");
+ frame_other->addConstrainedBy(shared_from_this());
}
}
+ for (auto& cap_ow : capture_other_list_)
+ {
+ auto capture_other = cap_ow.lock();
+ if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& ftr_ow : feature_other_list_)
+ {
+ auto feature_other = ftr_ow.lock();
+ if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& lmk_ow : landmark_other_list_)
+ {
+ auto landmark_other = lmk_ow.lock();
+ if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ }
}
-void FactorBase::link(FeatureBasePtr _ftr_ptr)
+
+void FactorBase::setProblem(ProblemPtr _problem)
{
- if(_ftr_ptr)
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ if (this->getStatus() == FAC_ACTIVE)
+ this->getProblem()->notifyFactor(shared_from_this(),ADD);
+}
+
+void FactorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Fac" << id() << " " << getType() << (getStatus() == FAC_ACTIVE ? "" : " INACTIVE") << " -->";
+ if ( getFrameOtherList() .empty()
+ && getCaptureOtherList() .empty()
+ && getFeatureOtherList() .empty()
+ && getLandmarkOtherList().empty())
+ _stream << " Abs";
+
+ for (const auto& Fow : getFrameOtherList())
+ if (!Fow.expired())
+ _stream << " Frm" << Fow.lock()->id();
+ for (const auto& Cow : getCaptureOtherList())
+ if (!Cow.expired())
+ _stream << " Cap" << Cow.lock()->id();
+ for (const auto& fow : getFeatureOtherList())
+ if (!fow.expired())
+ _stream << " Ftr" << fow.lock()->id();
+ for (const auto& Low : getLandmarkOtherList())
+ if (!Low.expired())
+ _stream << " Lmk" << Low.lock()->id();
+ _stream << std::endl;
+}
+
+void FactorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
+
+CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ _stream << _tabs << "Fac" << id() << " @ " << _fac_ptr.get();
+
+ if ( getFrameOtherList() .empty()
+ && getCaptureOtherList() .empty()
+ && getFeatureOtherList() .empty()
+ && getLandmarkOtherList().empty() ) // case ABSOLUTE:
{
- _ftr_ptr->addFactor(shared_from_this());
- this->setFeature(_ftr_ptr);
- this->setProblem(_ftr_ptr->getProblem());
- // add factor to be added in solver
- if (this->getProblem() != nullptr)
+ if (_verbose)
+ _stream << " --> Abs.";
+ }
+
+ // find constrained_by pointer in constrained frame
+ for (const auto& Fow : getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ if (_verbose)
{
- if (this->getStatus() == FAC_ACTIVE)
- this->getProblem()->addFactor(shared_from_this());
+ _stream << " ( --> Frm" << Fo->id() << " <- ";
+ for (auto cby : Fo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
}
- else
- WOLF_WARN("ADDING CONSTRAINT ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM.");
+
+ // check constrained_by pointer in constrained frame
+ bool found = Fo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
}
- else
+ if (_verbose && !getFrameOtherList().empty())
+ _stream << ")";
+ // find constrained_by pointer in constrained capture
+ for (const auto& Cow : getCaptureOtherList())
{
- WOLF_WARN("Linking with nullptr");
+ if (!Cow.expired())
+ {
+ const auto& Co = Cow.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> Cap" << Co->id() << " <- ";
+ for (auto cby : Co->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained frame
+ bool found = Co->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getCaptureOtherList().empty())
+ _stream << ")";
+ // find constrained_by pointer in constrained feature
+ for (const auto& fow : getFeatureOtherList())
+ {
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> Ftr" << fo->id() << " <- ";
+ for (auto cby : fo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained feature
+ bool found = fo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+ }
+ }
+ if (_verbose && !getFeatureOtherList().empty())
+ _stream << ")";
+
+ // find constrained_by pointer in constrained landmark
+ for (const auto& Low : getLandmarkOtherList())
+ {
+ if (Low.expired())
+ {
+ const auto& Lo = Low.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> Lmk" << Lo->id() << " <- ";
+ for (auto cby : Lo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained landmark
+ bool found = Lo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getLandmarkOtherList().empty())
+ _stream << ")";
+ if (_verbose)
+ _stream << std::endl;
+ //Check Problem and feature ptrs
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Ftr" << getFeature()->id() << " @ " << getFeature().get() << std::endl;
+ }
+ auto ftr_ptr = getFeature();
+ // check problem and feature pointers
+ inconsistency_explanation << "The factor " << id() << " @ " << _fac_ptr.get() << " problem ptr " << getProblem().get()
+ << " is different from Feature's problem ptr " << ftr_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == ftr_ptr->getProblem()), inconsistency_explanation);
+
+
+ inconsistency_explanation << "Fac" << id() << " @ " << _fac_ptr
+ << " ---> Ftr" << ftr_ptr->id() << " @ " << ftr_ptr
+ << " -X-> Fac" << id();
+ auto ftr_fac_list = ftr_ptr->getFactorList();
+ auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBasePtr fac){ return fac == _fac_ptr;});
+ log.assertTrue(ftr_has_fac!= ftr_fac_list.end(), inconsistency_explanation);
+
+ // find state block pointers in all constrained nodes
+ SensorBasePtr S = getSensor(); // get own sensor to check sb
+ FrameBasePtr F = getFrame();
+ CaptureBasePtr C = getCapture();
+
+ for (auto sb : getStateBlockPtrVector())
+ {
+ bool found = false;
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ }
+ bool found_here;
+
+ // find in own Frame
+ found_here = F->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Frm" << F->id();
+ found = found || found_here;
+
+ // find in own Capture
+ found_here = C->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Cap" << C->id();
+ found = found || found_here;
+
+ // Find in other Captures of the own Frame
+ if (!found_here)
+ for (auto FC : F->getCaptureList())
+ {
+ found_here = FC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Frm" << F->id() << ".Cap" << FC->id();
+ found = found || found_here;
+ }
+
+ // find in own Sensor
+ if (S)
+ {
+ found_here = S->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Sen" << S->id();
+ found = found || found_here;
+ }
+
+
+ // find in constrained Frame
+ for (const auto& Fow : getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ found_here = Fo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id();
+ found = found || found_here;
+
+ // find in feature other's captures
+ for (auto FoC : Fo->getCaptureList())
+ {
+ found_here = FoC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id() << ".C" << FoC->id();
+ found = found || found_here;
+ }
+
+ }
+ }
+
+ // find in constrained Capture
+ for (const auto& Cow : getCaptureOtherList())
+ {
+ if (!Cow.expired())
+ {
+ const auto& Co = Cow.lock();
+ found_here = Co->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " CapO" << Co->id();
+ found = found || found_here;
+ }
+ }
+
+ // find in constrained Feature
+ for (const auto& fow : getFeatureOtherList())
+ {
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ // find in constrained feature's Frame
+ auto foF = fo->getFrame();
+ found_here = foF->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOF" << foF->id();
+ found = found || found_here;
+
+ // find in constrained feature's Capture
+ auto foC = fo->getCapture();
+ found_here = foC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOC" << foC->id();
+ found = found || found_here;
+
+ // find in constrained feature's Sensor
+ auto foS = fo->getCapture()->getSensor();
+ found_here = foS->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOS" << foS->id();
+ found = found || found_here;
+ }
+ }
+
+ // find in constrained landmark
+ for (const auto& Low : getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ found_here = Lo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " LmkO" << Lo->id();
+ found = found || found_here;
+ }
+ }
+
+ if (_verbose)
+ {
+ if (found)
+ _stream << " found";
+ else
+ _stream << " NOT FOUND !";
+ _stream << std::endl;
+ }
+
+ // check that the state block has been found somewhere
+ inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
+ log.assertTrue((found), inconsistency_explanation);
+
+ inconsistency_explanation << "The stateblock " << sb << " of factor " << id() << " @ " << _fac_ptr << " is null\n";
+ log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
}
- auto frame_other = this->frame_other_ptr_.lock();
- if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
- auto capture_other = this->capture_other_ptr_.lock();
- if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
- auto feature_other = this->feature_other_ptr_.lock();
- if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
- auto landmark_other = this->landmark_other_ptr_.lock();
- if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ return log;
+}
+
+bool FactorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto fac_ptr = std::static_pointer_cast<FactorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, fac_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ return _log.is_consistent_;
}
} // namespace wolf
diff --git a/src/feature/CMakeLists.txt b/src/feature/CMakeLists.txt
deleted file mode 100644
index ca674167dd7c8d7a4bd36bf0e9af40f24232f926..0000000000000000000000000000000000000000
--- a/src/feature/CMakeLists.txt
+++ /dev/null
@@ -1,32 +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 (OPENCV_FOUND AND Apriltag_FOUND)
- SET(HDRS_FEATURE ${HDRS_FEATURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/feature_apriltag.h
- )
- SET(SRCS_FEATURE ${SRCS_FEATURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/feature_apriltag.cpp
- )
-ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
-
-#=========================================
-#=========================================
-
-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_base.cpp b/src/feature/feature_base.cpp
index dd86c7c8cb7eaa911baa731d67a413d76b3df1b4..c2af9127c6a2e028d98f4f5413490117bfbd7825 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -1,12 +1,13 @@
#include "core/feature/feature_base.h"
#include "core/factor/factor_base.h"
#include "core/capture/capture_base.h"
+#include "core/math/covariance.h"
namespace wolf {
unsigned int FeatureBase::feature_id_count_ = 0;
-FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type) :
+FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_uncertainty, UncertaintyType _uncertainty_type) :
NodeBase("FEATURE", _type),
capture_ptr_(),
feature_id_(++feature_id_count_),
@@ -36,30 +37,35 @@ FeatureBase::~FeatureBase()
// std::cout << "destructed -f" << id() << std::endl;
}
-void FeatureBase::remove()
+void FeatureBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
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
+ factor_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
while (!constrained_by_list_.empty())
{
- constrained_by_list_.front()->remove(); // remove constrained
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
+ }
+
+ // remove from upstream
+ CaptureBasePtr C = capture_ptr_.lock();
+ if (C)
+ {
+ C->removeFeature(this_f); // remove from upstream
+ if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
+ C->remove(viral_remove_empty_parent); // remove upstream
}
}
}
@@ -70,6 +76,11 @@ FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
return _co_ptr;
}
+void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
+{
+ factor_list_.remove(_co_ptr);
+}
+
FrameBasePtr FeatureBase::getFrame() const
{
return capture_ptr_.lock()->getFrame();
@@ -78,21 +89,40 @@ FrameBasePtr FeatureBase::getFrame() const
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()
+void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
+bool FeatureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
+const FactorBasePtrList& FeatureBase::getFactorList() const
{
return factor_list_;
}
-void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
+void FeatureBase::getFactorList(FactorBasePtrList & _fac_list) const
{
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
-void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
+void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov)
{
WOLF_ASSERT_COVARIANCE_MATRIX(_meas_cov);
@@ -103,7 +133,7 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
}
-void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
+void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info)
{
WOLF_ASSERT_INFORMATION_MATRIX(_meas_info);
@@ -117,37 +147,23 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
}
-void FeatureBase::setProblem(ProblemPtr _problem)
+void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
{
- NodeBase::setProblem(_problem);
- for (auto ctr : factor_list_)
- ctr->setProblem(_problem);
+ measurement_ = _meas;
}
-Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const
+void FeatureBase::setProblem(ProblemPtr _problem)
{
- // 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;
+ NodeBase::setProblem(_problem);
+ for (auto fac : factor_list_)
+ fac->setProblem(_problem);
}
void FeatureBase::link(CaptureBasePtr _cpt_ptr)
{
+ assert(!is_removing_ && "linking a removed feature");
+ assert(this->getCapture() == nullptr && "linking an already linked feature");
+
if(_cpt_ptr)
{
_cpt_ptr->addFeature(shared_from_this());
@@ -159,5 +175,95 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
WOLF_WARN("Linking with nullptr");
}
}
+void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "c " : "");
+ if (_constr_by)
+ {
+ _stream << " <--\t";
+ for (auto cby : getConstrainedByList())
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+ if (_metric)
+ _stream << _tabs << " " << "m = ( " << std::setprecision(2) << getMeasurement().transpose()
+ << " )" << std::endl;
+
+}
+
+void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 4)
+ for (auto c : getFactorList())
+ if (c)
+ c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Ftr" << id() << " @ " << _ftr_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Cap" << getCapture()->id() << " @ " << getCapture().get()
+ << std::endl;
+ }
+
+ auto cap_ptr = getCapture();
+ // check problem and capture pointers
+ inconsistency_explanation << "Feature frame problem pointer " << getProblem().get()
+ << " different from Capture problem pointer " << cap_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == cap_ptr->getProblem()), inconsistency_explanation);
+
+ // check contrained_by
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& fow : cby->getFeatureOtherList())
+ _stream << " Ftr" << fow.lock()->id();
+ _stream << std::endl;
+ }
+
+ // check constrained_by pointer to this feature
+ inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
+
+ }
+
+ auto cap_ftr = _ftr_ptr->getCapture();
+ inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
+ << " ---> Cap" << cap_ftr->id() << " @ " << cap_ftr
+ << " -X-> Ftr" << id();
+ auto cap_ftr_list = cap_ftr->getFeatureList();
+ auto frame_has_cap = std::find_if(cap_ftr_list.begin(), cap_ftr_list.end(), [&_ftr_ptr](FeatureBasePtr ftr){ return ftr == _ftr_ptr;});
+ log.assertTrue(frame_has_cap != cap_ftr_list.end(), inconsistency_explanation);
+
+ for(auto fac : getFactorList())
+ {
+ inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
+ << " ---> Fac" << fac->id() << " @ " << fac
+ << " -X-> Ftr" << id();
+ log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+bool FeatureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto ftr_ptr = std::static_pointer_cast<FeatureBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, ftr_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for(auto f : getFactorList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index 0796e18bc73708fd31f55ea4530aff70ac2918f2..9ba01d1f11c90e54ae118466b1cc2bf745d132ed 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.cpp
@@ -1,21 +1,19 @@
#include "core/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)
+namespace wolf
{
- //
-}
-const Eigen::MatrixXs& FeatureDiffDrive::getJacobianFactor() const
+FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::VectorXd& _diff_drive_params,
+ const Eigen::MatrixXd& _jacobian_diff_drive_params) :
+ FeatureMotion("FeatureDiffDrive",
+ _delta_preintegrated,
+ _delta_preintegrated_covariance,
+ _diff_drive_params,
+ _jacobian_diff_drive_params)
{
- return jacobian_diff_drive_factors_;
+ //
}
} /* namespace wolf */
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
index 3e01bd662cf69b07230a6c7f939c4dd18a754a07..d0b988b79efd5b8e376c6c244a7154933479538f 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.cpp
@@ -5,15 +5,19 @@
* Author: jsola
*/
-#include "feature_motion.h"
+#include "core/feature/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(const std::string& _type,
+ const VectorXd& _delta_preint,
+ const MatrixXd _delta_preint_cov,
+ const VectorXd& _calib_preint,
+ const MatrixXd& _jacobian_calib) :
+ FeatureBase(_type, _delta_preint, _delta_preint_cov),
+ calib_preint_(_calib_preint),
+ jacobian_calib_(_jacobian_calib)
{
//
}
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
deleted file mode 100644
index ee6cd60ba38a16a161925d79512eaf9fa1e6212d..0000000000000000000000000000000000000000
--- a/src/feature/feature_odom_2D.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "core/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_odom_2d.cpp b/src/feature/feature_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a4d2571b91da0ce812b125f7a7d603c3edd64f97
--- /dev/null
+++ b/src/feature/feature_odom_2d.cpp
@@ -0,0 +1,16 @@
+#include "core/feature/feature_odom_2d.h"
+
+namespace wolf {
+
+FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
+ FeatureBase("FeatureOdom2d", _measurement, _meas_covariance)
+{
+ //std::cout << "New FeatureOdom2d: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
+}
+
+FeatureOdom2d::~FeatureOdom2d()
+{
+ //
+}
+
+} // namespace wolf
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index 344d230a7164d0c222567ae9ea7e1000be23cb84..cb5ea4c105b0cf99d0f5322544983cd29345f01b 100644
--- a/src/feature/feature_pose.cpp
+++ b/src/feature/feature_pose.cpp
@@ -2,8 +2,8 @@
namespace wolf {
-FeaturePose::FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("POSE", _measurement, _meas_covariance)
+FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
+ FeatureBase("FeaturePose", _measurement, _meas_covariance)
{
//
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 49c5ae4f2d9474d34be5b83378f5b2517507cd62..71e344552517ef659bb3ae60f66abbb95c25056f 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -6,324 +6,193 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/factory_state_block.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_ESTIMATED),
- time_stamp_(_ts)
+FrameBase::FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
+ const VectorComposite& _state) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(_frame_structure),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ time_stamp_(_ts)
{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _v_ptr;
+ assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure");
+
+ for (auto key : getStructure())
+ {
+ StateBlockPtr sb = FactoryStateBlock::create(string(1,key), _state.at(key), false); // false = non fixed
+ addStateBlock(key, sb, getProblem());
+ }
}
-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)
+
+FrameBase::FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
+ const std::list<VectorXd>& _vectors) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(_frame_structure),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ time_stamp_(_ts)
{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _v_ptr;
+ assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
+
+ auto vec_it = _vectors.begin();
+ for (const auto key : _frame_structure)
+ {
+ const auto& vec = *vec_it;
+
+ StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
+
+ addStateBlock(key, sb, getProblem());
+
+ vec_it++;
+ }
}
-FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
- 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)
+
+FrameBase::FrameBase(const TimeStamp& _ts,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _v_ptr) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(""),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ time_stamp_(_ts)
{
- if(_frame_structure.compare("PO") == 0 and _dim == 2){
- // auto _x = Eigen::VectorXs::Zero(3);
- 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 );
- state_block_vec_[0] = p_ptr;
- state_block_vec_[1] = o_ptr;
- state_block_vec_[2] = v_ptr;
- this->setType("PO 2D");
- }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXs::Zero(7);
- 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 );
- state_block_vec_[0] = p_ptr;
- state_block_vec_[1] = o_ptr;
- state_block_vec_[2] = v_ptr;
- this->setType("PO 3D");
- }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXs::Zero(10);
- 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> ( ) ) );
- state_block_vec_[0] = p_ptr;
- state_block_vec_[1] = o_ptr;
- state_block_vec_[2] = v_ptr;
- this->setType("POV 3D");
- }else{
- std::cout << _frame_structure << " ^^ " << _dim << std::endl;
- throw std::runtime_error("Unknown frame structure");
+ if (_p_ptr)
+ {
+ addStateBlock('P', _p_ptr, getProblem());
+ }
+ if (_o_ptr)
+ {
+ addStateBlock('O', _o_ptr, getProblem());
+ }
+ if (_v_ptr)
+ {
+ addStateBlock('V', _v_ptr, getProblem());
}
-
}
+
FrameBase::~FrameBase()
{
}
-void FrameBase::remove()
+void FrameBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
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())
+ while (!constrained_by_list_.empty())
{
- capture_list_.front()->remove(); // remove downstream
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
- while (!constrained_by_list_.empty())
+ while (!capture_list_.empty())
{
- constrained_by_list_.front()->remove(); // remove constrained
+ capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
// Remove Frame State Blocks
- if ( isKeyOrAux() )
- removeStateBlocks();
-
-
- if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastKeyOrAuxFrame()->id() == this_F->id())
- getTrajectory()->updateLastFrames();
-
-// std::cout << "Removed F" << id() << std::endl;
- }
-}
+ removeStateBlocks(getProblem());
-void FrameBase::setTimeStamp(const TimeStamp& _ts)
-{
- time_stamp_ = _ts;
- if (isKeyOrAux() && 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)
+ // remove from upstream
+ TrajectoryBasePtr T = trajectory_ptr_.lock();
+ if (T)
{
- if (getProblem() != nullptr)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlock(i, nullptr);
+ T->removeFrame(this_F); // remove from upstream
}
}
}
-void FrameBase::setKey()
-{
- // register if previously not estimated
- if (!isKeyOrAux())
- registerNewStateBlocks();
-
- // WOLF_DEBUG("Set Key", this->id());
- type_ = KEY;
- getTrajectory()->sortFrame(shared_from_this());
- getTrajectory()->updateLastFrames();
-}
-
-void FrameBase::setAux()
+void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
- if (!isKeyOrAux())
- registerNewStateBlocks();
-
- // WOLF_DEBUG("Set Auxiliary", this->id());
- type_ = AUXILIARY;
- getTrajectory()->sortFrame(shared_from_this());
- getTrajectory()->updateLastFrames();
+ time_stamp_ = _ts;
}
-void FrameBase::fix()
+void FrameBase::link(ProblemPtr _prb)
{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
+ assert(_prb != nullptr && "Trying to link Frame with a null problem pointer!");
+ this->link(_prb->getTrajectory());
}
-void FrameBase::unfix()
+bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
+ return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
-bool FrameBase::isFixed() const
+FrameBasePtr FrameBase::getPreviousFrame() const
{
- bool fixed = true;
- for (auto sb : getStateBlockVec())
- {
- if (sb)
- fixed &= sb->isFixed();
- }
- return fixed;
-}
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
-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());
- }
+ auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
- //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");
+ assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
- unsigned int index = 0;
- const unsigned int _st_size = _state.size();
+ if (current_frame_it == getTrajectory()->getFrameMap().begin())
+ return nullptr;
- //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()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
- index += sb->getSize();
- }
+ return std::prev(current_frame_it)->second;
}
-Eigen::VectorXs FrameBase::getState() const
+FrameBasePtr FrameBase::getNextFrame() const
{
- Eigen::VectorXs state;
-
- getState(state);
-
- return state;
-}
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
-void FrameBase::getState(Eigen::VectorXs& _state) const
-{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
+ auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
- _state = Eigen::VectorXs(size);
+ assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
- SizeEigen index = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- {
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
-}
+ if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
+ return nullptr;
-unsigned int FrameBase::getSize() const
-{
- unsigned int size = 0;
- for (auto st : state_block_vec_)
- if (st)
- size += st->getSize();
- return size;
+ return std::next(current_frame_it)->second;
}
-bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
+CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
{
- return getProblem()->getFrameCovariance(shared_from_this(), _cov);
+ WOLF_WARN_COND(getCaptureOf(_capt_ptr->getSensor()) != nullptr, "FrameBase::addCapture adding new capture ", _capt_ptr->id(), " in a frame with another capture of the same sensor: ", getCaptureOf(_capt_ptr->getSensor())->id());
+ capture_list_.push_back(_capt_ptr);
+ return _capt_ptr;
}
-FrameBasePtr FrameBase::getPreviousFrame() const
+void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
{
- //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;
+ capture_list_.remove(_capt_ptr);
}
-FrameBasePtr FrameBase::getNextFrame() const
+CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type) 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;
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ return capture_ptr;
return nullptr;
}
-CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
+CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type) const
{
- capture_list_.push_back(_capt_ptr);
- return _capt_ptr;
+ CaptureBasePtrList captures;
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ captures.push_back(capture_ptr);
+ return captures;
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
+CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
{
for (CaptureBasePtr capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
@@ -331,15 +200,16 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const st
return nullptr;
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr)
+CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
{
- for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensor() == _sensor_ptr)
- return capture_ptr;
+ if (_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 FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
{
CaptureBasePtrList captures;
for (CaptureBasePtr capture_ptr : getCaptureList())
@@ -348,29 +218,40 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
return captures;
}
-void FrameBase::unlinkCapture(CaptureBasePtr _cap_ptr)
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
{
- _cap_ptr->unlinkFromFrame();
- capture_list_.remove(_cap_ptr);
+ for (const FactorBasePtr& factor_ptr : getConstrainedByList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ for (const FactorBasePtr& factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ return nullptr;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
{
- for (const FactorBasePtr& constaint_ptr : getConstrainedByList())
- if (constaint_ptr->getProcessor() == _processor_ptr && constaint_ptr->getType() == type)
- return constaint_ptr;
+ for (const FactorBasePtr& factor_ptr : getConstrainedByList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
+ for (const FactorBasePtr& factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
return nullptr;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
+FactorBasePtrList FrameBase::getFactorList() const
{
- for (const FactorBasePtr& constaint_ptr : getConstrainedByList())
- if (constaint_ptr->getProcessor() == _processor_ptr)
- return constaint_ptr;
- return nullptr;
+ FactorBasePtrList fac_list;
+ getFactorList(fac_list);
+ return fac_list;
}
-void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
+void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const
{
for (auto c_ptr : getCaptureList())
c_ptr->getFactorList(_fac_list);
@@ -379,66 +260,179 @@ void FrameBase::getFactorList(FactorBasePtrList& _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)
+void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
+bool FrameBase::isConstrainedBy(const FactorBasePtr &_factor) const
{
- 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;
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
}
-FrameBasePtr FrameBase::create_PO_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
+
+void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
- 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;
+ assert(!is_removing_ && "linking a removed frame");
+ assert(this->getTrajectory() == nullptr && "linking an already linked frame");
+
+ if(_trj_ptr)
+ {
+ _trj_ptr->addFrame(shared_from_this());
+ this->setTrajectory(_trj_ptr);
+ this->setProblem(_trj_ptr->getProblem());
+ }
+ else
+ {
+ WOLF_WARN("Linking Frame ", id(), " to a nullptr");
+ }
}
-FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
+
+void FrameBase::setProblem(ProblemPtr _problem)
{
- 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;
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ registerNewStateBlocks(_problem);
+
+ for (auto cap : capture_list_)
+ cap->setProblem(_problem);
}
-void FrameBase::link(TrajectoryBasePtr _ptr)
+
+void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- if(_ptr)
+ _stream << _tabs << "Frm" << id()
+ << " " << getStructure()
+ << " ts = " << std::setprecision(3) << getTimeStamp()
+ << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
+ if (_constr_by)
{
- _ptr->addFrame(shared_from_this());
- this->setTrajectory(_ptr);
- this->setProblem(_ptr->getProblem());
- if (this->isKey()) this->registerNewStateBlocks();
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
- else
+ _stream << std::endl;
+
+ printState(_metric, _state_blocks, _stream, _tabs);
+}
+
+void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 2)
+ for (auto C : getCaptureList())
+ if (C)
+ C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose) {
+ _stream << _tabs << "Frm"
+ << id() << " @ " << _frm_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
+ }
+ for (const auto &pair: getStateBlockMap()) {
+
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Frame " << id() << " @ "<< _frm_ptr.get()
+ << " has State block pointer " << sb.get()
+ << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose) {
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ // check problem pointer
+ auto trajectory_ptr = getTrajectory();
+ auto trajectory_problem_ptr = trajectory_ptr->getProblem();
+ inconsistency_explanation << "Frame problem pointer " << getProblem().get()
+ << " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n";
+ log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
+
+ // check constrained_by
+ for (auto cby : getConstrainedByList())
{
- WOLF_WARN("Linking with a nullptr");
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& Fow : cby->getFrameOtherList())
+ _stream << " Frm" << Fow.lock()->id() << std::endl;
+
+
+ // check constrained_by pointer to this frame
+ inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
+ << " not found among constrained-by factors\n";
+ auto F = std::static_pointer_cast<FrameBase>(_frm_ptr);
+ log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose) {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+ }
+
+ auto trj_ptr = getTrajectory();
+ inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
+ << " ---> Trj" << " @ " << trj_ptr
+ << " -X-> Frm" << id();
+ auto trj_has_frm = std::find_if(trj_ptr->begin(), trj_ptr->end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
+ log.assertTrue(trj_has_frm != trj_ptr->end(), inconsistency_explanation);
+
+ for(auto C : getCaptureList())
+ {
+ inconsistency_explanation << "Frm " << id() << " @ " << _frm_ptr
+ << " ---> Cap" << C->id() << " @ " << C
+ << " -X-> Frm" << id();
+ log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
}
+ return log;
}
-} // namespace wolf
-#include "core/common/factory.h"
-namespace wolf
+bool FrameBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
-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); }
+ auto frm_ptr = std::static_pointer_cast<FrameBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, frm_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for(auto C : getCaptureList()) C->check(_log, C, _verbose, _stream, _tabs + " ");
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index adda7984748ebfb2fa6e40517f0d1bedc331efd2..e6e5a15c3c216d2abfa9242a48bb97081e22d016 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -4,7 +4,7 @@
namespace wolf {
HardwareBase::HardwareBase() :
- NodeBase("HARDWARE", "Base")
+ NodeBase("HARDWARE", "HardwareBase")
{
// std::cout << "constructed H" << std::endl;
}
@@ -20,4 +20,42 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
return _sensor_ptr;
}
+void HardwareBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "") << std::endl;
+
+}
+void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 1)
+ for (auto S : getSensorList())
+ if (S)
+ S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+ if (_verbose)
+ {
+ _stream << _tabs << "Hrw @ " << _hwd_ptr.get() << std::endl;
+ }
+
+ // check pointer to Problem
+ inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_hwd_ptr->getProblem()->getHardware().get() == _hwd_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool HardwareBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto hwd_ptr = std::static_pointer_cast<HardwareBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, hwd_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto S : getSensorList())
+ S->check(_log, S, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 5bdfff041cc7a9bf8b2020b779736e61fac97da1..11e26334cfedde1e6ffcae6f4588a57651c51eaa 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -3,156 +3,91 @@
#include "core/factor/factor_base.h"
#include "core/map/map_base.h"
#include "core/state_block/state_block.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/common/factory.h"
#include "core/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(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
+LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
NodeBase("LANDMARK", _type),
- map_ptr_(_ptr),
- state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
+ HasStateBlocks(""),
+ map_ptr_(),
+ state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
+{
+ if (_p_ptr)
{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
-
- // std::cout << "constructed +L" << id() << std::endl;
+ addStateBlock('P', _p_ptr, nullptr);
+ }
+ if (_o_ptr)
+ {
+ addStateBlock('O', _o_ptr, nullptr);
}
+
+}
+
LandmarkBase::~LandmarkBase()
{
- removeStateBlocks();
-// std::cout << "destructed -L" << id() << std::endl;
+ removeStateBlocks(getProblem());
}
-void LandmarkBase::remove()
+void LandmarkBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
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();
+ constrained_by_list_.front()->remove(viral_remove_empty_parent);
}
// Remove State Blocks
- removeStateBlocks();
- }
-}
+ removeStateBlocks(getProblem());
-void LandmarkBase::fix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
+ // remove from upstream
+ auto M = map_ptr_.lock();
+ if (M)
+ M->removeLandmark(this_L);
+ }
}
-void LandmarkBase::unfix()
+std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
-}
+ std::vector<StateBlockPtr> used_state_block_vec(0);
-bool LandmarkBase::isFixed() const
-{
- bool fixed = true;
- for (auto sb : getStateBlockVec())
+ // normal state blocks in {P,O,V,W}
+ for (const auto& key : getStructure())
{
- if (sb)
- fixed &= sb->isFixed();
+ const auto& sbp = getStateBlock(key);
+ if (sbp)
+ used_state_block_vec.push_back(sbp);
}
- return fixed;
-}
-std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
-{
- std::vector<StateBlockPtr> used_state_block_vec(0);
+ // other state blocks in a vector
for (auto sbp : state_block_vec_)
if (sbp)
used_state_block_vec.push_back(sbp);
- return used_state_block_vec;
-}
-void LandmarkBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
- {
- for (auto sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
+ return used_state_block_vec;
}
-bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
+bool LandmarkBase::getCovariance(Eigen::MatrixXd& _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;
@@ -170,14 +105,17 @@ YAML::Node LandmarkBase::saveToYaml() const
}
return node;
}
+
void LandmarkBase::link(MapBasePtr _map_ptr)
{
+ assert(!is_removing_ && "linking a removed landmark");
+ assert(this->getMap() == nullptr && "linking an already linked landmark");
+
if(_map_ptr)
{
this->setMap(_map_ptr);
_map_ptr->addLandmark(shared_from_this());
this->setProblem(_map_ptr->getProblem());
- this->registerNewStateBlocks();
}
else
{
@@ -185,11 +123,174 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
}
}
+void LandmarkBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ registerNewStateBlocks(_problem);
+}
+
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setLandmarkOther(shared_from_this());
return _fac_ptr;
}
+void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
+bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
+void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Lmk" << id() << " " << getType();
+ if (_constr_by)
+ {
+ _stream << "\t<-- ";
+ for (auto cby : getConstrainedByList())
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ printState(_metric, _state_blocks, _stream, _tabs);
+}
+
+void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
+LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
+{
+ unsigned int id = _node["id"] .as< unsigned int >();
+ Eigen::VectorXd pos = _node["position"] .as< Eigen::VectorXd >();
+ bool pos_fixed = _node["position fixed"] .as< bool >();
+
+ StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed);
+ StateBlockPtr ori_sb = nullptr;
+
+ if (_node["orientation"])
+ {
+ Eigen::VectorXd ori = _node["orientation"].as< Eigen::VectorXd >();
+ bool ori_fixed = _node["orientation fixed"].as< bool >();
+
+ if (ori.size() == 4)
+ ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed);
+ else
+ ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed);
+ }
+
+ LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("LandmarkBase", pos_sb, ori_sb);
+ lmk->setId(id);
+
+ return lmk;
+}
+
+CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Lmk" << id() << " @ " << _lmk_ptr.get() << std::endl;
+ _stream << _tabs << " -> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " -> Map @ " << getMap().get() << std::endl;
+ for (const auto& pair : getStateBlockMap())
+ {
+ auto sb = pair.second;
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ auto map_ptr = getMap();
+ // check problem and map pointers
+ inconsistency_explanation << "Landmarks' problem ptr "
+ << getProblem().get() << " different from Map's problem ptr "
+ << map_ptr->getProblem().get() << "\n";
+
+ log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
+
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
+
+ for (const auto& Low : cby->getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ _stream << " Lmk" << Lo->id();
+ }
+ }
+ _stream << std::endl;
+ }
+
+ // check constrained-by factors
+ inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector()) {
+ if (_verbose) {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+
+ inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
+ << " ---> Map" << map_ptr
+ << " -X-> Lmk" << id();
+ auto map_lmk_list = map_ptr->getLandmarkList();
+ auto map_has_lmk = std::find_if(map_lmk_list.begin(), map_lmk_list.end(), [&_lmk_ptr](LandmarkBasePtr lmk){ return lmk == _lmk_ptr;});
+ log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
+
+ return log;
+}
+bool LandmarkBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto lmk_ptr = std::static_pointer_cast<LandmarkBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, lmk_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ return _log.is_consistent_;
+}
+// Register landmark creator
+namespace
+{
+const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create);
+}
+
} // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 204fc7b8ffd3d1b1a8c49030c74c0e803e11b7c4..7f6db136f0d1644bb4d2ff193fc3c7d776fdfa26 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -33,20 +33,9 @@ LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
return _landmark_ptr;
}
-void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
+void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
{
- for (auto lmk : _landmark_list)
- addLandmark(lmk);
-
- //TEMPORARY FIX, should be made compliant with the new emplace methodology
- 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);
+ landmark_list_.remove(_landmark_ptr);
}
void MapBase::load(const std::string& _map_file_dot_yaml)
@@ -60,8 +49,8 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
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);
+ LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
+ lmk_ptr->link(shared_from_this());
}
}
@@ -101,4 +90,40 @@ std::string MapBase::dateTimeNow()
return date_time;
}
+void MapBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "") << std::endl;
+}
+void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 1)
+ for (auto L : getLandmarkList())
+ if (L)
+ L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ _stream << _tabs << "Map @ " << _map_ptr.get() << std::endl;
+
+ // check pointer to Problem
+ inconsistency_explanation << "Map->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getMap() [" << getProblem()->getMap().get() << "] -> Map [" << _map_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_map_ptr->getProblem()->getMap().get() == _map_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool MapBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto map_ptr = std::static_pointer_cast<MapBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, map_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto L : getLandmarkList())
+ L->check(_log, L, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 62c035cf0c7adfc58f2a1ee8c23ac7483d5f0564..9c939355a2fc1fcef0f472f8e936f076e210365a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -7,56 +7,68 @@
#include "core/processor/processor_motion.h"
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_pose.h"
+#include "core/capture/capture_void.h"
#include "core/factor/factor_base.h"
-#include "core/sensor/sensor_factory.h"
-#include "core/processor/processor_factory.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/factor/factor_quaternion_absolute.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/processor/factory_processor.h"
#include "core/state_block/state_block.h"
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
-#include "core/utils/loader.hpp"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/tree_manager/factory_tree_manager.h"
+#include "core/tree_manager/tree_manager_base.h"
+#include "core/utils/params_server.h"
+#include "core/utils/loader.h"
+#include "core/utils/check_log.h"
+#include "core/math/covariance.h"
+#include "core/state_block/factory_state_block.h"
// IRI libs includes
// C++ includes
#include <algorithm>
+#include <map>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <unordered_set>
-namespace wolf
-{
-// unnamed namespace used for helper functions local to this file.
-namespace
+namespace wolf
{
-std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;}
-}
Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
+ tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
- trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
+ trajectory_ptr_(std::make_shared<TrajectoryBase>()),
map_ptr_(std::make_shared<MapBase>()),
- processor_motion_ptr_(),
- prior_is_set_(false),
- frame_structure_(_frame_structure)
+ processor_is_motion_map_(),
+ frame_structure_(_frame_structure),
+ prior_options_(std::make_shared<PriorOptions>())
{
+ dim_ = _dim;
if (_frame_structure == "PO" and _dim == 2)
{
state_size_ = 3;
state_cov_size_ = 3;
- dim_ = 2;
}else if (_frame_structure == "PO" and _dim == 3)
{
state_size_ = 7;
state_cov_size_ = 6;
- dim_ = 3;
- } else if (_frame_structure == "POV" and _dim == 3)
+ }else if (_frame_structure == "POV" and _dim == 3)
{
state_size_ = 10;
state_cov_size_ = 9;
- dim_ = 3;
+ }else if (_frame_structure == "POVCDL" and _dim == 3)
+ {
+ state_size_ = 19;
+ state_cov_size_ = 18;
}
else std::runtime_error(
"Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
-
}
void Problem::setup()
@@ -72,36 +84,124 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
p->setup();
return p->shared_from_this();
}
-ProblemPtr Problem::autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file)
-{
- auto p = Problem::create(_frame_structure, _dim);
- // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/src/params.yaml";
- parserYAML parser = parserYAML(_yaml_file);
- parser.parse();
- paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
- // cout << "PRINTING SERVER MAP" << endl;
- // server.print();
+
+ProblemPtr Problem::autoSetup(ParamsServer &_server)
+{
+#if __APPLE__
+ std::string lib_extension = ".dylib";
+#else
+ std::string lib_extension = ".so";
+#endif
+ // Problem structure and dimension
+ std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
+ int dim = _server.getParam<int> ("problem/dimension");
+ auto problem = Problem::create(frame_structure, dim);
+ //
+ // cout << "PRINTING SERVER MAP" << endl;
+ // _server.print();
// cout << "-----------------------------------" << endl;
- auto loaders = vector<Loader*>();
- for(auto it : parser.getFiles()) {
- cout << "LOADING " << it << endl;
- auto l = new LoaderRaw(it);
+ WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
+
+ // Load plugins
+ auto loaders = std::vector<Loader *>();
+ std::string plugins_path;
+ try{
+ plugins_path = _server.getParam<std::string>("plugins_path");
+ }
+ catch(MissingValueException& e){
+ WOLF_WARN(e.what());
+ WOLF_WARN("Setting '/usr/local/lib/iri-algorithms/' as plugins path...");
+ plugins_path="/usr/local/lib/iri-algorithms/";
+ }
+ for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins")) {
+ if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
+ std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin);
+ auto l = new LoaderRaw(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
+ std::string packages_path;
+ try {
+ packages_path = _server.getParam<std::string>("packages_path");
+ } catch (MissingValueException& e) {
+ WOLF_WARN(e.what());
+ WOLF_WARN("Support for subscribers disabled...");
+ }
+ for (auto it : _server.getParam<std::vector<std::string>>("packages_subscriber")) {
+ std::string subscriber = packages_path + "/libsubscriber_" + it + lib_extension;
+ WOLF_TRACE("Loading subscriber " + subscriber);
+ auto l = new LoaderRaw(subscriber);
l->load();
loaders.push_back(l);
}
- //TODO: To be fixed. This prior should be set in here, but now it is set externally.
- // setPrior(Eigen::Vector3s::Zero(), 0.1*Eigen::Matrix3s::Identity(), TimeStamp(), Scalar(0.1));
- auto sensorMap = map<string, SensorBasePtr>();
- auto procesorMap = map<string, ProcessorBasePtr>();
- for(auto s : server.getSensors()){
- cout << s._name << " " << s._type << endl;
- sensorMap.insert(pair<string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, server)));
- }
- for(auto s : server.getProcessors()){
- cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
- procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
- }
- return p;
+ for (auto it : _server.getParam<std::vector<std::string>>("packages_publisher")) {
+ std::string subscriber = packages_path + "/libpublisher_" + it + lib_extension;
+ WOLF_TRACE("Loading publisher " + subscriber);
+ auto l = new LoaderRaw(subscriber);
+ l->load();
+ loaders.push_back(l);
+ }
+ std::vector<std::string> raw_libs;
+ try {
+ raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
+ } catch (MissingValueException& e) {
+ WOLF_TRACE("No raw libraries to load...");
+ raw_libs = std::vector<std::string>();
+ }
+ for (auto lib : raw_libs) {
+ WOLF_TRACE("Loading raw lib " + lib);
+ auto l = new LoaderRaw(lib);
+ l->load();
+ loaders.push_back(l);
+ }
+
+ // Install sensors and processors
+ auto sensorMap = std::map<std::string, SensorBasePtr>();
+ auto procesorMap = std::map<std::string, ProcessorBasePtr>();
+ auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
+ for(auto sen : sensors){
+ sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen["name"], problem->installSensor(sen["type"], sen["name"], _server)));
+ }
+ auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
+ for(auto prc : processors){
+ procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
+ }
+
+ // Tree manager
+ std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
+ WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
+ if (tree_manager_type != "None" and tree_manager_type != "none")
+ problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
+
+ // Prior
+ std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
+ assert((prior_mode == "nothing" || prior_mode == "initial_guess" || prior_mode == "fix" || prior_mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
+ WOLF_TRACE("Prior mode: ", prior_mode);
+ if (prior_mode == "nothing")
+ {
+ problem->setPriorOptions(prior_mode);
+ }
+ else if (prior_mode == "factor")
+ {
+ problem->setPriorOptions(prior_mode,
+ _server.getParam<double>("problem/prior/time_tolerance"),
+ _server.getParam<VectorComposite>("problem/prior/state"),
+ _server.getParam<VectorComposite>("problem/prior/sigma"));
+
+
+
+ }
+ else
+ {
+ WOLF_TRACE("Prior mode: ", prior_mode);
+ problem->setPriorOptions(prior_mode,
+ _server.getParam<double>("problem/prior/time_tolerance"),
+ _server.getParam<VectorComposite>("problem/prior/state"));
+ }
+
+ // Done
+ return problem;
}
Problem::~Problem()
@@ -109,50 +209,47 @@ Problem::~Problem()
// WOLF_DEBUG("destructed -P");
}
-void Problem::addSensor(SensorBasePtr _sen_ptr)
-{
- // getHardware()->addSensor(_sen_ptr);
- _sen_ptr->link(getHardware());
-}
-
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
- IntrinsicsBasePtr _intrinsics)
+ const Eigen::VectorXd& _extrinsics, //
+ ParamsSensorBasePtr _intrinsics)
{
- SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
- addSensor(sen_ptr);
+ SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
+ sen_ptr->link(getHardware());
return sen_ptr;
}
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
+
+ const Eigen::VectorXd& _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);
+ ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::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());
+ return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());
}
- SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const paramsServer& _server)
- {
- SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _server);
- addSensor(sen_ptr);
- return sen_ptr;
- }
+
+SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
+ const std::string& _unique_sensor_name, //
+ const ParamsServer& _server)
+{
+ SensorBasePtr sen_ptr = AutoConfFactorySensor::create(_sen_type, _unique_sensor_name, _server);
+ sen_ptr->link(getHardware());
+ return sen_ptr;
+}
+
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
SensorBasePtr _corresponding_sensor_ptr, //
- ProcessorParamsBasePtr _prc_params)
+ ParamsProcessorBasePtr _prc_params)
{
if (_corresponding_sensor_ptr == nullptr)
{
@@ -161,18 +258,15 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
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);
- prc_ptr->link(_corresponding_sensor_ptr);
+ ProcessorBasePtr prc_ptr = FactoryProcessor::create(_prc_type, _unique_processor_name, _prc_params);
- // 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());
+ //Dimension check
+ int prc_dim = prc_ptr->getDim();
+ auto prb = this;
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
+ prc_ptr->configure(_corresponding_sensor_ptr);
+ prc_ptr->link(_corresponding_sensor_ptr);
return prc_ptr;
}
@@ -184,13 +278,13 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
{
SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
- throw std::runtime_error("Sensor not found. Cannot bind Processor.");
+ throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and 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);
+ ParamsProcessorBasePtr prc_params = FactoryParamsProcessor::create(_prc_type, _params_filename);
return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
}
}
@@ -198,27 +292,26 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
const std::string& _corresponding_sensor_name, //
- const paramsServer& _server)
+ const ParamsServer& _server)
{
SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
- throw std::runtime_error("Sensor not found. Cannot bind Processor.");
- ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr);
- prc_ptr->configure(sen_ptr);
- sen_ptr->addProcessor(prc_ptr);
+ throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
+
+ ProcessorBasePtr prc_ptr = AutoConfFactoryProcessor::create(_prc_type, _unique_processor_name, _server);
- // 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());
+ //Dimension check
+ int prc_dim = prc_ptr->getDim();
+ auto prb = this;
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
+ prc_ptr->configure(sen_ptr);
+ prc_ptr->link(sen_ptr);
return prc_ptr;
}
-SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
+SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
{
auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
getHardware()->getSensorList().end(),
@@ -233,268 +326,424 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
return (*sen_it);
}
-ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state)
{
- for (auto sen : getHardware()->getSensorList()) // loop all sensors
+ VectorComposite state;
+ SizeEigen index = 0;
+ SizeEigen size = 0;
+ for (const auto& key : _frame_structure)
{
- 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 (key == 'O')
{
- if ((*prc_it)->isMotion()) // found, and it's motion!
- {
- std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(*prc_it);
- return processor_motion_ptr_;
- }
- else // found, but it's not motion!
- {
- std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl;
- return nullptr;
- }
+ if (_dim == 2) size = 1;
+ else size = 4;
}
+ else
+ size = _dim;
+
+ assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");
+
+ state.emplace(key, _frame_state.segment(index, size));
+
+ // append new key to frame structure
+ if (frame_structure_.find(key) == std::string::npos) // not found
+ frame_structure_ += std::string(1,key);
+
+ index += size;
}
- // nothing found!
- std::cout << "Processor '" << _processor_name << "' not found!" << std::endl;
- return nullptr;
-}
-void Problem::setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr)
-{
- processor_motion_ptr_ = _processor_motion_ptr;
-}
+ assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
-void Problem::clearProcessorMotion()
-{
- processor_motion_ptr_.reset();
+ auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
+ _time_stamp,
+ _frame_structure,
+ state);
+
+ return frm;
}
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
- const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim)
{
- auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
- return frm;
+ return emplaceFrame(_time_stamp,
+ _frame_structure,
+ getState(_time_stamp));
}
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
- const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state)
{
- return emplaceFrame(_frame_structure, _dim, _frame_key_type, getState(_time_stamp), _time_stamp);
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _time_stamp,
+ _frame_structure,
+ _frame_state);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state)
{
- return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _frame_state, _time_stamp);
+ // append new keys to frame structure
+ for (const auto& pair_key_vec : _frame_state)
+ {
+ const auto& key = pair_key_vec.first;
+ if (frame_structure_.find(key) == std::string::npos) // not found
+ frame_structure_ += std::string(1,key);
+ }
+
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _time_stamp,
+ getFrameStructure(),
+ _frame_state);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state)
{
- return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
+ return emplaceFrame(_time_stamp,
+ this->getFrameStructure(),
+ this->getDim(),
+ _frame_state);
}
-Eigen::VectorXs Problem::getCurrentState()
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
{
- Eigen::VectorXs state(getFrameStructureSize());
- getCurrentState(state);
- return state;
+ return emplaceFrame(_time_stamp,
+ this->getFrameStructure(),
+ this->getDim());
}
-void Problem::getCurrentState(Eigen::VectorXs& state)
+TimeStamp Problem::getTimeStamp ( ) const
{
- if (processor_motion_ptr_ != nullptr)
- processor_motion_ptr_->getCurrentState(state);
- else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
- trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state);
- else
- state = zeroState();
+ TimeStamp ts = TimeStamp::Invalid();
+
+ for (const auto& prc_pair : processor_is_motion_map_)
+ if (prc_pair.second->getTimeStamp().ok())
+ if ( (not ts.ok() ) or prc_pair.second->getTimeStamp() > ts)
+ ts = prc_pair.second->getTimeStamp();
+
+
+ if (not ts.ok())
+ {
+ const auto& last_kf = trajectory_ptr_->getLastFrame();
+
+ if (last_kf)
+ ts = last_kf->getTimeStamp(); // Use last estimated frame's state
+
+ }
+
+ WOLF_DEBUG_COND(not ts.ok(), "Problem has nowhere to find a valid timestamp!");
+
+ return ts;
}
-void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
+
+VectorComposite Problem::getState(const StateStructure& _structure) const
{
- if (processor_motion_ptr_ != nullptr)
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
+ VectorComposite state;
+
+ // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state
+ for (const auto& prc_pair : processor_is_motion_map_)
{
- processor_motion_ptr_->getCurrentState(state);
- processor_motion_ptr_->getCurrentTimeStamp(ts);
+ const auto& prc_state = prc_pair.second->getState(structure);
+
+ // transfer processor vector blocks to problem state
+ for (const auto& pair_key_vec : prc_state)
+ {
+ if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
+ state.insert(pair_key_vec);
+ }
}
- else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
+
+ // check for empty blocks and fill them with the last KF, with the prior, or with zeros in the worst case
+
+ VectorComposite state_last;
+
+ const auto& last_kf = trajectory_ptr_->getLastFrame();
+
+ if (last_kf)
+ state_last = last_kf->getState(structure);
+
+ else if (prior_options_ and prior_options_->mode != "nothing")
+ state_last = prior_options_->state;
+
+ for (const auto& key : structure)
{
- trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
- trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state);
+ if (state.count(key) == 0)
+ {
+ auto state_last_it = state_last.find(key);
+
+ if (state_last_it != state_last.end())
+ state.emplace(key, state_last_it->second);
+
+ else
+ state.emplace(key, stateZero(string(1,key)).at(key));
+ }
}
- else
- state = zeroState();
+
+ return state;
}
-void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
+VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
{
- // try to get the state from processor_motion if any, otherwise...
- if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
+ VectorComposite state;
+
+ // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state
+ for (const auto& prc_pair : processor_is_motion_map_)
{
- FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
- if (closest_frame != nullptr)
- closest_frame->getState(state);
- else
- state = zeroState();
+ const auto& prc_state = prc_pair.second->getState(_ts, structure);
+
+ // transfer processor vector blocks to problem state
+ for (const auto& pair_key_vec : prc_state)
+ {
+ if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
+ state.insert(pair_key_vec);
+ }
+ }
+
+ // check for empty blocks and fill them with the closest KF to ts, with the prior, or with zeros in the worst case
+
+ VectorComposite state_last;
+
+ const auto& last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);
+
+ if (last_kf)
+ state_last = last_kf->getState(structure);
+
+ else if (prior_options_ and prior_options_->mode != "nothing")
+ state_last = prior_options_->state;
+
+ for (const auto& key : structure)
+ {
+ if (state.count(key) == 0)
+ {
+ auto state_last_it = state_last.find(key);
+
+ if (state_last_it != state_last.end())
+ state.emplace(key, state_last_it->second);
+
+ else
+ state.emplace(key, stateZero(string(1,key)).at(key));
+ }
}
-}
-Eigen::VectorXs Problem::getState(const TimeStamp& _ts)
-{
- Eigen::VectorXs state(getFrameStructureSize());
- getState(_ts, state);
return state;
}
-SizeEigen Problem::getFrameStructureSize() const
+VectorComposite Problem::getOdometry(const StateStructure& _structure) const
{
- return state_size_;
-}
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
-void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const
-{
- _x_size = state_size_;
- _cov_size = state_cov_size_;
+ VectorComposite odom_state;
+
+ // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state
+ for (const auto& prc_pair : processor_is_motion_map_)
+ {
+ const auto& prc_state = prc_pair.second->getOdometry();
+
+ // transfer processor vector blocks to problem state
+ for (const auto& pair_key_vec : prc_state)
+ {
+ if (odom_state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
+ odom_state.insert(pair_key_vec);
+ }
+ }
+
+ // check for empty blocks and fill them with the the prior, or with zeros in the worst case
+
+ VectorComposite state_last;
+
+ if (prior_options_ and prior_options_->mode != "nothing")
+ state_last = prior_options_->state;
+
+ for (const auto& key : structure)
+ {
+ if (odom_state.count(key) == 0)
+ {
+ auto state_last_it = state_last.find(key);
+
+ if (state_last_it != state_last.end())
+ odom_state.emplace(key, state_last_it->second);
+
+ else
+ odom_state.emplace(key, stateZero(string(1,key)).at(key));
+ }
+ }
+
+ return odom_state;
}
SizeEigen Problem::getDim() const
{
return dim_;
}
-std::string Problem::getFrameStructure() const
+
+const StateStructure& Problem::getFrameStructure() const
{
return frame_structure_;
}
-Eigen::VectorXs Problem::zeroState()
+void Problem::appendToStructure(const StateStructure& _structure)
{
- Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());
-
- // Set the quaternion identity for 3D states. Set only the real part to 1:
- if(this->getDim() == 3)
- state(6) = 1.0;
- return state;
+ for (const auto& key : _structure)
+ if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
+ frame_structure_ += key;
}
-bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
+void Problem::setTreeManager(TreeManagerBasePtr _gm)
{
- // 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.
+ if (tree_manager_)
+ tree_manager_->setProblem(nullptr);
+ tree_manager_ = _gm;
+ tree_manager_->setProblem(shared_from_this());
- // Currently allowing all processors to vote:
- return true;
}
-void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
+void Problem::addProcessorIsMotion(IsMotionPtr _is_motion_ptr)
{
- if (_processor_ptr)
+ // Check if is state getter
+ if (not _is_motion_ptr->isStateGetter())
{
- WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+ WOLF_WARN("Problem::addProcessorIsMotion: adding a IsMotion processor with state_getter=false. Not adding this processor");
+ return;
}
- else
+
+ // check duplicated priority
+ while (processor_is_motion_map_.count(_is_motion_ptr->getStatePriority()) == 1)
{
- WOLF_DEBUG("External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+ WOLF_ERROR("Problem::addProcessorIsMotion: adding a IsMotion processor with priority = ",
+ _is_motion_ptr->getStatePriority(),
+ " which is already taken. Trying to add it with priority = ",
+ _is_motion_ptr->getStatePriority()+1);
+ _is_motion_ptr->setStatePriority(_is_motion_ptr->getStatePriority()+1);
}
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if (processor && (processor != _processor_ptr) )
- processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
+ // add to map ordered by priority
+ processor_is_motion_map_.emplace(_is_motion_ptr->getStatePriority(), _is_motion_ptr);
+ appendToStructure(_is_motion_ptr->getStateStructure());
}
-bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr)
+void Problem::removeProcessorIsMotion(IsMotionPtr proc)
{
- // This should implement a black list of processors that have forbidden auxiliary frame creation
- // This decision is made at problem level, not at processor configuration level.
- // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.
+ WOLF_WARN_COND(processor_is_motion_map_.count(proc->getStatePriority()) == 0, "Problem::clearProcessorIsMotion: missing processor");
- // Currently allowing all processors to vote:
- return true;
+ processor_is_motion_map_.erase(proc->getStatePriority());
+
+ // rebuild frame structure with remaining motion processors
+ frame_structure_.clear();
+ for (const auto& pm : processor_is_motion_map_)
+ appendToStructure(pm.second->getStateStructure());
}
-void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
+VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
{
- if (_processor_ptr)
- {
- WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
- }
- else
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
+ VectorComposite state;
+ for (const auto& key : structure)
{
- WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+ VectorXd vec;
+ if (key == 'O')
+ {
+ if (dim_ == 2) vec = VectorXd::Zero(1);
+ else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
+ }
+ else
+ {
+ vec = VectorXd::Zero(dim_);
+ }
+ state.emplace(key, vec);
}
-
- processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+ return state;
}
-LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
+bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
{
- getMap()->addLandmark(_lmk_ptr);
- return _lmk_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.
-void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
-{
- getMap()->addLandmarkList(_lmk_list);
+ // Currently allowing all processors to vote:
+ return true;
}
-StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
+void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
- //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
+ WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+ WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
- // 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;
+ // pause processor profiling
+ if (_processor_ptr)
+ _processor_ptr->stopCaptureProfiling();
- return _state_ptr;
+ for (auto sensor : hardware_ptr_->getSensorList())
+ for (auto processor : sensor->getProcessorList())
+ if (processor && (processor != _processor_ptr) )
+ {
+#ifdef PROFILING
+ auto start = std::chrono::high_resolution_clock::now();
+#endif
+ processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
+#ifdef PROFILING
+ auto stop = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
+ WOLF_INFO("keyFrameCallback Prc_id: ", processor->id(),
+ " Prc_type: ", processor->getType(),
+ " Prc_name: ", processor->getName(),
+ " keyframe_id: ", _keyframe_ptr->id(),
+ " keyframe_type: ", _keyframe_ptr->getType(),
+ " timestamp: ", _keyframe_ptr->getTimeStamp(),
+ " microseconds: ", duration.count());
+#endif
+ }
+
+ // notify tree manager
+ if (tree_manager_)
+ tree_manager_->keyFrameCallback(_keyframe_ptr);
+
+ // resume processor profiling
+ if (_processor_ptr)
+ _processor_ptr->startCaptureProfiling();
}
-void Problem::removeStateBlock(StateBlockPtr _state_ptr)
+StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
- //std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
+ std::lock_guard<std::mutex> lock(mut_notifications_);
+ //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
+
+ // exsiting notification for this state block
if (notification_it != state_block_notification_map_.end())
{
- if (notification_it->second == REMOVE)
+ // duplicated notification
+ if ( notification_it->second == _noti)
{
- WOLF_WARN("There is already an REMOVE notification of this state block");
+ WOLF_WARN("This notification has been already notified");
}
- // Remove ADD notification
+ // opposite notification -> cancell out eachother
else
- {
state_block_notification_map_.erase(notification_it);
- }
}
- // Add REMOVE notification
+ // Add notification
else
- state_block_notification_map_[_state_ptr] = REMOVE;
+ state_block_notification_map_[_state_ptr] = _noti;
+
+ return _state_ptr;
}
bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
if (state_block_notification_map_.find(sb_ptr) == state_block_notification_map_.end())
return false;
@@ -502,50 +751,38 @@ bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notificatio
return true;
}
-FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
+FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _noti)
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
//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::lock_guard<std::mutex> lock(mut_factor_notifications_);
- //std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
-
- // Check if there is already a notification for this state block
+ // Check if there is already the same notification for this factor
auto notification_it = factor_notification_map_.find(_factor_ptr);
+ // exsiting notification for this factor
if (notification_it != factor_notification_map_.end())
{
- if (notification_it->second == REMOVE)
+ // duplicated notification
+ if (notification_it->second == _noti)
{
- WOLF_WARN("There is already an REMOVE notification of this state block");
+ WOLF_WARN("This notification has been already notified");
}
- // Remove ADD notification
+ // opposite notification -> cancell out eachother
else
+ {
factor_notification_map_.erase(notification_it);
+ }
+
}
- // Add REMOVE notification
+ // Add notification
else
- factor_notification_map_[_factor_ptr] = REMOVE;
+ factor_notification_map_[_factor_ptr] = _noti;
+
+ return _factor_ptr;
}
bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
if (factor_notification_map_.find(fac_ptr) == factor_notification_map_.end())
return false;
@@ -559,7 +796,7 @@ void Problem::clearCovariance()
covariances_.clear();
}
-void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov)
+void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov)
{
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
@@ -568,7 +805,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
}
-void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov)
+void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov)
{
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
@@ -577,8 +814,8 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row,
- const int _col)
+bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row,
+ const int _col) const
{
//std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
//std::cout << "_row " << _row << std::endl;
@@ -597,10 +834,10 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
+ covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2));
else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose();
+ covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)).transpose();
else
{
WOLF_DEBUG("Could not find the requested covariance block.");
@@ -610,7 +847,7 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
return true;
}
-bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
+bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
{
std::lock_guard<std::mutex> lock(mut_covariances_);
@@ -631,8 +868,8 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_12];
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_12].transpose();
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_12);
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_12).transpose();
}
else if (covariances_.find(pair_21) != covariances_.end())
{
@@ -641,8 +878,8 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_21].transpose();
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_21];
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_21).transpose();
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_21);
}
else
return false;
@@ -651,107 +888,112 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
return true;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col)
+bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
{
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
-bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance)
+bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const
{
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->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ SizeEigen sz = _frame_ptr->getLocalSize();
+ _covariance.resize(sz, sz);
// filling covariance
- for (const auto& sb_i : state_bloc_vec)
+ int i = 0, j = 0;
+ for (const auto& sb_i : _frame_ptr->getStateBlockVec())
{
- if (sb_i)
+ j = 0;
+ for (const auto& sb_j : _frame_ptr->getStateBlockVec())
{
- 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->getLocalSize();
- }
- }
- i += sb_i->getLocalSize();
+ success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+ j += sb_j->getLocalSize();
}
+ i += sb_i->getLocalSize();
}
- return success;
-}
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
-{
- FrameBasePtr frm = getLastKeyFrame();
- return getFrameCovariance(frm, cov);
+ return success;
}
-bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov)
+bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const
{
- FrameBasePtr frm = getLastKeyOrAuxFrame();
+ FrameBasePtr frm = getLastFrame();
return getFrameCovariance(frm, cov);
}
-bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
+bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const
{
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->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ SizeEigen sz = _landmark_ptr->getLocalSize();
+ _covariance.resize(sz, sz);
// filling covariance
-
- for (const auto& sb_i : state_bloc_vec)
+ int i = 0, j = 0;
+ for (const auto& sb_i : _landmark_ptr->getStateBlockVec())
{
- if (sb_i)
+ j = 0;
+ for (const auto& sb_j : _landmark_ptr->getStateBlockVec())
{
- 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->getLocalSize();
- }
- }
- i += sb_i->getLocalSize();
+ success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+ j += sb_j->getLocalSize();
}
+ i += sb_i->getLocalSize();
}
+
return success;
-}
-MapBasePtr Problem::getMap()
+
+
+// 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->getLocalSize();
+//
+// // 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->getLocalSize();
+// }
+// }
+// i += sb_i->getLocalSize();
+// }
+// }
+// return success;
+}
+
+MapBasePtr Problem::getMap() const
{
return map_ptr_;
}
-TrajectoryBasePtr Problem::getTrajectory()
+TrajectoryBasePtr Problem::getTrajectory() const
{
return trajectory_ptr_;
}
-HardwareBasePtr Problem::getHardware()
+HardwareBasePtr Problem::getHardware() const
{
return hardware_ptr_;
}
@@ -761,71 +1003,151 @@ FrameBasePtr Problem::getLastFrame() const
return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::getLastKeyFrame() const
+FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
{
- return trajectory_ptr_->getLastKeyFrame();
+ return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
-FrameBasePtr Problem::getLastKeyOrAuxFrame() const
+void Problem::setPriorOptions(const std::string& _mode,
+ const double _time_tolerance ,
+ const VectorComposite& _state ,
+ const VectorComposite& _sigma )
{
- return trajectory_ptr_->getLastKeyOrAuxFrame();
-}
+ assert(prior_options_ != nullptr && "prior options have already been applied");
+ assert(prior_options_->mode == "" && "prior options have already been set");
+ assert((_mode == "nothing" || _mode == "initial_guess" || _mode == "fix" || _mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
-{
- return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-}
+ // Store options (optionals depending on the mode)
+ WOLF_TRACE("prior mode: ", _mode);
+ prior_options_->mode = _mode;
-FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
-{
- return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ if (prior_options_->mode != "nothing")
+ {
+ assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
+ assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
+
+ WOLF_TRACE("prior state: ", _state);
+ WOLF_TRACE("prior time tolerance: ", _time_tolerance);
+ prior_options_->state = _state;
+ prior_options_->time_tolerance = _time_tolerance;
+
+ if (prior_options_->mode == "factor")
+ {
+ assert(_sigma.includesStructure(frame_structure_) && "any missing key in prior sigma");
+
+ bool isPositive = true;
+ for(const auto& it: _sigma)
+ isPositive = isPositive and (it.second.array() > Constants::EPS).all();
+
+ assert(isPositive && "prior sigma is not positive");
+
+ MatrixComposite Q;
+ for (const auto& pair_key_sig : _sigma)
+ {
+ const auto& key = pair_key_sig.first;
+ const auto& sig_blk = pair_key_sig.second;
+
+ const auto& cov_blk = (sig_blk.array() * sig_blk.array()).matrix().asDiagonal();
+ Q.emplace(key,key,cov_blk);
+ }
+ WOLF_TRACE("prior covariance:" , Q);
+ prior_options_->cov = Q;
+ }
+ }
}
-FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
+FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
- if ( ! prior_is_set_ )
+ assert(!isPriorSet() && "applyPriorOptions can be called once!");
+
+ FrameBasePtr prior_keyframe(nullptr);
+
+ if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- // Create origin frame
- FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);
-
- // create origin capture with the given state as data
- // Capture fix only takes 3D position and Quaternion orientation
- CapturePosePtr init_capture;
- CaptureBasePtr init_capture_base;
- // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
- if (this->getFrameStructure() == "POV" and this->getDim() == 3)
- init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- else
- init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+ prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
+
+ // Update origin for odometry processors
+ for (auto proc_pair : processor_is_motion_map_)
+ proc_pair.second->setOdometry(prior_options_->state);
+
+ if (prior_options_->mode == "fix")
+ prior_keyframe->fix();
+ else if (prior_options_->mode == "factor")
+ {
+ // ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)
- init_capture = std::static_pointer_cast<CapturePose>(init_capture_base);
- // origin_keyframe->addCapture(init_capture);
+ // Emplace a capture
+ auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
- // emplace feature and factor
- init_capture->emplaceFeatureAndFactor();
+ // Emplace a feature and a factor for each state block
+ for (const auto& pair_key_sb : prior_keyframe->getStateBlockMap())
+ {
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
- WOLF_DEBUG("Set prior callback: KF", origin_keyframe->id(), " Callback emitted with ts = ", origin_keyframe->getTimeStamp());
+ const auto& state_blk = prior_options_->state.at(key);
+ const auto& covar_blk = prior_options_->cov.at(key,key);
- // Notify all motion processors 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);
+ assert(sb->getSize() == state_blk.size() && "prior_options state wrong size");
+ assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
- prior_is_set_ = true;
+ // feature
+ auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), state_blk, covar_blk);
- // 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);
+ // factor
+ if (sb->hasLocalParametrization())
+ {
+ if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
+ auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
+ else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
+ else
+ throw std::runtime_error("not implemented...!");
+ }
+ else
+ {
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
+ }
- return origin_keyframe;
+ }
+
+ }
+ else
+ assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
+
+ // notify all processors
+ keyFrameCallback(prior_keyframe, nullptr, prior_options_->time_tolerance);
}
- else
- throw std::runtime_error("Origin already set!");
+ // remove prior options
+ prior_options_ = nullptr;
+
+ return prior_keyframe;
+}
+
+FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_sigma,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("factor", _time_tol, _state, _sigma);
+ return applyPriorOptions(_ts);
+}
+
+
+FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("fix", _time_tol, _state);
+ return applyPriorOptions(_ts);
+}
+
+FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("initial_guess", _time_tol, _state);
+ return applyPriorOptions(_ts);
}
void Problem::loadMap(const std::string& _filename_dot_yaml)
@@ -838,670 +1160,84 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
+void Problem::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream) const
{
- 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()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
- << pm->getOrigin()->getFrame()->id() << endl;
- if (pm->getLast())
- cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " 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()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
-// << ptt->getPrevOrigin()->getFrame()->id() << endl;
-// }
- if (pt->getOrigin())
- cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getOrigin()->getFrame()->id() << endl;
- if (pt->getLast())
- cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " 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->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " 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;
+ _stream << std::endl;
+ _stream << "P: wolf tree status ---------------------" << std::endl;
+
+ getHardware()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
+
+ getTrajectory()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
+
+ getMap()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
+
+ _stream << "-----------------------------------------" << std::endl;
+ _stream << std::endl;
}
-bool Problem::check(int verbose_level)
+bool Problem::check(bool _verbose, std::ostream& _stream) const
{
- using std::cout;
- using std::endl;
- bool is_consistent = true; // true if all checks passed; false if any check fails.
+ CheckLog log(true, "");
+ log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
+
+ std::string tabs = "";
- if (verbose_level) cout << endl;
- if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl;
- auto P_raw = this;
- if (verbose_level > 0)
+ if (_verbose) _stream << std::endl;
+ if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
+ auto P = shared_from_this();
+ if (_verbose)
{
- cout << "P @ " << P_raw << endl;
+ _stream << "Prb @ " << P.get() << std::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);
- }
- }
+ H->check(log, H, _verbose, _stream, tabs);
// ------------------------
// 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->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " 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);
+ T->check(log, T, _verbose, _stream, tabs);
- // 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);
+ M->check(log, M, _verbose, _stream, tabs);
- // 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;
+ if (_verbose) _stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
+ if (_verbose) _stream << std::endl;
+ if (_verbose and not log.is_consistent_) _stream << log.explanation_ << std::endl;
- return is_consistent;
+ return log.is_consistent_;
}
-void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks)
+bool Problem::check(int _verbose_level) const
{
- 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);
+ return check((_verbose_level > 0), std::cout);
+}
+
+void Problem::perturb(double amplitude)
+{
+ // Sensors
+ for (auto& S : getHardware()->getSensorList())
+ S->perturb(amplitude);
+
+ // Frames and Captures
+ for (auto& F : *(getTrajectory()))
+ {
+ F->perturb(amplitude);
+ for (auto& C : F->getCaptureList())
+ C->perturb(amplitude);
+ }
+
+ // Landmarks
+ for (auto& L : getMap()->getLandmarkList())
+ L->perturb(amplitude);
}
} // namespace wolf
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57333a05529ea41f3866cf7c52fb782c695ae3ca
--- /dev/null
+++ b/src/processor/is_motion.cpp
@@ -0,0 +1,23 @@
+#include "core/processor/is_motion.h"
+#include "core/problem/problem.h"
+
+using namespace wolf;
+
+void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
+{
+ setOdometry(_prb_ptr->stateZero(state_structure_));
+ if (not isStateGetter())
+ {
+ WOLF_WARN("IsMotion::addToProblem: IsMotion processor with state_getter=false. Not adding this processor");
+ return;
+ }
+ _prb_ptr->addProcessorIsMotion(_motion_ptr);
+}
+
+void IsMotion::setOdometry(const VectorComposite& _odom)
+{
+ assert(_odom.includesStructure(state_structure_) && "IsMotion::setOdometry(): any key missing in _odom.");
+
+ for (auto key : state_structure_)
+ odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_
+}
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 5735441cb55404882bf9cd32c712a843daa61324..52383b14cbf0cb7c355f416ef96a20e726bd711b 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -3,15 +3,15 @@ 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) :
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _delta,
+ const MatrixXd& _delta_cov,
+ const VectorXd& _delta_integr,
+ const MatrixXd& _delta_integr_cov,
+ const MatrixXd& _jac_delta,
+ const MatrixXd& _jac_delta_int,
+ const MatrixXd& _jac_calib) :
data_size_(_data.size()),
delta_size_(_delta.size()),
cov_size_(_delta_cov.cols()),
@@ -62,27 +62,19 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
////////////////////////////////////////////////////////////////////////////////////////
-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)
+MotionBuffer::MotionBuffer()
{
- container_.clear();
+ this->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)
+ //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->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--;
@@ -92,12 +84,12 @@ const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
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)
+ //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->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--;
@@ -107,73 +99,29 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
{
- assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
- assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
-
- _buffer_part_before_ts.setCalibrationPreint(calib_preint_);
+ assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
+ assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->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();
+ _buffer_part_before_ts.clear();
}
else
{
// Transfer part of the buffer
- _buffer_part_before_ts.container_.splice(_buffer_part_before_ts.container_.begin(),
- container_,
- container_.begin(),
+ _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(),
+ *this,
+ this->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)
{
@@ -182,19 +130,17 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
if (!show_data && !show_delta && !show_delta_int && !show_jacs)
{
- cout << "Buffer state [" << container_.size() << "] : <";
- for (Motion mot : container_)
+ cout << "Buffer state [" << this->size() << "] : <";
+ for (Motion mot : *this)
cout << " " << mot.ts_;
cout << " >" << endl;
}
else
{
print(0,0,0,0);
- for (Motion mot : container_)
+ for (Motion mot : *this)
{
cout << "-- Motion (" << mot.ts_ << ")" << endl;
-// if (show_ts)
-// cout << " ts: " << mot.ts_ << endl;
if (show_data)
{
cout << " data: " << mot.data_.transpose() << endl;
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 07da090c9b6e789f3d942a8aef226665927972cf..28963ed644aacd240d2d01a0bb23e6280dc6619a 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -7,11 +7,18 @@ namespace wolf {
unsigned int ProcessorBase::processor_id_count_ = 0;
-ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params) :
+ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ParamsProcessorBasePtr _params) :
NodeBase("PROCESSOR", _type),
processor_id_(++processor_id_count_),
params_(_params),
- sensor_ptr_()
+ dim_(_dim),
+ sensor_ptr_(),
+ n_capture_callback_(0),
+ n_kf_callback_(0),
+ acc_duration_capture_(0),
+ acc_duration_kf_(0),
+ max_duration_capture_(0),
+ max_duration_kf_(0)
{
// WOLF_DEBUG("constructed +p" , id());
}
@@ -26,35 +33,50 @@ bool ProcessorBase::permittedKeyFrame()
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
}
-bool ProcessorBase::permittedAuxFrame()
+void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other)
{
- return isVotingAuxActive() && getProblem()->permitAuxFrame(shared_from_this());
-}
+ assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
+ WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
-FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr)
-{
- std::cout << "Making " << (_type == KEY ? "key-" : (_type == AUXILIARY ? "aux-" : "")) << "frame" << std::endl;
+ // profiling
+ n_kf_callback_++;
+ startKFProfiling();
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp());
- // new_frame_ptr->addCapture(_capture_ptr);
- _capture_ptr->link(new_frame_ptr);
+ // asking if key frame should be stored
+ if (storeKeyFrame(_keyframe_ptr))
+ buffer_pack_kf_.add(_keyframe_ptr, _time_tol_other);
- return new_frame_ptr;
+ // if trigger true -> processKeyFrame
+ if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other))
+ processKeyFrame(_keyframe_ptr, _time_tol_other);
+
+ // profiling
+ stopKFProfiling();
}
-FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state)
+void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
{
- FrameBasePtr new_frame_ptr = emplaceFrame(_type, _capture_ptr);
- new_frame_ptr->setState(_state);
+ assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
+ WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
- return new_frame_ptr;
-}
+ // profiling
+ n_capture_callback_++;
+ startCaptureProfiling();
-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);
+ // apply prior in problem if not done (very first capture)
+ if (getProblem() && !getProblem()->isPriorSet())
+ getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
+
+ // asking if capture should be stored
+ if (storeCapture(_capture_ptr))
+ buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
+
+ // if trigger, process directly without buffering
+ if (triggerInCapture(_capture_ptr))
+ processCapture(_capture_ptr);
+
+ // profiling
+ stopCaptureProfiling();
}
void ProcessorBase::remove()
@@ -64,54 +86,69 @@ void ProcessorBase::remove()
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();
+ auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() );
+ if(P && this_proc_cast_attempt )
+ P->removeProcessorIsMotion(this_proc_cast_attempt);
}
// remove from upstream
SensorBasePtr sen = sensor_ptr_.lock();
if(sen)
- sen->getProcessorList().remove(this_p);
+ sen->removeProcessor(this_p);
}
}
- void ProcessorBase::link(SensorBasePtr _sen_ptr)
+
+void ProcessorBase::link(SensorBasePtr _sen_ptr)
+{
+ assert(!is_removing_ && "linking a removed processor");
+ assert(this->getSensor() == nullptr && "linking an already linked processor");
+
+ if(_sen_ptr)
{
- if(_sen_ptr)
- {
- _sen_ptr->addProcessor(shared_from_this());
- this->setSensor(_sen_ptr);
- this->setProblem(_sen_ptr->getProblem());
- }
- else
- {
- WOLF_WARN("Linking with a nullptr");
- }
+ _sen_ptr->addProcessor(shared_from_this());
+ this->setSensor(_sen_ptr);
+ this->setProblem(_sen_ptr->getProblem());
}
-/////////////////////////////////////////////////////////////////////////////////////////
+ else
+ {
+ WOLF_WARN("Linking Processor ", id(), " to a nullptr");
+ }
+}
-void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp)
+void ProcessorBase::setProblem(ProblemPtr _problem)
{
- PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
- container_.erase(container_.begin(), post); // erasing by range
+ std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+ assert(((dim_ == 0) or (dim_ == _problem->getDim())) && str.c_str());
+
+ if (_problem == nullptr or _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+
+ // adding processor is motion to the processor is motion vector
+ auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this());
+ if (is_motion_ptr)
+ is_motion_ptr->addToProblem(_problem, is_motion_ptr);
}
-void PackKeyFrameBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
+/////////////////////////////////////////////////////////////////////////////////////////
+
+void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const double& _time_tolerance)
{
TimeStamp time_stamp = _key_frame->getTimeStamp();
PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
- container_.emplace(time_stamp, kfpack);
+ Buffer::add(time_stamp, kfpack);
}
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
if (container_.empty())
return nullptr;
- PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
+ BufferPackKeyFrame::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())
@@ -122,13 +159,13 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
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);
+ bool post_ok = post_exists && doubleCheckTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
if (prev_exists)
{
- PackKeyFrameBuffer::Iterator prev = std::prev(post);
+ BufferPackKeyFrame::Iterator prev = std::prev(post);
- bool prev_ok = checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
+ bool prev_ok = doubleCheckTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev->second;
@@ -149,12 +186,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
return nullptr;
}
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectPack(const CaptureBasePtr _capture, const double& _time_tolerance)
{
return selectPack(_capture->getTimeStamp(), _time_tolerance);
}
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
// remove packs corresponding to removed KFs
while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
@@ -170,7 +207,7 @@ PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time
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))
+ if (doubleCheckTimeTolerance(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)
@@ -178,12 +215,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time
}
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const CaptureBasePtr _capture, const double& _time_tolerance)
{
return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
}
-void PackKeyFrameBuffer::print(void)
+void BufferPackKeyFrame::print(void) const
{
std::cout << "[ ";
for (auto iter : container_)
@@ -193,12 +230,65 @@ void PackKeyFrameBuffer::print(void)
std::cout << "]" << std::endl;
}
-bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
- const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
+void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+}
+
+void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
+CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ auto sen_ptr = getSensor();
+ if (_verbose)
+ {
+ _stream << _tabs << "Prc" << id() << " @ " << _prc_ptr.get() << " -> Sen" << sen_ptr->id() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Sen" << sen_ptr->id() << " @ " << sen_ptr.get() << std::endl;
+ }
+
+ // check problem and sensor pointers
+ inconsistency_explanation << "Processor problem pointer " << getProblem().get()
+ << " is different from Sensor problem pointer " << sen_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == sen_ptr->getProblem()), inconsistency_explanation);
+
+ inconsistency_explanation << "Prc" << id() << " @ " << _prc_ptr
+ << " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr
+ << " -X-> Prc" << id();
+ auto sen_prc_list = sen_ptr->getProcessorList();
+ auto sen_has_prc = std::find_if(sen_prc_list.begin(), sen_prc_list.end(), [&_prc_ptr](ProcessorBasePtr prc){ return prc == _prc_ptr;});
+ log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation);
+
+ return log;
+}
+bool ProcessorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto prc_ptr = std::static_pointer_cast<ProcessorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, prc_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ return _log.is_consistent_;
+}
+
+void ProcessorBase::printProfiling(std::ostream& _stream) const
{
- 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;
+ _stream << "\n" << getType() << " - " << getName() << ":"
+ << "\n\ttotal time: " << 1e-6*(acc_duration_capture_ + acc_duration_kf_).count()<< " s"
+ << "\n\tProcessing captures:"
+ << "\n\t\ttotal time: " << 1e-6*acc_duration_capture_.count() << " s"
+ << "\n\t\tcaptures processed: " << n_capture_callback_
+ << "\n\t\taverage time: " << 1e-3*acc_duration_capture_.count() / n_capture_callback_ << " ms"
+ << "\n\t\tmax time: " << 1e-3*max_duration_capture_.count() << " ms"
+ << "\n\tProcessing keyframes:"
+ << "\n\t\ttotal time: " << 1e-6*acc_duration_kf_.count() << " s"
+ << "\n\t\tkf processed: " << n_kf_callback_
+ << "\n\t\taverage time: " << 1e-3*acc_duration_kf_.count() / n_kf_callback_ << " ms"
+ << "\n\t\tmax time: " << 1e-3*max_duration_kf_.count() << " ms" << std::endl;
+
}
+
} // namespace wolf
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
deleted file mode 100644
index d8a98166c11ecc1361ed1194a5b2ad0e1b241cb6..0000000000000000000000000000000000000000
--- a/src/processor/processor_capture_holder.cpp
+++ /dev/null
@@ -1,152 +0,0 @@
-/**
- * \file processor_capture_holder.h
- *
- * Created on: Jul 12, 2017
- * \author: Jeremie Deray
- */
-
-//Wolf includes
-#include "core/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);
- existing_capture->link(_keyframe_ptr);
-
- //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 "core/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
index 6c508a35a1f25693e7fa350945dab18b0c020cf2..43f15c52d8e0eee217a4c824c67bca1e9ae5b9e3 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -1,273 +1,180 @@
+/**
+ * \file processor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
+
#include "core/processor/processor_diff_drive.h"
#include "core/sensor/sensor_diff_drive.h"
+#include "core/feature/feature_motion.h"
+#include "core/factor/factor_diff_drive.h"
-#include "core/capture/capture_wheel_joint_position.h"
-#include "core/capture/capture_velocity.h"
-
-#include "core/math/rotations.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/feature/feature_diff_drive.h"
+#include "core/math/SE2.h"
namespace wolf
{
-ProcessorDiffDrive::ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params) :
- ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 3, _params),
- params_motion_diff_drive_(_params)
+ProcessorDiffDrive::ProcessorDiffDrive(const ParamsProcessorDiffDrivePtr _params) :
+ ProcessorOdom2d(_params),
+ params_prc_diff_drv_selfcal_(_params),
+ radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor).
{
- unmeasured_perturbation_cov_ = Matrix3s::Identity() * params_motion_diff_drive_->unmeasured_perturbation_std * params_motion_diff_drive_->unmeasured_perturbation_std;
+ setType("ProcessorDiffDrive"); // overwrite odom2d setting
+ calib_size_ = 3; // overwrite odom2d setting
+ unmeasured_perturbation_cov_ = Matrix1d(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2d setting
}
-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)
+ProcessorDiffDrive::~ProcessorDiffDrive()
{
- 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 =
- *(std::static_pointer_cast<SensorDiffDrive>(getSensor())->getIntrinsics());
-
- VelocityComand<Scalar> vel;
- Eigen::MatrixXs J_vel_data;
- Eigen::MatrixXs J_vel_calib;
-
- 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_,
- _calib,
- _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_,
- _calib, _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_,
- // _calib, _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_;
-
- _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)
+void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
- 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(_sensor && "Provided sensor is nullptr");
- /// Simple 2d frame composition
+ SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
- _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));
+ radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
-void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar /*_Dt2*/,
- Eigen::VectorXs& _delta1_plus_delta2,
- MatrixXs& _jacobian1, MatrixXs& _jacobian2)
+
+
+//////////// MOTION INTEGRATION ///////////////
+
+void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
- using std::sin;
- using std::cos;
+ /*
+ * Differential drive model -----------------------------------------------
+ *
+ * Variables:
+ * k : encoder + gear multiplication, [radians per encoder tick]
+ * r_r, r_l : right and left wheel radii [m]
+ * d : wheel separation [m]
+ * dl : arc length [m]
+ * dth : arc angle [rad]
+ */
+ const double& k = radians_per_tick_;
+ const double& r_l = _calib(0); // wheel radii
+ const double& r_r = _calib(1);
+ const double& d = _calib(2); // wheel separation
+ double dl = k * (_data(1) * r_r + _data(0) * r_l) / 2.0;
+ double dth = k * (_data(1) * r_r - _data(0) * r_l) / d;
- 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
+ /// 1. Tangent space: calibrate and go to se2 tangent -----------------------------------------------
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() +
- Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+ Vector3d tangent(dl, 0, dth); // second value 'ds' is wheel sideways motion, supposed zero. Will have a covariance for allowing skidding.
- _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+ Matrix3d J_tangent_calib;
+ J_tangent_calib(0,0) = k * _data(0) / 2.0; // d dl / d r_l
+ J_tangent_calib(0,1) = k * _data(1) / 2.0; // d dl / d r_r
+ J_tangent_calib(0,2) = 0.0; // d dl / d d
+ J_tangent_calib(1,0) = 0.0; // d ds / d r_l
+ J_tangent_calib(1,1) = 0.0; // d ds / d r_r
+ J_tangent_calib(1,2) = 0.0; // d ds / d d
+ J_tangent_calib(2,0) = - k * _data(0) / d; // d dth / d r_l
+ J_tangent_calib(2,1) = k * _data(1) / d; // d dth / d r_r
+ J_tangent_calib(2,2) = - dth / d; // d dth / d d
- // 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);
+ Matrix<double, 3, 2> J_tangent_data;
+ J_tangent_data(0,0) = k * r_l / 2.0; // d dl / d data(0)
+ J_tangent_data(0,1) = k * r_r / 2.0; // d dl / d data(1)
+ J_tangent_data(1,0) = 0.0; // d ds / d data(0)
+ J_tangent_data(1,1) = 0.0; // d ds / d data(1)
+ J_tangent_data(2,0) = - k * r_l / d; // d dth / d data(0)
+ J_tangent_data(2,1) = k * r_r / d; // d dth / d data(1)
- // jac wrt delta
- _jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
- _jacobian2.topLeftCorner<2,2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
-}
+ Matrix<double, 3, 1> J_tangent_side;
+ J_tangent_side(0,0) = 0.0; // d dl / d ds
+ J_tangent_side(1,0) = 1.0; // d ds / d ds
+ J_tangent_side(2,0) = 0.0; // d dth / d ds
-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");
+ /// 2. Current delta: go to SE2 manifold -----------------------------------------------
- // 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));
-}
+ Matrix3d J_delta_tangent;
-Eigen::VectorXs ProcessorDiffDrive::deltaZero() const
-{
- return Eigen::VectorXs::Zero(delta_size_);
-}
+ SE2::exp(tangent, _delta, J_delta_tangent);
-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);
+ /// 3. delta covariance -----------------------------------------------
-}
+ // Compose Jacobians -- chain rule
+ Matrix<double, 3, 2> J_delta_data = J_delta_tangent * J_tangent_data;
+ Matrix<double, 3, 1> J_delta_side = J_delta_tangent * J_tangent_side;
-CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
-{
+ // Propagate covariance
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose()
+ + J_delta_side * unmeasured_perturbation_cov_ * J_delta_side.transpose();
- StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic()?
- std::make_shared<StateBlock>(3, false) : nullptr;
+ /// 4. Jacobian of delta wrt calibration params -----------------------------------------------
+
+ _jacobian_calib = J_delta_tangent * J_tangent_calib;
- return std::make_shared<CaptureWheelJointPosition>(_ts, _sensor, _data, _data_cov,
- _frame_origin, nullptr, nullptr, i_ptr);
}
-FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin)
+
+CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin)
{
- // FactorOdom2DPtr fac_odom =
- // std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
- // shared_from_this());
- auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
- shared_from_this());
- // _feature->addFactor(fac_odom);
- // _capture_origin->getFrame()->addConstrainedBy(fac_odom);
-
- return fac_odom;
+ auto cap_motion = CaptureBase::emplace<CaptureDiffDrive>(_frame_own,
+ _ts,
+ _sensor,
+ _data,
+ _data_cov,
+ _capture_origin);
+ setCalibration (cap_motion, _calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
}
-FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- 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_INFO(_capture_motion->getBuffer().getCalibrationPreint());
- WOLF_INFO(_capture_motion->getBuffer().get().back().jacobian_calib_);
+ auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
+ "ProcessorDiffDrive",
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_,
+ _capture_motion->getCalibrationPreint(),
+ _capture_motion->getBuffer().back().jacobian_calib_);
return key_feature_ptr;
}
-ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr)
+FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
+ CaptureBasePtr _capture_origin)
{
- 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>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
+ auto ftr_motion = std::static_pointer_cast<FeatureMotion>(_feature);
+ auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion,
+ ftr_motion,
+ _capture_origin,
+ shared_from_this(),
+ params_prc_diff_drv_selfcal_->apply_loss_function);
+ return fac_odom;
}
-} // namespace wolf
+} /* namespace wolf */
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive)
+WOLF_REGISTER_PROCESSOR(ProcessorDiffDrive);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorDiffDrive);
} // namespace wolf
+
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fix_wing_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7c12e90f8f08005d2f0086d57ad1633534dbe8ea
--- /dev/null
+++ b/src/processor/processor_fix_wing_model.cpp
@@ -0,0 +1,66 @@
+/*
+ * processor_fix_wing_model.cpp
+ *
+ * Created on: Sep 6, 2021
+ * Author: joanvallve
+ */
+
+#include "core/processor/processor_fix_wing_model.h"
+
+#include "core/capture/capture_base.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_velocity_local_direction_3d.h"
+
+namespace wolf
+{
+
+ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params) :
+ ProcessorBase("ProcessorFixWingModel", 3, _params),
+ params_processor_(_params)
+{
+}
+
+ProcessorFixWingModel::~ProcessorFixWingModel()
+{
+}
+
+void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
+{
+ assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
+}
+
+void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+ if (_keyframe_ptr->getV()->isFixed())
+ return;
+
+ if (_keyframe_ptr->getFactorOf(shared_from_this()) != nullptr)
+ return;
+
+ // emplace capture
+ auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
+ _keyframe_ptr->getTimeStamp(), getSensor());
+
+ // emplace feature
+ auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
+ params_processor_->velocity_local,
+ Eigen::Matrix1d(params_processor_->angle_stdev * params_processor_->angle_stdev));
+
+ // emplace factor
+ auto fac = FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
+ fea,
+ params_processor_->min_vel_norm,
+ shared_from_this(),
+ false);
+}
+
+} /* namespace wolf */
+
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorFixWingModel);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixWingModel);
+} // 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 b4f6303b128b37ba74104cddf02dd82e328c08e8..0000000000000000000000000000000000000000
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ /dev/null
@@ -1,506 +0,0 @@
-#include "core/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_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..486be875ac6eb306e6e0a7973742b7f3e52241c7
--- /dev/null
+++ b/src/processor/processor_loop_closure.cpp
@@ -0,0 +1,154 @@
+#include "core/processor/processor_loop_closure.h"
+
+namespace wolf
+{
+
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type,
+ int _dim,
+ ParamsProcessorLoopClosurePtr _params_loop_closure):
+ ProcessorBase(_type, _dim, _params_loop_closure),
+ params_loop_closure_(_params_loop_closure)
+{
+ //
+}
+
+void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
+{
+ /* This function has 3 scenarios:
+ * 1. Capture already linked to a frame (in trajectory) -> process
+ * 2. Capture has a timestamp compatible with any stored frame -> link + process
+ * 3. Otherwise -> store capture (Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one)
+ */
+
+ WOLF_DEBUG("ProcessorLoopClosure::processCapture capture ", _capture->id());
+
+ // CASE 1:
+ if (_capture->getFrame() and _capture->getFrame()->getTrajectory())
+ {
+ WOLF_DEBUG("CASE 1");
+
+ process(_capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+
+ return;
+ }
+
+ // Search for any stored frame within time tolerance of capture
+ auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance);
+
+ // CASE 2:
+ if (_capture->getFrame() == nullptr and frame_pack)
+ {
+ WOLF_DEBUG("CASE 2");
+
+ _capture->link(frame_pack->key_frame);
+
+ process(_capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+
+ return;
+ }
+ // CASE 3:
+ WOLF_DEBUG("CASE 3");
+ buffer_capture_.add(_capture->getTimeStamp(), _capture);
+}
+
+void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
+{
+ /* This function has 4 scenarios:
+ * 1. Frame already have a capture of the sensor -> process
+ * 2. Frame has a timestamp within time tolerances of any stored capture -> link + process
+ * 3. Frame is more recent than any stored capture -> store frame to be processed later in processCapture
+ * 4. Otherwise: The frame is not compatible with any stored capture -> discard frame
+ */
+
+ WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+
+ // CASE 1:
+ auto cap = _frame->getCaptureOf(getSensor());
+ if (cap)
+ {
+ WOLF_DEBUG("CASE 1");
+
+ process(cap);
+
+ // remove the capture (if stored)
+ buffer_capture_.getContainer().erase(cap->getTimeStamp());
+
+ return;
+ }
+
+ // Search for any stored capture within time tolerance of frame
+ auto capture = buffer_capture_.select(_frame->getTimeStamp(), params_->time_tolerance);
+
+ // CASE 2:
+ if (capture and not capture->getFrame())
+ {
+ WOLF_DEBUG("CASE 2");
+
+ capture->link(_frame);
+
+ process(capture);
+
+ // remove the capture (if stored)
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+
+ // remove old captures (10s of old captures are kept in case frames arrives unordered)
+ buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+
+ return;
+ }
+ // CASE 3:
+ if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ {
+ WOLF_DEBUG("CASE 3");
+
+ // store frame
+ buffer_pack_kf_.add(_frame, _time_tolerance);
+
+ return;
+ }
+ // CASE 4:
+ WOLF_DEBUG("CASE 4");
+ // nothing (discard frame)
+}
+
+void ProcessorLoopClosure::process(CaptureBasePtr _capture)
+{
+ assert(_capture->getFrame() != nullptr && "ProcessorLoopClosure::process _capture not linked to _frame");
+ WOLF_DEBUG("ProcessorLoopClosure::process frame ", _capture->getFrame()->id(), " capture ", _capture->id());
+
+ // Detect and emplace features
+ WOLF_DEBUG("emplacing features...");
+ emplaceFeatures(_capture);
+
+ // Vote for loop closure search
+ if (voteFindLoopClosures(_capture))
+ {
+ WOLF_DEBUG("finding loop closures...");
+
+ // Find loop closures
+ auto match_lc_map = findLoopClosures(_capture);
+
+ WOLF_DEBUG(match_lc_map.size(), " loop closures found");
+
+ // Emplace factors for each LC if validated
+ auto n_loops = 0;
+ for (const auto& match_pair : match_lc_map)
+ if (validateLoopClosure(match_pair.second))
+ {
+ emplaceFactors(match_pair.second);
+ n_loops++;
+
+ if (params_loop_closure_->max_loops > 0 and
+ n_loops >= params_loop_closure_->max_loops)
+ break;
+ }
+ }
+}
+
+}// namespace wolf
diff --git a/src/processor/processor_loopclosure_base.cpp b/src/processor/processor_loopclosure_base.cpp
deleted file mode 100644
index 7ce19414499ead4b38e8432077f47e15116ca27e..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 "core/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
index bb2812f75847545fde2813640d9f8413fa9fa413..9b7f8da44e1e58c86277a2cb689401426932c723 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -1,17 +1,31 @@
+/**
+ * \file processor_motion.cpp
+ *
+ * Created on: 15/03/2016
+ * \author: jsola
+ */
+
+
+
#include "core/processor/processor_motion.h"
+#include "core/state_block/factory_state_block.h"
+
namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
+ int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
- ProcessorParamsMotionPtr _params_motion) :
- ProcessorBase(_type, _params_motion),
+ ParamsProcessorMotionPtr _params_motion) :
+ ProcessorBase(_type, _dim, _params_motion),
+ IsMotion(_state_structure, _params_motion),
params_motion_(_params_motion),
- processing_step_(RUNNING_WITHOUT_PACK),
+ processing_step_(RUNNING_WITHOUT_KF),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
@@ -20,8 +34,9 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
origin_ptr_(),
last_ptr_(),
incoming_ptr_(),
- dt_(0.0), x_(_state_size),
- data_(_data_size),
+ last_frame_ptr_(),
+ dt_(0.0),
+ x_(_state_size),
delta_(_delta_size),
delta_cov_(_delta_cov_size, _delta_cov_size),
delta_integrated_(_delta_size),
@@ -29,8 +44,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
calib_preint_(_calib_size),
jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
jacobian_delta_(delta_cov_size_, delta_cov_size_),
- jacobian_calib_(delta_size_, calib_size_)
-{
+ jacobian_calib_(delta_cov_size_, calib_size_)
+{
//
}
@@ -39,37 +54,85 @@ ProcessorMotion::~ProcessorMotion()
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
-void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
+
+void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
+ CaptureMotionPtr cap_target)
+{
+ assert(cap_prev != nullptr);
+ assert(cap_target != nullptr);
+ assert(cap_prev == cap_target->getOriginCapture() && "merging not consecutive capture motions");
+
+ // add prev buffer (discarding the first zero motion)
+ cap_target->getBuffer().pop_front();
+ cap_target->getBuffer().insert(cap_target->getBuffer().begin(),
+ cap_prev->getBuffer().begin(),
+ cap_prev->getBuffer().end());
+
+ // change origin
+ cap_target->setOriginCapture(cap_prev->getOriginCapture());
+
+ // change calibration params for preintegration from origin
+ cap_target->setCalibrationPreint(getCalibration(cap_prev->getOriginCapture()));
+
+ // reintegrate buffer
+ reintegrateBuffer(cap_target);
+
+ // remove existing feature and factor (if they exist)
+ if (!cap_target->getFeatureList().empty())
+ {
+ // remove feature and factor (automatically)
+ cap_target->getFeatureList().back()->remove();
+
+ assert(cap_target->getFeatureList().empty());// there should be one feature only
+ }
+
+ // emplace new feature and factor (if origin has frame)
+ if (cap_prev->getOriginCapture() and cap_prev->getOriginCapture()->getFrame())
+ {
+ auto new_feature = emplaceFeature(cap_target);
+ emplaceFactor(new_feature, cap_prev->getOriginCapture());
+ }
+}
+
+void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
TimeStamp _ts_split,
const FrameBasePtr& _keyframe_target,
- const wolf::CaptureMotionPtr& _capture_target)
+ const CaptureMotionPtr& _capture_target) const
{
+ /** we are doing this:
+ *
+ * Before split:
+ *
+ * ts_split
+ * KF | F or KF
+ * * -----------------------*
+ * origin source
+ *
+ *
+ * After split:
+ *
+ * KF KF F or KF
+ * * ----------* -----------*
+ * origin target source
+ *
+ */
+
// 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);
- }
+ // start with empty motion
+ TimeStamp t_zero = _capture_target->getBuffer().back().ts_; // last tick of previous buffer is zero tick of current buffer
+ _capture_source->getBuffer().push_front(motionZero(t_zero));
// Update the existing capture
- _capture_source->setOriginFrame(_keyframe_target);
+ _capture_source->setOriginCapture(_capture_target);
-// // Get optimized calibration params from 'origin' keyframe
-// VectorXs calib_preint_optim = _capture_source->getOriginFrame()->getCaptureOf(getSensor())->getCalibration();
-//
-// // Write the calib params into the capture before re-integration
-// _capture_source->setCalibrationPreint(calib_preint_optim);
+ // Get optimized calibration params from 'origin' keyframe
+ VectorXd calib_preint_optim = getCalibration(_capture_source->getOriginCapture());
+
+ // Write the calib params into the capture before re-integration
+ _capture_source->setCalibrationPreint(calib_preint_optim);
// re-integrate target capture's buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer(_capture_target);
@@ -78,7 +141,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
reintegrateBuffer(_capture_source);
}
-void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
+void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
{
if (_incoming_ptr == nullptr)
{
@@ -86,107 +149,248 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
return;
}
- incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+ incoming_ptr_ = std::dynamic_pointer_cast<CaptureMotion>(_incoming_ptr);
+ assert(incoming_ptr_ != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureMotion").c_str());
+
+ // if origin_ was removed reset (let it die)
+ if (origin_ptr_ and origin_ptr_->isRemoving())
+ {
+ origin_ptr_ = nullptr;
+ if (last_ptr_)
+ last_ptr_->remove();
+ }
preProcess(); // Derived class operations
PackKeyFramePtr pack = computeProcessingStep();
if (pack)
- kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
-
+ buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
+
switch(processing_step_)
{
+ case FIRST_TIME_WITHOUT_KF :
+ {
+ // There is no KF: create own origin
+ setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
+ {
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case FIRST_TIME_WITH_KF_ON_INCOMING :
+ {
+ // can joint to the KF
+ setOrigin(pack->key_frame);
+ break;
+ }
+ case FIRST_TIME_WITH_KF_AFTER_INCOMING :
+ {
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case RUNNING_WITHOUT_KF :
+ case RUNNING_WITH_KF_ON_ORIGIN :
+ break;
- case RUNNING_WITHOUT_PACK :
- case RUNNING_WITH_PACK_ON_ORIGIN :
+ default :
break;
+ }
+
+
+ // integrate data
+ // Done at this place because setPrior() needs
+ integrateOneStep();
- case RUNNING_WITH_PACK_BEFORE_ORIGIN :
+
+ switch(processing_step_)
+ {
+ case RUNNING_WITH_KF_BEFORE_ORIGIN :
{
+
+ /*
+ * Legend:
+ * * : any frame or keyframe
+ * x : any capture
+ * k : arriving keyframe
+ * o : origin capture
+ * l : last capture
+ * e : existing capture -> new KF splits its buffer in two parts
+ * n : new capture -> part of the split buffer will be given to this
+ * f : motion feature and factor
+ * --- : buffer history
+ *
+ * Trajectory before the KF, and arriving KF 'k'
+ *
+ * k
+ * * ========= * === ... === * ========= *
+ * x ----------e ... ----o ----------l
+ * \____f____/
+ *
+ * Trajectory after the KF, and arriving KF 'k'
+ *
+ * k
+ * * === * === * === ... === * ========= *
+ * x ----n ----e ... ----o ----------l
+ * \_f_/ \_f_/
+ */
+
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ 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); // k
+ auto capture_existing = findCaptureContainingTimeStamp(ts_from_callback); // k
+
+ if (!capture_existing)
+ {
+ WOLF_WARN("A KF before first motion capture (TS = ", ts_from_callback, "). ProcessorMotion cannot do anything.");
+ break;
+ }
- // Find the frame acting as the capture's origin
- auto keyframe_origin = existing_capture->getOriginFrame(); // i
+ // update KF state (adding missing StateBlocks)
+ auto proc_state = getState(ts_from_callback);
+ for (auto pair_ckey_vec : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
+ keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
+ FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+ pair_ckey_vec.second,
+ false),
+ getProblem());
+ keyframe_from_callback->setState(proc_state);
- auto capture_origin = std::static_pointer_cast<CaptureMotion>(keyframe_origin->getCaptureOf(getSensor()));
+ // Find the capture acting as the buffer's origin
+ auto capture_origin = capture_existing->getOriginCapture();
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXs calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
- auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
- getSensor(),
- ts_from_callback,
- Eigen::VectorXs::Zero(data_size_),
- capture_origin->getDataCovariance(),
- calib_origin,
- calib_origin,
- keyframe_origin);
+ auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
+ getSensor(),
+ ts_from_callback,
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->getNoiseCov(),
+ calib_origin,
+ calib_origin,
+ capture_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);
+ splitBuffer(capture_existing, 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);
+ auto feature_new = emplaceFeature(capture_for_keyframe_callback);
// create motion factor and add it to the feature, and constrain to the other capture (origin)
- emplaceFactor(new_feature, keyframe_origin->getCaptureOf(getSensor()) );
+ emplaceFactor(feature_new, capture_origin );
// modify existing feature and factor (if they exist in the existing capture)
- if (!existing_capture->getFeatureList().empty())
+ if (!capture_existing->getFeatureList().empty())
{
- auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
+ // setMeasurement is not allowed: feature (and factor) will be removed and a new feature and factor will be emplaced
+ capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
- // Modify existing feature --------
- existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_);
- existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
+ assert(capture_existing->getFeatureList().empty());// there was only one feature!
+ auto new_feature_existing = emplaceFeature(capture_existing);
+ emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
- // 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.)
+// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
+//
+// // Modify existing feature --------
+// feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
+// feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
+//
+// // Modify existing factor --------
+// // Instead of modifying, we remove one ctr, and create a new one.
+// auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
+// auto new_ctr = emplaceFactor(feature_existing, 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 :
+ case RUNNING_WITH_KF_AFTER_ORIGIN :
{
+ /*
+ * Legend:
+ * * : any frame or keyframe
+ * x : any capture
+ * k : arriving keyframe
+ * o : origin capture
+ * l : last capture -> new KF splits its buffer in two parts
+ * n : new capture -> part of the split buffer will be given to this
+ * f : motion feature and factor
+ * --- : buffer history
+ *
+ * Trajectory before the KF, and arriving KF 'k'
+ *
+ * k
+ * * ========= * ========= *
+ * x ----------o ----------l
+ * \____f____/
+ *
+ * Trajectory after the KF, and arriving KF 'k'
+ *
+ * k
+ * * ========= * === * === *
+ * x ----------o ----n ----l
+ * \____f____/ \_f_/
+ *
+ * Trajectory after the KF, and after reset
+ *
+ * k
+ * * ========= * === * === *
+ * x ----------x ----o ----l
+ * \____f____/ \_f_/
+ */
+
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
- // Find the frame acting as the capture's origin
- auto keyframe_origin = last_ptr_->getOriginFrame();
+ // update KF state (adding missing StateBlocks)
+ auto proc_state = getState(ts_from_callback);
+ for (auto pair_ckey_vec : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
+ keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
+ FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+ pair_ckey_vec.second,
+ false),
+ getProblem());
+ keyframe_from_callback->setState(proc_state);
+
+ auto & capture_existing = last_ptr_;
+
+ // Find the capture acting as the buffer's origin
+ auto & capture_origin = origin_ptr_;
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXs calib_origin = origin_ptr_->getCalibration();
+ VectorXd calib_origin = getCalibration(capture_origin);
// 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_),
- origin_ptr_->getDataCovariance(),
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->getNoiseCov(),
calib_origin,
calib_origin,
- keyframe_origin);
+ capture_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);
+ splitBuffer(capture_existing, 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()) );
+ emplaceFactor(feature_for_keyframe_callback, capture_origin );
// reset processor origin
origin_ptr_ = capture_for_keyframe_callback;
@@ -198,112 +402,304 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
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());
+ const auto& ts = getTimeStamp();
+ last_ptr_->setTimeStamp( ts );
+ last_ptr_->getFrame()->setTimeStamp( ts );
+ VectorComposite state_propa = getState( ts );
+ for (auto pair_ckey_vec : state_propa)
+ if (!last_ptr_->getFrame()->isInStructure(pair_ckey_vec.first))
+ last_ptr_->getFrame()->addStateBlock(pair_ckey_vec.first,
+ FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+ pair_ckey_vec.second,
+ false),
+ getProblem());
+ last_ptr_->getFrame()->setState( state_propa );
if (permittedKeyFrame() && voteForKeyFrame())
{
+ /*
+ * Legend:
+ * * : any keyframe
+ * + : last frame
+ * x : any capture
+ * o : origin capture
+ * l : last capture -> we'll make a KF here
+ * i : incoming capture
+ * n : new capture ->
+ * --- : buffer history
+ *
+ * Trajectory before the KF
+ *
+ * * ========= * ========= +
+ * x ----------o ----------l i
+ * \____f____/
+ *
+ * Trajectory after creating KF at last
+ *
+ * * ========= * ========= * = +
+ * x ----------o ----------l --n
+ * \____f____/ \____f____/
+ *
+ * Trajectory after creating KF at last and reset
+ *
+ * * ========= * ========= * = +
+ * x ----------x ----------o --l
+ * \____f____/ \____f____/
+ *
+ */
+
+ // Set the capture's calibration with best results available (in case origin_ptr_ received a solve())
+ setCalibration(last_ptr_, getCalibration(origin_ptr_));
+
// Set the frame of last_ptr as key
- auto key_frame_ptr = last_ptr_->getFrame();
- key_frame_ptr->setKey();
+ auto key_frame = last_ptr_->getFrame();
+ key_frame ->link(getProblem());
// create motion feature and add it to the key_capture
- auto key_feature_ptr = emplaceFeature(last_ptr_);
+ auto key_feature = 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_);
+ auto factor = emplaceFactor(key_feature, origin_ptr_);
// create a new frame
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- getCurrentState(),
- getCurrentTimeStamp());
+ auto frame_new = std::make_shared<FrameBase>(getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState());
// 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);
+ auto capture_new = emplaceCapture(frame_new,
+ getSensor(),
+ key_frame->getTimeStamp(),
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->getNoiseCov(),
+ getCalibration(origin_ptr_),
+ getCalibration(origin_ptr_),
+ last_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();
+ capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
// reset derived things
resetDerived();
- // Update pointers
+ // Reset pointers
origin_ptr_ = last_ptr_;
- last_ptr_ = new_capture_ptr;
+ last_ptr_ = capture_new;
+ last_frame_ptr_ = frame_new;
// callback to other processors
- getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), params_motion_->time_tolerance);
- }
+ getProblem()->keyFrameCallback(key_frame, 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)
+VectorComposite ProcessorMotion::getState(const StateStructure& _structure) const
{
- 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);
+ const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
- if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
+
+ if (origin_ptr_ == nullptr or
+ origin_ptr_->isRemoving() or
+ last_ptr_ == nullptr or
+ last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
{
- // 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
- Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
- statePlusDelta(state_0, delta, dt, _x);
+ WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
+ return VectorComposite(); // return empty state
+ }
+
+
+ // From here on, we do have info to compute a valid state
+
+ // if buffer is empty --> we did not advance from origin!
+ // this may happen when in the very first frame where the capture has no motion info --> empty buffer
+ if (last_ptr_->getBuffer().empty())
+ {
+ return last_ptr_->getFrame()->getState(structure);
+ }
+
+ /* Doing this:
+ *
+ * x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
+ *
+ * or, put in code form:
+ *
+ * _state = state_origin [+] (delta_preint (+) Jac_delta_calib * (calib - calib_preint) )
+ *
+ * with
+ * [+] : group composition
+ * (+) : block-wise plus
+ */
+
+ // Get state of origin
+ const auto& x_origin = getOrigin()->getFrame()->getState(state_structure_);
+
+ // Get most recent motion
+ const auto& motion = last_ptr_->getBuffer().back();
+
+ // Get delta preintegrated up to now
+ const auto& delta_preint = motion.delta_integr_;
+
+ // Get calibration preint -- stored in last capture
+ const auto& calib_preint = last_ptr_->getCalibrationPreint();
+
+ VectorComposite state;
+ //WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp());
+ //WOLF_INFO("processorMotion origin timestamp: ", origin_ptr_->getTimeStamp());
+ if ( hasCalibration())
+ {
+ // Get current calibration -- from origin capture
+ const auto& calib = getCalibration(origin_ptr_);
+
+ // get Jacobian of delta wrt calibration
+ const auto& J_delta_calib = motion.jacobian_calib_;
+
+ // compute delta change
+ const auto& delta_step = J_delta_calib * (calib - calib_preint);
+
+ // correct delta // this is (+)
+ const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+
+ // compute current state // this is [+]
+ statePlusDelta(x_origin, delta_corrected, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
}
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;
+ statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
+ }
+
+ if (_structure == "")
+ return state;
+
+ else
+ {
+ // remove states not requested by structure
+ auto pair_key_vec_it = state.begin();
+ while (pair_key_vec_it != state.end())
+ {
+ if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+ pair_key_vec_it = state.erase(pair_key_vec_it);
+
+ else
+ pair_key_vec_it ++;
+
+ }
+ return state;
}
- return true;
}
-FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
+
+
+// _x needs to have the size of the processor state
+VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStructure& _structure) const
{
- FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
+ assert(_ts.ok());
+
+ const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
+
+ // We need to search for the capture containing a motion buffer with the queried time stamp
+ CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
+
+ if (capture_motion == nullptr) // we do not have any info of where to find a valid state
+ {
+ WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
+ return VectorComposite(); // return empty state
+ }
+
+
+ else // We found a CaptureMotion whose buffer contains the time stamp
+ {
+ // if buffer is empty --> we did not advance from origin!
+ // this may happen when in the very first frame where the capture has no motion info --> empty buffer
+ if (capture_motion->getBuffer().empty())
+ {
+ return capture_motion->getFrame()->getState(structure);
+ }
+
+ /* Doing this:
+ *
+ * x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
+ *
+ * or, put in code form:
+ *
+ * _state = state_origin [+] (delta_preint (+) Jac_delta_calib * (calib - calib_preint) )
+ *
+ * with
+ * [+] : group composition
+ * (+) : block-wise plus
+ */
+
+ // Get state of origin
+ CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
+ const auto& x_origin = cap_orig->getFrame()->getState(state_structure_);
+
+ // Get motion at time stamp
+ const auto& motion = capture_motion->getBuffer().getMotion(_ts);
+
+ // Get delta preintegrated up to time stamp
+ const auto& delta_preint = motion.delta_integr_;
+
+ // Get calibration preint -- stored in last capture
+ const auto& calib_preint = capture_motion->getCalibrationPreint();
+
+ VectorComposite state;
+
+ if ( hasCalibration() )
+ {
+ // Get current calibration -- from origin capture
+ const auto& calib = getCalibration(cap_orig);
+
+ // get Jacobian of delta wrt calibration
+ const auto& J_delta_calib = motion.jacobian_calib_;
+
+ // compute delta change
+ const auto& delta_step = J_delta_calib * (calib - calib_preint);
+
+ // correct delta // this is (+)
+ const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+
+ // compute current state // this is [+]
+ statePlusDelta(x_origin, delta_corrected, _ts - cap_orig->getTimeStamp(), state);
+ }
+ else
+ {
+ statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), state);
+ }
+
+ if (_structure == "")
+ return state;
+
+ else
+ {
+ // remove states not requested by structure
+ auto pair_key_vec_it = state.begin();
+ while (pair_key_vec_it != state.end())
+ {
+ if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+ pair_key_vec_it = state.erase(pair_key_vec_it);
+
+ else
+ pair_key_vec_it ++;
+
+ }
+ return state;
+ }
+
+ }
+}
+
+FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
+{
+ FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ _ts_origin,
+ getStateStructure(),
+ _x_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -314,43 +710,38 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
assert(_origin_frame->getTrajectory() != nullptr
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
- assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
+
+ TimeStamp origin_ts = _origin_frame->getTimeStamp();
// -------- 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(),
+ origin_ts,
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->getNoiseCov(),
+ getCalibration(),
+ getCalibration(),
nullptr);
// ---------- LAST ----------
// Make non-key-frame for last Capture
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
- _origin_frame->getTimeStamp());
+ last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts,
+ getStateStructure(),
+ _origin_frame->getState());
+
// emplace (emtpy) last Capture
- last_ptr_ = emplaceCapture(new_frame_ptr,
+ last_ptr_ = emplaceCapture(last_frame_ptr_,
getSensor(),
- _origin_frame->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
- getSensor()->getCalibration(),
- getSensor()->getCalibration(),
- _origin_frame);
+ origin_ts,
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->getNoiseCov(),
+ getCalibration(),
+ getCalibration(),
+ origin_ptr_);
// 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();
+ getBuffer().push_back(motionZero(_origin_frame->getTimeStamp()));
// Reset derived things
resetDerived();
@@ -363,7 +754,7 @@ void ProcessorMotion::integrateOneStep()
assert(dt_ >= 0 && "Time interval _dt is negative!");
// get vector of parameters to calibrate
- calib_preint_ = getBuffer().getCalibrationPreint();
+ calib_preint_ = getLast()->getCalibrationPreint();
// get data and convert it to delta, and obtain also the delta covariance
computeCurrentDelta(incoming_ptr_->getData(),
@@ -375,7 +766,7 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_calib_);
// integrate the current delta to pre-integrated measurements, and get Jacobians
- deltaPlusDelta(getBuffer().get().back().delta_integr_,
+ deltaPlusDelta(getBuffer().back().delta_integr_,
delta_,
dt_,
delta_integrated_,
@@ -383,44 +774,56 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_);
// integrate Jacobian wrt calib
- if ( hasCalibration() )
+ if ( hasCalibration() ) // if no calibration, matrices can have mismatching sizes, and this computation is nevertheless pointless
+ {
jacobian_calib_.noalias()
- = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
+ = jacobian_delta_preint_ * getBuffer().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_preint_ * getBuffer().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_);
+ getBuffer().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_);
+
+ // integrate odometry
+ statePlusDelta(odometry_, delta_, dt_, odometry_);
}
-void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
+void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
{
- // start with empty motion
- _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFrame()->getTimeStamp()));
+ VectorXd calib = _capture_ptr->getCalibrationPreint();
+
+ // some temporaries for integration
+ delta_integrated_ =deltaZero();
+ delta_integrated_cov_.setZero();
+ jacobian_calib_ .setZero();
- VectorXs calib = _capture_ptr->getCalibrationPreint();
+ // check that first motion in buffer is zero!
+ assert(_capture_ptr->getBuffer().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
// Iterate all the buffer
- auto motion_it = _capture_ptr->getBuffer().get().begin();
+ auto motion_it = _capture_ptr->getBuffer().begin();
auto prev_motion_it = motion_it;
motion_it++;
- while (motion_it != _capture_ptr->getBuffer().get().end())
+ while (motion_it != _capture_ptr->getBuffer().end())
{
// get dt
- const Scalar dt = motion_it->ts_ - prev_motion_it->ts_;
+ const double dt = motion_it->ts_ - prev_motion_it->ts_;
// re-convert data to delta with the new calibration parameters
computeCurrentDelta(motion_it->data_,
@@ -432,7 +835,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
jacobian_delta_calib_);
// integrate delta into delta_integr, and rewrite the buffer
- deltaPlusDelta(prev_motion_it->delta_integr_,
+ deltaPlusDelta(delta_integrated_,
motion_it->delta_,
dt,
motion_it->delta_integr_,
@@ -441,11 +844,17 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
// 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_;
+ motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * 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();
+ motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose()
+ + motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
+
+ // update temporaries
+ delta_integrated_ = motion_it->delta_integr_;
+ delta_integrated_cov_ = motion_it->delta_integr_cov_;
+ jacobian_calib_ = motion_it->jacobian_calib_;
// advance in buffer
motion_it++;
@@ -453,232 +862,159 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
}
}
-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)
+CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
- // 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.");
+ assert(_ts.ok());
+ // assert(last_ptr_ != nullptr);
- // Fraction of the time interval
- Scalar tau = (_ts - _ref1.ts_) / (_ref2.ts_ - _ref1.ts_);
+ //Need to uncomment this line so that wolf_ros_apriltag doesn't crash
+ if(last_ptr_ == nullptr) return nullptr;
+ // First check if last_ptr is the one we are looking for
+ if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
+ return last_ptr_;
+ // Then look in the Wolf tree...
+ // -----------------------------
+ //
- Motion interpolated(_ref1);
- interpolated.ts_ = _ts;
- interpolated.data_ = (1-tau)*_ref1.data_ + tau*_ref2.data_;
- interpolated.data_cov_ = (1-tau)*_ref1.data_cov_ + tau*_ref2.data_cov_; // bof
- computeCurrentDelta(interpolated.data_,
- interpolated.data_cov_,
- calib_preint_,
- _ts.get() - _ref1.ts_.get(),
- interpolated.delta_,
- interpolated.delta_cov_,
- interpolated.jacobian_calib_);
- deltaPlusDelta(_ref1.delta_integr_,
- interpolated.delta_,
- _ts.get() - _ref1.ts_.get(),
- interpolated.delta_integr_,
- interpolated.jacobian_delta_integr_,
- interpolated.jacobian_delta_);
-
- _second.ts_ = _ref2.ts_;
- _second.data_ = tau*_ref1.data_ + (1-tau)*_ref2.data_;
- _second.data_cov_ = tau*_ref1.data_cov_ + (1-tau)*_ref2.data_cov_; // bof
- computeCurrentDelta(_second.data_,
- _second.data_cov_,
- calib_preint_,
- _ref2.ts_.get() - _ts.get(),
- _second.delta_,
- _second.delta_cov_,
- _second.jacobian_calib_);
- deltaPlusDelta(_second.delta_integr_,
- _second.delta_,
- _second.ts_.get() - _ref1.ts_.get(),
- _second.delta_integr_,
- _second.jacobian_delta_integr_,
- _second.jacobian_delta_);
-
- return interpolated;
-
-
-
-
- // if (tau < 0.5)
- // {
- // // _ts is closest to _ref1
- // Motion interpolated ( _ref1 );
- // // interpolated.ts_ = _ref1.ts_;
- // // interpolated.data_ = _ref1.data_;
- // // interpolated.data_cov_ = _ref1.data_cov_;
- // interpolated.delta_ = deltaZero();
- // interpolated.delta_cov_ . setZero();
- // // interpolated.delta_integr_ = _ref1.delta_integr_;
- // // interpolated.delta_integr_cov_ = _ref1.delta_integr_cov_;
- // interpolated.jacobian_delta_integr_ . setIdentity();
- // interpolated.jacobian_delta_ . setZero();
-
- // _second = _ref2;
-
- // return interpolated;
- // }
- // else
- // {
- // // _ts is closest to _ref2
- // Motion interpolated ( _ref2 );
-
- // _second = _ref2;
- // // _second.data_ = _ref2.data_;
- // // _second.data_cov_ = _ref2.data_cov_;
- // _second.delta_ = deltaZero();
- // _second.delta_cov_ . setZero();
- // // _second.delta_integr_ = _ref2.delta_integr_;
- // // _second.delta_integr_cov_ = _ref2.delta_integr_cov_;
- // _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
+ // 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend();
+ for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
+ frame_rev_iter != getProblem()->getTrajectory()->rend();
++frame_rev_iter)
{
frame = *frame_rev_iter;
- capture = frame->getCaptureOf(getSensor());
+ auto sensor = getSensor();
+ capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
{
+ assert(std::dynamic_pointer_cast<CaptureMotion>(capture) != nullptr);
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
- if (_ts >= capture_motion->getBuffer().get().front().ts_)
+
+ if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
+ {
// Found time stamp satisfying rule 3 above !! ==> break for loop
break;
+ }
+ else
+ capture_motion = nullptr;
}
}
return capture_motion;
}
-CaptureMotionPtr ProcessorMotion::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);
- capture->link(_frame_own);
- return capture;
-}
-
-FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureBasePtr feature = createFeature(_capture_motion);
- // _capture_motion->addFeature(feature);
- feature->link(_capture_motion);
- return feature;
-}
-
PackKeyFramePtr ProcessorMotion::computeProcessingStep()
{
- if (!getProblem()->priorIsSet())
+ // Origin not set
+ if (!origin_ptr_)
{
- 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 = buffer_pack_kf_.selectFirstPackBefore(incoming_ptr_, params_motion_->time_tolerance);
- PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+ if (pack)
+ {
+ if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ {
+ WOLF_DEBUG("First time with a KF compatible.")
+ processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
+ }
+ else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
+ {
+ WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
+ processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
+ }
+ else
+ {
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
+ }
+ }
+ else
+ {
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITHOUT_KF;
+ }
- if (pack)
+ return pack;
+ }
+ else
{
- if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+
+ // ignore "future" KF to avoid MotionBuffer::split() error
+ if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
+ pack = nullptr;
+
+ if (pack)
{
- 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;
+ if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+
+ processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
+
+ else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+ processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
+
+ else
+
+ processing_step_ = RUNNING_WITH_KF_AFTER_ORIGIN;
+
+ }
else
- processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
+ processing_step_ = RUNNING_WITHOUT_KF;
+ return pack;
}
- else
- processing_step_ = RUNNING_WITHOUT_PACK;
- return pack;
+ // not reached
+ return nullptr;
}
+bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
+{
+ return true;
}
+bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr)
+{
+ return false;
+}
+
+TimeStamp ProcessorMotion::getTimeStamp ( ) const
+{
+ if (not origin_ptr_ or
+ origin_ptr_->isRemoving() or
+ not last_ptr_)
+ {
+ WOLF_DEBUG("Processor has no time stamp. Returning a non-valid timestamp equal to 0");
+ return TimeStamp::Invalid();
+ }
+
+ if (getBuffer().empty())
+ return last_ptr_->getTimeStamp();
+
+ return getBuffer().back().ts_;
+}
+
+void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+ if (getOrigin())
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
+
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
deleted file mode 100644
index 9b19d4e8c4eda8db40eac71574534f3419880634..0000000000000000000000000000000000000000
--- a/src/processor/processor_odom_2D.cpp
+++ /dev/null
@@ -1,224 +0,0 @@
-#include "core/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());
- auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _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;
-}
-ProcessorBasePtr ProcessorOdom2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr)
-{
-
- ProcessorOdom2DPtr prc_ptr;
-
- std::shared_ptr<ProcessorParamsOdom2D> params;
- params = std::make_shared<ProcessorParamsOdom2D>();
- params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "true");
- params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.1");
- params->max_time_span = _server.getParam<double>(_unique_name + "/max_time_span", "999");
- params->dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled", "0.95"); // Will make KFs automatically every 1m displacement
- params->angle_turned = _server.getParam<double>(_unique_name + "/angle_turned", "999");
- params->cov_det = _server.getParam<double>(_unique_name + "/cov_det", "999");
- params->unmeasured_perturbation_std = _server.getParam<double>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
-
- prc_ptr = std::make_shared<ProcessorOdom2D>(params);
- prc_ptr->setName(_unique_name);
-
- return prc_ptr;
-}
-
-}
-
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("ODOM 2D", ProcessorOdom2D)
-} // namespace wolf
-#include "core/processor/autoconf_processor_factory.h"
-namespace wolf {
- WOLF_REGISTER_PROCESSOR_AUTO("ODOM 2D", ProcessorOdom2D)
-} // namespace wolf
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..573406efe82a815daecddc2abfdde1bd39ff2fb6
--- /dev/null
+++ b/src/processor/processor_odom_2d.cpp
@@ -0,0 +1,177 @@
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/math/covariance.h"
+#include "core/state_block/state_composite.h"
+#include "core/factor/factor_relative_pose_2d.h"
+
+namespace wolf
+{
+
+ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
+ ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
+ params_odom_2d_(_params)
+{
+ unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
+}
+
+ProcessorOdom2d::~ProcessorOdom2d()
+{
+}
+
+void ProcessorOdom2d::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib, const double _dt, Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _jacobian_calib) const
+{
+ 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::MatrixXd J_delta_data(delta_cov_size_, data_size_);
+ J_delta_data(0, 0) = cos(_data(1) / 2);
+ J_delta_data(1, 0) = sin(_data(1) / 2);
+ J_delta_data(2, 0) = 0;
+ J_delta_data(0, 1) = -_data(0) * sin(_data(1) / 2) / 2;
+ J_delta_data(1, 1) = _data(0) * cos(_data(1) / 2) / 2;
+ J_delta_data(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_delta_data * _data_cov * J_delta_data.transpose() + unmeasured_perturbation_cov_;
+}
+
+void ProcessorOdom2d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
+{
+ 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::Rotation2Dd(_delta1(2)).matrix() * _delta2.head<2>();
+ _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+}
+
+void ProcessorOdom2d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2, Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
+{
+ 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::Rotation2Dd(_delta1(2)).matrix() * _delta2.head<2>();
+ _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
+ // Jac wrt delta_integrated
+ _jacobian1 = Eigen::MatrixXd::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::MatrixXd::Identity(delta_cov_size_, delta_cov_size_);
+ _jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Dd(_delta1(2)).matrix();
+}
+
+void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const
+{
+ assert(_delta.size() == delta_size_ && "Wrong _x_plus_delta vector size");
+
+ // This is just a frame composition in 2d
+ _x_plus_delta['P'] = _x.at('P') + Eigen::Rotation2Dd(_x.at('O')(0)).matrix() * _delta.head<2>();
+ _x_plus_delta['O'] = Vector1d(pi2pi(_x.at('O')(0) + _delta(2)));
+}
+
+bool ProcessorOdom2d::voteForKeyFrame() const
+{
+ // Distance criterion
+ if (getBuffer().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
+ {
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
+ return true;
+ }
+ if (getBuffer().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
+ {
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
+ return true;
+ }
+ // Uncertainty criterion
+ if (getBuffer().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
+ {
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance");
+ return true;
+ }
+ // Time criterion
+ if (getBuffer().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
+ {
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per time");
+ return true;
+ }
+ return false;
+}
+
+CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr)
+{
+ auto cap_motion = CaptureBase::emplace<CaptureOdom2d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
+ setCalibration(cap_motion, _calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
+}
+
+FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
+{
+ auto fac_odom = FactorBase::emplace<FactorRelativePose2d>(_feature,
+ _feature,
+ _capture_origin->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
+ return fac_odom;
+}
+
+FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
+{
+ Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
+ makePosDef(covariance);
+
+ FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
+ "ProcessorOdom2d",
+ _capture_motion->getBuffer().back().delta_integr_,
+ covariance);
+ return key_feature_ptr;
+}
+
+
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorOdom2d);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom2d);
+} // namespace wolf
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
deleted file mode 100644
index 9c29e0f89fb6dba5d4495fd3a4d2b1241812cb68..0000000000000000000000000000000000000000
--- a/src/processor/processor_odom_3D.cpp
+++ /dev/null
@@ -1,441 +0,0 @@
-#include "core/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 of this motion step
- if (_data.size() == (long int)6)
- {
- // rotation in vector form
- _delta.head<3>() = _data.head<3>();
- new (&q_out_) Eigen::Map<Eigen::Quaternions>(_delta.data() + 3);
- q_out_ = v2q(_data.tail<3>());
- disp = _data.head<3>().norm();
- rot = _data.tail<3>().norm();
- }
- else
- {
- // rotation in quaternion form
- _delta = _data;
- disp = _data.head<3>().norm();
- rot = 2 * acos(_data(3));
- }
- /* Jacobians of d = data2delta(data, dt)
- * with: d = [Dp Dq]
- * data = [dp do]
- *
- * Dp = dp
- * Dq = v2q(do)
- *
- * dDp/ddp = I
- * dDp/ddo = 0
- * dDo/ddp = 0
- * dDo/ddo = I
- *
- * so, J = I, and delta_cov = _data_cov
- */
- // 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());
- auto fac_odom = FactorBase::emplace<FactorOdom3D>(_feature_motion, _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 "core/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("ODOM 3D", ProcessorOdom3D)
-} // namespace wolf
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e57ba32bd0d6c312d10e956e6806830b55164100
--- /dev/null
+++ b/src/processor/processor_odom_3d.cpp
@@ -0,0 +1,252 @@
+#include "core/processor/processor_odom_3d.h"
+#include "core/math/SE3.h"
+
+namespace wolf
+{
+
+ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
+ ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
+ params_odom_3d_ (_params)
+{
+ // Set constant parts of Jacobians
+ jacobian_delta_preint_.setIdentity(6,6);
+ jacobian_delta_.setIdentity(6,6);
+ jacobian_calib_.setZero(6,0);
+ unmeasured_perturbation_cov_ = pow(params_odom_3d_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix6d::Identity();
+}
+
+ProcessorOdom3d::~ProcessorOdom3d()
+{
+}
+
+void ProcessorOdom3d::configure(SensorBasePtr _sensor)
+{
+ assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
+
+ SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor);
+}
+
+void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
+{
+ assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3d.");
+ if (_data.size() == (long int)6)
+ {
+ // rotation in vector form
+ _delta.head<3>() = _data.head<3>();
+ Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3);
+ q = v2q(_data.tail<3>());
+ }
+ else
+ {
+ // rotation in quaternion form
+ _delta = _data;
+ }
+ /* Jacobians of d = data2delta(data, dt)
+ * with: d = [Dp Dq]
+ * data = [dp do]
+ *
+ * Dp = dp
+ * Dq = v2q(do)
+ *
+ * dDp/ddp = I
+ * dDp/ddo = 0
+ * dDo/ddp = 0
+ * dDo/ddo = I
+ *
+ * so, J = I, and delta_cov = _data_cov
+ */
+ _delta_cov = _data_cov;
+}
+
+void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2, Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
+{
+ 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");
+
+ Eigen::Map<const Eigen::Vector3d> dp1 (_delta1.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq1 (_delta1.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> dp2 (_delta2.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq2 (_delta2.data() + 3 );
+ Eigen::Map<Eigen::Vector3d> dp_out (_delta1_plus_delta2.data() );
+ Eigen::Map<Eigen::Quaterniond> dq_out (_delta1_plus_delta2.data() + 3 );
+
+ /* 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::Matrix3d dR1 = dq1.matrix();
+ Eigen::Matrix3d dR2 = dq2.matrix();
+
+ // fill Jacobians
+ _jacobian1.setIdentity();
+ _jacobian1.block<3, 3>(0, 3) = -dR1 * skew(dp2); // (Sola 16, ex. B.3.2 and Sec. 7.2.3)
+ _jacobian1.block<3, 3>(3, 3) = dR2.transpose(); // (Sola 16, Sec. 7.2.3)
+
+ _jacobian2.setIdentity();
+ _jacobian2.block<3, 3>(0, 0) = dR1; // (Sola 16, Sec. 7.2.3)
+
+ // perform composition here to avoid aliasing problems if _delta1 and _delta_plus_delta share the same storage
+ dp_out = dp1 + dq1 * dp2;
+ dq_out = dq1 * dq2;
+}
+
+void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
+{
+ 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");
+
+ Eigen::Map<const Eigen::Vector3d> dp1 (_delta1.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq1 (_delta1.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> dp2 (_delta2.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq2 (_delta2.data() + 3 );
+ Eigen::Map<Eigen::Vector3d> dp_out (_delta1_plus_delta2.data() );
+ Eigen::Map<Eigen::Quaterniond> dq_out (_delta1_plus_delta2.data() + 3 );
+
+
+ dp_out = dp1 + dq1 * dp2;
+ dq_out = dq1 * dq2;
+}
+
+void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const
+{
+ assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
+
+ Eigen::Map<const Eigen::Vector3d> p (_x.at('P').data() );
+ Eigen::Map<const Eigen::Quaterniond> q (_x.at('O').data() );
+ Eigen::Map<const Eigen::Vector3d> dp (_delta.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 );
+
+ _x_plus_delta['P'] = p + q * dp;
+ _x_plus_delta['O'] = (q * dq).coeffs();
+}
+
+
+
+bool ProcessorOdom3d::voteForKeyFrame() const
+{
+ //WOLF_DEBUG( "Time span : " , getBuffer().back().ts_ - getBuffer().front().ts_ );
+ //WOLF_DEBUG( " last ts : ", getBuffer().back().ts_);
+ //WOLF_DEBUG( " first ts : ", getBuffer().front().ts_);
+ //WOLF_DEBUG( "BufferLength: " , getBuffer().size() );
+ //WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
+ //WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
+ // time span
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_odom_3d_->max_time_span)
+ {
+ WOLF_DEBUG( "PM: vote: time span" );
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_odom_3d_->max_buff_length)
+ {
+ WOLF_DEBUG( "PM: vote: buffer size" );
+ return true;
+ }
+ // distance traveled
+ double dist = getMotion().delta_integr_.head(3).norm();
+ if (dist > params_odom_3d_->dist_traveled)
+ {
+ WOLF_DEBUG( "PM: vote: distance traveled" );
+ return true;
+ }
+ // angle turned
+ double angle = q2v(Quaterniond(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::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin)
+{
+ auto cap_motion = CaptureBase::emplace<CaptureOdom3d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
+ setCalibration(cap_motion, _calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
+}
+
+FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
+{
+ FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
+ "ProcessorOdom3d",
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_);
+ return key_feature_ptr;
+}
+
+Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ VectorXd delta_corrected(7);
+ SE3::plus(delta_preint, delta_step, delta_corrected);
+ return delta_corrected;
+}
+
+FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+{
+ auto fac_odom = FactorBase::emplace<FactorRelativePose3d>(_feature_motion,
+ _feature_motion,
+ _capture_origin->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
+ return fac_odom;
+}
+
+VectorXd ProcessorOdom3d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+
+void ProcessorOdom3d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorOdom3d);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom3d);
+} // namespace wolf
+
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..70d9c6c976190d8a105e5984be914c1d4933a2e9
--- /dev/null
+++ b/src/processor/processor_pose.cpp
@@ -0,0 +1,122 @@
+/**
+ * \file processor_pose.cpp
+ *
+ * Created on: Feb 19, 2020
+ * \author: mfourmy
+ */
+
+#include "core/processor/processor_pose.h"
+
+
+namespace wolf{
+
+
+inline ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) :
+ ProcessorBase("ProcessorPose", 3, _params_pfnomove),
+ params_pfnomove_(std::make_shared<ParamsProcessorPose>(*_params_pfnomove))
+{
+
+}
+
+void ProcessorPose::configure(SensorBasePtr _sensor)
+{
+}
+
+void ProcessorPose::createFactorIfNecessary(){
+ auto sensor_pose = std::static_pointer_cast<SensorPose>(getSensor());
+ auto kf_it_last = buffer_pack_kf_.getContainer().end();
+ auto kf_it = buffer_pack_kf_.getContainer().begin();
+ while (kf_it != buffer_pack_kf_.getContainer().end())
+ {
+ TimeStamp t = kf_it->first;
+ double time_tolerance = std::min(getTimeTolerance(), kf_it->second->time_tolerance);
+ if (getTimeTolerance() == 0.0){
+ WOLF_WARN("Time tolerance set to zero -> value not used");
+ time_tolerance = kf_it->second->time_tolerance;
+ }
+ auto cap_it = buffer_capture_.selectIterator(t, time_tolerance);
+
+ // if capture with corresponding timestamp is not found, assume you will get it later
+ if (cap_it != buffer_capture_.getContainer().end())
+ {
+ // if a corresponding capture exists, link it to the KF and create a factor
+ auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
+ cap->link(kf_it->second->key_frame);
+ FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
+ FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_MOTION);
+
+ // erase removes range [first, last): it does not removes last
+ // so increment the iterator so that it points to the next element in the container
+ buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
+
+ // we cannot erase on the kf buffer since we are looping over it so we store the iterator for later
+ kf_it_last = kf_it;
+ }
+
+ kf_it++;
+ }
+
+ // whatever happened, remove very old captures
+ buffer_capture_.removeUpTo(buffer_pack_kf_.getContainer().begin()->first - 5);
+
+ // now we erase the kf buffer if there was a match
+ if (kf_it_last != buffer_pack_kf_.getContainer().end()){
+ buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), std::next(kf_it_last));
+ }
+
+}
+
+
+inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
+{
+ if (!_capture)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+
+}
+
+inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+ if (!_keyframe_ptr)
+ {
+ WOLF_ERROR("Received KF is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("ProcessorPose: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+}
+
+
+
+
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorPose);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorPose);
+} // namespace wolf
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 7c4f2e6e5fe1ab437ece2beedee0ac50d2b0e8cd..52421e86398bc049321e8b3dc5f75dc56d2d3001 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -15,14 +15,17 @@ namespace wolf
{
ProcessorTracker::ProcessorTracker(const std::string& _type,
- ProcessorParamsTrackerPtr _params_tracker) :
- ProcessorBase(_type, _params_tracker),
+ const StateStructure& _structure,
+ int _dim,
+ ParamsProcessorTrackerPtr _params_tracker) :
+ ProcessorBase(_type, _dim, _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)
+ last_frame_ptr_(nullptr),
+ state_structure_(_structure)
{
//
}
@@ -32,7 +35,7 @@ ProcessorTracker::~ProcessorTracker()
//
}
-void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
+void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
using std::abs;
@@ -44,6 +47,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
incoming_ptr_ = _incoming_ptr;
+
preProcess(); // Derived class operations
computeProcessingStep();
@@ -52,13 +56,12 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
case FIRST_TIME_WITH_PACK :
{
- PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+ PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
+ buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
// Append incoming to KF
- // pack->key_frame->addCapture(incoming_ptr_);
incoming_ptr_->link(pack->key_frame);
// Process info
@@ -76,7 +79,12 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
case FIRST_TIME_WITHOUT_PACK :
{
- FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp());
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
+
+ FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(kfrm);
// Process info
@@ -86,8 +94,9 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// Issue KF callback with new KF
getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
- // Update pointers
resetDerived();
+
+ // Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -97,14 +106,18 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
case 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")
+ PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
+ }
+ // Fall through
case SECOND_TIME_WITHOUT_PACK :
{
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
- incoming_ptr_->link(frm);
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
+ incoming_ptr_->link(frm);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
@@ -118,28 +131,29 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
incoming_ptr_ = nullptr;
break;
}
case RUNNING_WITH_PACK :
{
- PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+ PackKeyFramePtr pack = buffer_pack_kf_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
+ buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
processKnown();
// Capture last_ is added to the new keyframe
FrameBasePtr last_old_frame = last_ptr_->getFrame();
- last_old_frame->unlinkCapture(last_ptr_);
+ last_ptr_->move(pack->key_frame);
last_old_frame->remove();
- // pack->key_frame->addCapture(last_ptr_);
- last_ptr_->link(pack->key_frame);
// Create new frame
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(frm);
// Detect new Features, initialize Landmarks, create Factors, ...
@@ -152,38 +166,32 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
incoming_ptr_ = nullptr;
break;
}
case RUNNING_WITHOUT_PACK :
{
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_PACK");
+
processKnown();
- // eventually add more features
- if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe)
+ if (voteForKeyFrame() && permittedKeyFrame())
{
- //WOLF_TRACE("Adding more features...");
+ // process
processNew(params_tracker_->max_new_features);
- }
- if (voteForKeyFrame() && permittedKeyFrame())
- {
- // We create a KF
+ //TODO abort KF if known_features_last_.size() < params_tracker_->min_features_for_keyframe
+ // We create a KF
// set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->setKey();
-
- // make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
- incoming_ptr_->link(frm);
+ last_ptr_->getFrame()->link(getProblem());
- // process
- processNew(params_tracker_->max_new_features);
-
- // Set state to the keyframe
- last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+ // // make F; append incoming to new F
+ // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
+ // incoming_ptr_->link(frm);
// Establish factors
establishFactors();
@@ -193,57 +201,36 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// Update pointers
resetDerived();
+
+ // make F; append incoming to new F
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_->link(frm);
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
incoming_ptr_ = nullptr;
}
- /* TODO: create an auxiliary frame
- else if (voteForAuxFrame() && permittedAuxFrame())
- {
- // We create an Auxiliary Frame
-
- // set AuxF on last
- last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->setAuxiliary();
-
- // make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
- frm->addCapture(incoming_ptr_);
-
- // Set state to the keyframe
- last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-
- // Establish factors
- establishFactors();
-
- // Call the new auxframe callback in order to let the ProcessorMotion to establish their factors
- getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
-
- // Advance this
- last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
- // do not remove! last_ptr_->remove();
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
-
- // Update pointers
- advanceDerived();
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
- }*/
else
{
// We do not create a KF
// Advance this
- // last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
- incoming_ptr_->link(last_ptr_->getFrame());
- last_ptr_->remove();
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
+ advanceDerived();
+
+ // Replace last frame for a new one in incoming
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_->link(frm);
+ last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
// Update pointers
- advanceDerived();
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
+ last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
+ incoming_ptr_ = nullptr;
}
break;
}
@@ -251,7 +238,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
break;
}
- number_of_tracks_ = last_ptr_->getFeatureList().size();
postProcess();
}
@@ -271,7 +257,7 @@ void ProcessorTracker::computeProcessingStep()
{
case FIRST_TIME :
- if (kf_pack_buffer_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
+ if (buffer_pack_kf_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
processing_step_ = FIRST_TIME_WITH_PACK;
else // ! last && ! pack(incoming)
processing_step_ = FIRST_TIME_WITHOUT_PACK;
@@ -279,7 +265,7 @@ void ProcessorTracker::computeProcessingStep()
case SECOND_TIME :
- if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+ if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
processing_step_ = SECOND_TIME_WITH_PACK;
else
processing_step_ = SECOND_TIME_WITHOUT_PACK;
@@ -288,16 +274,15 @@ void ProcessorTracker::computeProcessingStep()
case RUNNING :
default :
- if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+ if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
{
- if (last_ptr_->getFrame()->isKey())
+ if (last_ptr_->getFrame()->getProblem())
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
- WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
+ WOLF_INFO("Pack's KF and last's Frame have matching time stamps (i.e. below time tolerances)");
WOLF_INFO("Check the following for correctness:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
- WOLF_INFO(" - You 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).");
}
@@ -309,5 +294,30 @@ void ProcessorTracker::computeProcessingStep()
}
}
+bool ProcessorTracker::triggerInCapture(CaptureBasePtr _cap_ptr) const
+{
+ return true;
+}
+bool ProcessorTracker::storeKeyFrame(FrameBasePtr _frame_ptr)
+{
+ return true;
+}
+bool ProcessorTracker::storeCapture(CaptureBasePtr _cap_ptr)
+{
+ return false;
+}
+
+void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+ if (getOrigin())
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
+}
} // namespace wolf
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index dbcc3646b0954a4e34da071ba7b34c878d79e265..cda95e5ed6259e7ef11d94d01c3fe939ffe61941 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -11,8 +11,10 @@ namespace wolf
{
ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
- ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTracker(_type, _params_tracker_feature),
+ const StateStructure& _structure,
+ int _dim,
+ ParamsProcessorTrackerFeaturePtr _params_tracker_feature) :
+ ProcessorTracker(_type, _structure, _dim, _params_tracker_feature),
params_tracker_feature_(_params_tracker_feature)
{
}
@@ -36,78 +38,103 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
matches_last_from_incoming_.clear();
// Populate the last Capture with new Features. The result is in new_features_last_.
- unsigned int n = detectNewFeatures(_max_new_features, new_features_last_);
+ unsigned int n = detectNewFeatures(_max_new_features,
+ last_ptr_,
+ new_features_last_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "some features not linked after returning from detectNewFeatures()");
+
+ // fill the track matrix
for (auto ftr : new_features_last_)
- track_matrix_.newTrack(last_ptr_, ftr);
+ {
+ assert(std::find(known_features_last_.begin(), known_features_last_.end(), ftr) == known_features_last_.end() && "detectNewFeatures() provided a new feature that is already in known_features_last_`");
+ track_matrix_.newTrack(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_);
+ trackFeatures(new_features_last_,
+ incoming_ptr_,
+ new_features_incoming_,
+ matches_last_from_incoming_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by trackFeatures()");
+
+ // fill the track matrix
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);
+ assert(matches_last_from_incoming_.count(ftr) != 0);
+ track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
}
- // Append all new Features to the incoming Captures' list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
-
- // Append all new Features to the last Captures' list of Features
- last_ptr_->addFeatureList(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()");
+ //assert(incoming_ptr_->getFeatureList().size() == 0
+ // && "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
// clear list of known features
+ matches_last_from_incoming_.clear();
known_features_incoming_.clear();
- if (!last_ptr_ || last_ptr_->getFeatureList().empty())
+ if (!last_ptr_)
+ {
+ WOLF_TRACE("null last, returning...");
+ return 0;
+ }
+
+ if (known_features_last_.empty())
{
+ WOLF_DEBUG("Empty last feature list, 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_)
+ trackFeatures(known_features_last_,
+ incoming_ptr_,
+ known_features_incoming_,
+ matches_last_from_incoming_);
+
+ // check all features have been emplaced
+ assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by trackFeatures()");
+
+ // fill the track matrix
+ for (auto ftr : known_features_incoming_)
{
- SizeStd trk_id_from_last = match.second->feature_ptr_->trackId();
- track_matrix_.add(trk_id_from_last, incoming_ptr_, match.first);
+ assert(matches_last_from_incoming_.count(ftr) != 0);
+ track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
}
// Check/correct incoming-origin correspondences
if (origin_ptr_ != nullptr)
{
- for (auto feature_in_incoming : known_features_incoming_)
+ auto ftr_inc_it = known_features_incoming_.begin();
+ while (ftr_inc_it != known_features_incoming_.end())
{
- SizeStd track_id = feature_in_incoming->trackId();
+ SizeStd track_id = (*ftr_inc_it)->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))
- {
- feature_in_incoming->setProblem(this->getProblem());
- }
- else
+ if (!correctFeatureDrift(feature_in_origin, feature_in_last, (*ftr_inc_it)))
{
// 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
+ assert(matches_last_from_incoming_ .count(*ftr_inc_it));
+ matches_last_from_incoming_ .erase (*ftr_inc_it); // remove match
+ track_matrix_ .remove(*ftr_inc_it); // remove from track matrix
+ (*ftr_inc_it) ->remove(); // remove from wolf tree
+ ftr_inc_it = known_features_incoming_.erase(ftr_inc_it); // remove from known features list
}
+ else
+ ftr_inc_it++;
}
}
- // Add to wolf tree
- incoming_ptr_->addFeatureList(known_features_incoming_);
-
return matches_last_from_incoming_.size();
}
@@ -115,10 +142,9 @@ 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());
+ known_features_last_ = std::move(known_features_incoming_);
+ //new_features_incoming should be zero!
+ //known_features_last_.splice(new_features_incoming_);
// // remove last from track matrix in case you want to have only KF in the track matrix
// track_matrix_.remove(last_ptr_);
@@ -128,22 +154,21 @@ 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());
- }
+ known_features_last_ = std::move(known_features_incoming_);
+ known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
+
+ // Debug
+ //for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))
+ //{
+ // WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", pair_trkid_pair.second.first->id(), " last: ", pair_trkid_pair.second.second->id());
+ //}
}
void ProcessorTrackerFeature::establishFactors()
{
+ if (origin_ptr_ == last_ptr_)
+ return;
+
TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
for (auto const & pair_trkid_pair : matches_origin_last)
@@ -152,12 +177,13 @@ void ProcessorTrackerFeature::establishFactors()
FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- auto fac_ptr = createFactor(feature_in_last, feature_in_origin);
- fac_ptr->link(feature_in_last);
+ auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
+
+ assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()");
WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
+ " origin: " , feature_in_origin->id() ,
+ " from last: " , feature_in_last->id() );
}
}
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
deleted file mode 100644
index c6776995796c55b82a70acd396ea0dca63237190..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 "core/processor/processor_tracker_feature_dummy.h"
-#include "core/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[_features_incoming_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,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 incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
-}
-
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_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::Ones(1, 1)));
- _features_incoming_out.push_back(ftr);
-
- WOLF_INFO("feature " , ftr->id() , " detected!" );
- }
-
- WOLF_INFO(_features_incoming_out.size() , " features detected!");
-
- return _features_incoming_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<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr, shared_from_this());
-
- return ctr;
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 1706985724d3d61d388a2d70e83fb87c12483a4f..ec16f76dc095de7cf01ebf45400d0eacd7733516 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -14,8 +14,12 @@ namespace wolf
{
ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTracker(_type, _params_tracker_landmark),
+ const StateStructure& _structure,
+ ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark) :
+ ProcessorTracker(_type,
+ _structure,
+ 0,
+ _params_tracker_landmark),
params_tracker_landmark_(_params_tracker_landmark)
{
//
@@ -23,49 +27,47 @@ ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
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_);
+ known_features_last_ = std::move(known_features_incoming_);
+ known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
+
+ matches_landmark_from_incoming_.clear();
+ new_features_incoming_.clear();
+ new_landmarks_.clear();
}
void ProcessorTrackerLandmark::resetDerived()
{
- 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_);
+ known_features_last_ = std::move(known_features_incoming_);
+ known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
+
+ matches_landmark_from_incoming_.clear();
+ new_features_incoming_.clear();
+ new_landmarks_.clear();
}
unsigned int ProcessorTrackerLandmark::processKnown()
{
- // clear matches list
matches_landmark_from_incoming_.clear();
+ known_features_incoming_.clear();
// 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);
+ incoming_ptr_,
+ known_features_incoming_,
+ matches_landmark_from_incoming_);
+
+ // check all features have been emplaced
+ assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by findLandmarks()");
return n;
}
@@ -75,54 +77,56 @@ 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.
+ * eventually create Landmarks with them.
+ * When done, we need to find these new Landmarks in the incoming Capture.
*/
- // clear new lists
- new_features_last_.clear();
+ // new lists cleared
+ new_features_last_.clear();
new_features_incoming_.clear();
new_landmarks_.clear();
// We first need to populate the \b last Capture with new Features
- unsigned int n = detectNewFeatures(_max_features, new_features_last_);
+ unsigned int n = detectNewFeatures(_max_features,
+ last_ptr_,
+ new_features_last_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by detectNewFeatures()");
// create new landmarks with the new features discovered
- createNewLandmarks();
+ emplaceNewLandmarks();
// 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 incoming Capture's list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
+ findLandmarks(new_landmarks_,
+ incoming_ptr_,
+ new_features_incoming_,
+ matches_landmark_from_incoming_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "some features not linked after returning from detectNewFeatures()");
}
- // Append all new Features to the last 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()
+void ProcessorTrackerLandmark::emplaceNewLandmarks()
{
- // First, make sure the list is empty and will only contain new lmks
- new_landmarks_.clear();
+ // assuming empty list
+ assert(new_landmarks_.empty());
// 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);
+ LandmarkBasePtr new_lmk_ptr = emplaceLandmark(new_feature_ptr);
+
+ assert(new_lmk_ptr->getMap() != nullptr && "not linked landmark returned by emplaceLandmark()");
new_landmarks_.push_back(new_lmk_ptr);
@@ -131,31 +135,19 @@ void ProcessorTrackerLandmark::createNewLandmarks()
}
}
-// 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
+ // Loop all features tracked in last_ptr_ (two lists known_features_last_ and new_features_last_)
+ std::list<FeatureBasePtrList> both_lists{known_features_last_, new_features_last_};
+ for (auto feature_list : both_lists)
+ for (auto last_feature : feature_list)
{
- fac_ptr->link(last_feature);
+ assert(matches_landmark_from_last_.count(last_feature) == 1);
+
+ FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
+
+ assert(fac_ptr->getFeature() != nullptr && "not linked factor returned by emplaceFactor()");
}
- }
}
} // namespace wolf
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index af0bff7f350f7a317548de3b9048fad4874a8bfd..da6a88b75e98e0ada648f76155f5d12501c21c4d 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -22,7 +22,7 @@ TrackMatrix::~TrackMatrix()
//
}
-Track TrackMatrix::track(size_t _track_id)
+Track TrackMatrix::track(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id);
@@ -30,42 +30,47 @@ Track TrackMatrix::track(size_t _track_id)
return Track();
}
-Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
+Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture) const
{
- if (_capture && snapshots_.count(_capture->id()) > 0)
- return snapshots_.at(_capture->id());
+ if (_capture && snapshots_.count(_capture) > 0)
+ return snapshots_.at(_capture);
else
return Snapshot();
}
-void TrackMatrix::newTrack(CaptureBasePtr _cap, FeatureBasePtr _ftr)
+void TrackMatrix::newTrack(FeatureBasePtr _ftr)
{
track_id_count_ ++;
- add(track_id_count_, _cap, _ftr);
+ add(track_id_count_, _ftr);
}
-void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr)
+void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr)
{
- assert( (_track_id > 0) && (_track_id <= track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
+ assert(( tracks_.count(_track_id) != 0 || _track_id == track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
+ assert( _ftr->getCapture() != nullptr && "adding a feature not linked to any capture");
_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
+ tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr);
+ snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
}
-void TrackMatrix::remove(size_t _track_id)
+void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new)
+{
+ add(_ftr_existing->trackId(), _ftr_new);
+}
+
+void TrackMatrix::remove(const SizeStd& _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);
+ CaptureBasePtr cap = pair_time_ftr.second->getCapture();
+ snapshots_.at(cap).erase(_track_id);
+ if (snapshots_.at(cap).empty())
+ snapshots_.erase(cap);
+
}
// Remove track
@@ -76,10 +81,10 @@ void TrackMatrix::remove(size_t _track_id)
void TrackMatrix::remove(CaptureBasePtr _cap)
{
// remove snapshot features from all tracks
- if (snapshots_.count(_cap->id()))
+ if (snapshots_.count(_cap))
{
TimeStamp ts = _cap->getTimeStamp();
- for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
+ for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
{
SizeStd trk_id = pair_trkid_ftr.first;
tracks_.at(trk_id).erase(ts);
@@ -88,7 +93,7 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
}
// remove snapshot
- snapshots_.erase(_cap->id());
+ snapshots_.erase(_cap);
}
}
@@ -99,28 +104,33 @@ void TrackMatrix::remove(FeatureBasePtr _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());
+ if(tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp()))
+ {
+ tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
+ if (tracks_.at(_ftr->trackId()).empty())
+ tracks_.erase(_ftr->trackId());
+ }
+ if(snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId()))
+ {
+ snapshots_. at(cap).erase(_ftr->trackId());
+ if (snapshots_.at(cap).empty())
+ snapshots_.erase(cap);
+ }
}
}
}
-size_t TrackMatrix::numTracks()
+size_t TrackMatrix::numTracks() const
{
return tracks_.size();
}
-size_t TrackMatrix::trackSize(size_t _track_id)
+size_t TrackMatrix::trackSize(const SizeStd& _track_id) const
{
return track(_track_id).size();
}
-FeatureBasePtr TrackMatrix::firstFeature(size_t _track_id)
+FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).begin()->second;
@@ -128,7 +138,7 @@ FeatureBasePtr TrackMatrix::firstFeature(size_t _track_id)
return nullptr;
}
-FeatureBasePtr TrackMatrix::lastFeature(size_t _track_id)
+FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).rbegin()->second;
@@ -136,7 +146,7 @@ FeatureBasePtr TrackMatrix::lastFeature(size_t _track_id)
return nullptr;
}
-vector<FeatureBasePtr> TrackMatrix::trackAsVector(size_t _track_id)
+vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
{
vector<FeatureBasePtr> vec;
if (tracks_.count(_track_id))
@@ -148,16 +158,16 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(size_t _track_id)
return vec;
}
-std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
+std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap) const
{
std::list<FeatureBasePtr> lst;
- if (snapshots_.count(_cap->id()))
- for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
+ if (snapshots_.count(_cap))
+ for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
lst.push_back(pair_trkid_ftr.second);
return lst;
}
-TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
+TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const
{
TrackMatches pairs;
@@ -181,7 +191,7 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
return pairs;
}
-FeatureBasePtr TrackMatrix::feature(size_t _track_id, CaptureBasePtr _cap)
+FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap) const
{
if (snapshot(_cap).count(_track_id))
return snapshot(_cap).at(_track_id);
@@ -189,9 +199,28 @@ FeatureBasePtr TrackMatrix::feature(size_t _track_id, CaptureBasePtr _cap)
return nullptr;
}
-CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
+CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
{
return firstFeature(_track_id)->getCapture();
}
+Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
+{
+ // We assemble a track_kf on the fly by checking each capture's frame.
+ if (tracks_.count(_track_id))
+ {
+ Track track_kf;
+ for (auto& pair_ts_ftr : tracks_.at(_track_id))
+ {
+ auto& ts = pair_ts_ftr.first;
+ auto& ftr = pair_ts_ftr.second;
+ if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem())
+ track_kf[ts] = ftr;
+ }
+ return track_kf;
+ }
+ else
+ return Track();
+}
+
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index c633261ade4a3835e38969eb122d59d5be014928..0700d403f00f24fc4effcc8d176cdea9277be1a5 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -14,50 +14,61 @@ SensorBase::SensorBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const unsigned int _noise_size,
- const bool _extr_dyn,
+ const bool _p_dyn,
+ const bool _o_dyn,
const bool _intr_dyn) :
NodeBase("SENSOR", _type),
+ HasStateBlocks(""),
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_cov_(_noise_size, _noise_size),
+ last_capture_(nullptr)
{
+ assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
+ assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
+ assert((_intr_ptr or not _intr_dyn) and "Trying to set dynamic intrinsics state block without providing an intrinsics state block. It is required anyway.");
+
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();
+
+ if (_p_ptr)
+ addStateBlock('P', _p_ptr, _p_dyn);
+
+ if (_o_ptr)
+ addStateBlock('O', _o_ptr, _o_dyn);
+
+ if (_intr_ptr)
+ addStateBlock('I', _intr_ptr, _intr_dyn);
+
}
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 Eigen::VectorXd & _noise_std,
+ const bool _p_dyn,
+ const bool _o_dyn,
const bool _intr_dyn) :
NodeBase("SENSOR", _type),
+ HasStateBlocks(""),
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())
+ noise_cov_(_noise_std.size(), _noise_std.size()),
+ last_capture_(nullptr)
{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
setNoiseStd(_noise_std);
- updateCalibSize();
+
+ if (_p_ptr)
+ addStateBlock('P', _p_ptr, _p_dyn);
+
+ if (_o_ptr)
+ addStateBlock('O', _o_ptr, _o_dyn);
+
+ if (_intr_ptr)
+ addStateBlock('I', _intr_ptr, _intr_dyn);
}
SensorBase::~SensorBase()
@@ -68,86 +79,71 @@ SensorBase::~SensorBase()
void SensorBase::removeStateBlocks()
{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const auto& key : getStructure())
{
- auto sbp = getStateBlockPtrStatic(i);
+ auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
- if (getProblem() != nullptr && !extrinsic_dynamic_)
+ if (getProblem() != nullptr && !isStateBlockInCapture(key))
{
- getProblem()->removeStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,REMOVE);
}
- setStateBlockPtrStatic(i, nullptr);
+ removeStateBlock(key);
}
}
}
-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++)
+ for (const auto& key : std::string("PO"))
{
- auto sbp = state_block_vec_[i];
+ auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
sbp->fix();
}
- updateCalibSize();
}
void SensorBase::unfixExtrinsics()
{
- for (unsigned int i = 0; i < 2; i++)
+ for (const auto& key : std::string("PO"))
{
- auto sbp = state_block_vec_[i];
+ auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
sbp->unfix();
}
- updateCalibSize();
}
void SensorBase::fixIntrinsics()
{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
+ for (const auto& key : getStructure())
{
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
+ if (key != 'P' and key != 'O') // exclude extrinsics
+ {
+ auto sbp = getStateBlockDynamic(key);
+ if (sbp != nullptr)
+ sbp->fix();
+ }
}
- updateCalibSize();
}
void SensorBase::unfixIntrinsics()
{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
+ for (const auto& key : getStructure())
{
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
+ if (key != 'P' and key != 'O') // exclude extrinsics
+ {
+ auto sbp = getStateBlockDynamic(key);
+ 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)
+void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _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");
+ assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters");
- StateBlockPtr _sb = getStateBlockPtrStatic(_i);
+ StateBlockPtr _sb = getStateBlock(_key);
bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
@@ -161,173 +157,168 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs&
_sb->setState(_x);
else
{
- Eigen::VectorXs new_x = _sb->getState();
+ Eigen::VectorXd 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();
+ if (params_prior_map_.find(_key) != params_prior_map_.end())
+ params_prior_map_[_key]->remove();
// create feature
- FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
+ FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov); //std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
// set feature problem
- ftr_prior->setProblem(getProblem());
+ if(getProblem()) ftr_prior->setProblem(getProblem());
// create & add factor absolute
- // ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
- // ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
if (is_quaternion)
- FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb);
+ FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, ftr_prior, _sb, nullptr, false);
else
- FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size);
+ FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _size, nullptr, false);
// store feature in params_prior_map_
- params_prior_map_[_i] = ftr_prior;
+ params_prior_map_[_key] = ftr_prior;
}
-void SensorBase::registerNewStateBlocks()
+void SensorBase::registerNewStateBlocks() const
{
if (getProblem() != nullptr)
{
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
+ for (auto & pair_key_sbp : getStateBlockMap())
{
- 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);
- }
+ auto key = pair_key_sbp.first;
+ auto sbp = pair_key_sbp.second;
+
+ if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key))
+ getProblem()->notifyStateBlock(sbp, ADD);
}
}
}
-void SensorBase::setNoiseStd(const Eigen::VectorXs& _noise_std) {
+void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) {
noise_std_ = _noise_std;
noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
}
-void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
+void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
noise_std_ = _noise_cov.diagonal().array().sqrt();
noise_cov_ = _noise_cov;
}
-CaptureBasePtr SensorBase::lastKeyCapture(void)
+void SensorBase::setLastCapture(CaptureBasePtr cap)
+{
+ assert(cap);
+ assert(cap->getSensor() == shared_from_this());
+ assert(cap->getTimeStamp().ok());
+ assert(not last_capture_ or last_capture_->getTimeStamp() < cap->getTimeStamp());
+ last_capture_ = cap;
+}
+
+void SensorBase::updateLastCapture()
{
// 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 (getProblem())
{
- if ((*frame_rev_it)->isKey())
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+ auto trajectory = getProblem()->getTrajectory();
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
- capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
+ auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture and not capture->isRemoving())
+ {
// found the most recent Capture made by this sensor !
- break;
+ last_capture_ = capture;
+ return;
+ }
+ frame_rev_it++;
}
- frame_rev_it++;
}
- return capture;
+
+ // no captures found
+ last_capture_ = nullptr;
}
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
+CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
{
// we search for the most recent Capture of this sensor before _ts
- CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+ if (not getProblem())
+ return nullptr;
+
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+ auto trajectory = getProblem()->getTrajectory();
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
if ((*frame_rev_it)->getTimeStamp() <= _ts)
{
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
- break;
+ return capture;
}
frame_rev_it++;
}
- return capture;
-}
-StateBlockPtr SensorBase::getP(const TimeStamp _ts)
-{
- return getStateBlock(0, _ts);
+ return nullptr;
}
-StateBlockPtr SensorBase::getO(const TimeStamp _ts)
+StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
{
- return getStateBlock(1, _ts);
+ return getStateBlockDynamic('P', _ts);
}
-StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
+StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
{
- return getStateBlock(2, _ts);
+ return getStateBlockDynamic('O', _ts);
}
-StateBlockPtr SensorBase::getP()
+StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
{
- return getStateBlock(0);
+ return getStateBlockDynamic('I', _ts);
}
-StateBlockPtr SensorBase::getO()
+StateBlockPtr SensorBase::getP() const
{
- return getStateBlock(1);
+ return getStateBlockDynamic('P');
}
-StateBlockPtr SensorBase::getIntrinsic()
+StateBlockPtr SensorBase::getO() const
{
- return getStateBlock(2);
+ return getStateBlockDynamic('O');
}
-SizeEigen SensorBase::computeCalibSize() const
+StateBlockPtr SensorBase::getIntrinsic() 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;
+ return getStateBlockDynamic('I');
}
bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
capture_ptr->setSensor(shared_from_this());
- for (const auto processor : processor_list_)
+ for (const auto& processor : processor_list_)
{
- processor->process(capture_ptr);
+
+#ifdef PROFILING
+ auto start = std::chrono::high_resolution_clock::now();
+#endif
+ processor->captureCallback(capture_ptr);
+#ifdef PROFILING
+ auto stop = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
+ WOLF_INFO("captureCallback Prc_id: ", processor->id(),
+ " Prc_type: ", processor->getType(),
+ " Prc_name: ", processor->getName(),
+ " Cptr_id: ", capture_ptr->id(),
+ " Cptr_type: ", capture_ptr->getType(),
+ " timestamp: ", capture_ptr->getTimeStamp(),
+ " microseconds: ", duration.count());
+#endif
}
return true;
@@ -339,62 +330,67 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
return _proc_ptr;
}
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
+void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
+{
+ processor_list_.remove(_proc_ptr);
+}
+
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
{
CaptureBasePtr cap;
- if (isStateBlockDynamic(_i, cap))
- return cap->getStateBlock(_i);
+ if (isStateBlockInCapture(_key, cap))
+ return cap->getStateBlock(_key);
- return getStateBlockPtrStatic(_i);
+ return getStateBlock(_key);
}
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
- if (isStateBlockDynamic(_i, _ts, cap))
- return cap->getStateBlock(_i);
+ if (isStateBlockInCapture(_key, _ts, cap))
+ return cap->getStateBlock(_key);
- return getStateBlockPtrStatic(_i);
+ return getStateBlock(_key);
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
+bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const
{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
+ if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
{
- cap = lastKeyCapture();
+ _cap = last_capture_;
- return cap != nullptr;
+ return _cap != nullptr;
}
else
return false;
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap)
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
+ if (isStateBlockDynamic(_key))
{
- cap = lastCapture(_ts);
+ _cap = findLastCaptureBefore(_ts);
- return cap != nullptr;
+ return _cap != nullptr;
}
else
return false;
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i)
+bool SensorBase::isStateBlockInCapture(const char& _key) const
{
CaptureBasePtr cap;
- return isStateBlockDynamic(_i,cap);
+ return isStateBlockInCapture(_key, cap);
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts)
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
- return isStateBlockDynamic(_i,_ts,cap);
+ return isStateBlockInCapture(_key, _ts, cap);
}
void SensorBase::setProblem(ProblemPtr _problem)
@@ -402,11 +398,14 @@ void SensorBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
for (auto prc : processor_list_)
prc->setProblem(_problem);
+ for(auto ftr_prior : params_prior_map_)
+ ftr_prior.second->setProblem(_problem);
}
void SensorBase::link(HardwareBasePtr _hw_ptr)
{
- std::cout << "Linking SensorBase" << std::endl;
+ assert(!is_removing_ && "linking a removed sensor");
+ assert(this->getHardware() == nullptr && "linking an already linked sensor");
if(_hw_ptr)
{
this->setHardware(_hw_ptr);
@@ -416,8 +415,129 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
}
else
{
- WOLF_WARN("Linking with a nullptr");
+ WOLF_WARN("Linking Sensor ", id(), " to a nullptr");
+ }
+}
+
+void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Sen" << id() << " " << getType() << " \"" << getName() << "\"";
+ if (_depth < 2)
+ _stream << " -- " << getProcessorList().size() << "p";
+ _stream << std::endl;
+
+ printState(_metric, _state_blocks, _stream, _tabs);
+}
+
+void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ if (_metric && _state_blocks)
+ {
+ for (const auto &key : getStructure())
+ {
+ auto sb = getStateBlockDynamic(key);
+ if (sb)
+ _stream << _tabs << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
+ << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") << ",\t x = ( " << std::setprecision(3)
+ << getStateVector().transpose() << " )" << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (const auto &key : getStructure())
+ {
+ const auto &sb = getStateBlockDynamic(key);
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
+ }
+ _stream << std::endl;
}
}
+void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+
+ if (_depth >= 2)
+ for (auto p : getProcessorList())
+ if (p)
+ p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ if (_verbose)
+ {
+ _stream << _tabs << "Sen" << id() << " @ " << _sen_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Hrw @ " << getHardware().get() << std::endl;
+ for (auto pair: getStateBlockMap())
+ {
+ auto sb = pair.second;
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ std::stringstream inconsistency_explanation;
+ auto hwd_ptr = getHardware();
+ // check problem and hardware pointers
+ inconsistency_explanation << "Sensor problem pointer " << getProblem().get()
+ << " different from Hardware problem pointer " << hwd_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == hwd_ptr->getProblem()), inconsistency_explanation);
+
+ inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
+ << " ---> Hwd" << " @ " << hwd_ptr
+ << " -X-> Sen" << id();
+ auto hwd_sen_list = hwd_ptr->getSensorList();
+ auto hwd_has_sen = std::find_if(hwd_sen_list.begin(), hwd_sen_list.end(), [&_sen_ptr](SensorBasePtr sen){ return sen == _sen_ptr;});
+ log.assertTrue(hwd_has_sen != hwd_sen_list.end(), inconsistency_explanation);
+
+ for(auto prc : getProcessorList())
+ {
+ inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
+ << " ---> Prc" << prc->id() << " @ " << prc
+ << " -X-> Sen" << id();
+ log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
+ }
+ // check last_capture_
+ if (getProblem()->getTimeStamp().ok())
+ {
+ auto last_capture_found = findLastCaptureBefore(getProblem()->getTimeStamp());
+ inconsistency_explanation << "SensorBase::last_capture_: "
+ << (last_capture_ ? std::to_string(last_capture_->id()) : "-")
+ << " @ " << last_capture_
+ << " is not the actual last capture: "
+ << (last_capture_found ?
+ std::to_string(last_capture_found->id()) :
+ "-")
+ << " @ " << last_capture_found << std::endl;
+ log.assertTrue(last_capture_ == last_capture_found, inconsistency_explanation);
+ }
+ return log;
+}
+
+bool SensorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto sen_ptr = std::static_pointer_cast<SensorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for (auto p : getProcessorList()) p->check(_log, p, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
+
+
} // namespace wolf
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 727e8bfb6308bef33798785eabc9beff109f2bc2..afdc43e02a8085feb99c4ae0ac1a9a4cdefe2162 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -1,133 +1,52 @@
-#include "core/sensor/sensor_diff_drive.h"
-#include "core/state_block/state_block.h"
-#include "core/capture/capture_motion.h"
-#include "core/utils/eigen_assert.h"
+/**
+ * \file sensor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
-namespace wolf {
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/state_block/state_angle.h"
-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}
+namespace wolf
{
- //
-}
-// Define the factory method
-SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
- const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
+SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
+ ParamsSensorDiffDrivePtr _intrinsics) :
+ SensorBase("SensorDiffDrive",
+ std::make_shared<StateBlock>(_extrinsics.head(2), true),
+ std::make_shared<StateAngle>(_extrinsics(2), true),
+ std::make_shared<StateBlock>(3, false), 2),
+ params_diff_drive_(_intrinsics)
{
- 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_, false);
-
- SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params);
+ radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
+ getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
+ unfixIntrinsics();
- odo->setName(_unique_name);
+ if(params_diff_drive_->set_intrinsics_prior)
+ addPriorIntrinsics(getIntrinsic()->getState(), params_diff_drive_->prior_cov_diag.asDiagonal());
- /// @todo make calibration optional at creation
- //if (calibrate)
- // odo->unfixIntrinsics();
+ // compute noise covariance
+ // 1. measured wheel revolutions: sigma = 2*radians_per_tick
+ double sigma_rev = 2*radians_per_tick;
+ Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
- return odo;
+ setNoiseStd(noise_sigma);
+
}
-/// @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);
-// }
+SensorDiffDrive::~SensorDiffDrive()
+{
+ // Auto-generated destructor stub
+}
-// /// @todo tofix
-// return false;//SensorBase::addCapture(_capture_ptr);
-//}
-} // namespace wolf
+} /* namespace wolf */
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
+WOLF_REGISTER_SENSOR(SensorDiffDrive);
+WOLF_REGISTER_SENSOR_AUTO(SensorDiffDrive);
} // namespace wolf
+
diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6eb00f021667436360821f27e5684a512eb425e4
--- /dev/null
+++ b/src/sensor/sensor_model.cpp
@@ -0,0 +1,24 @@
+#include "core/sensor/sensor_model.h"
+
+namespace wolf {
+
+
+SensorModel::SensorModel() :
+ SensorBase("SensorModel", nullptr, nullptr, nullptr, 6)
+{
+ //
+}
+
+SensorModel::~SensorModel()
+{
+ //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorModel);
+WOLF_REGISTER_SENSOR_AUTO(SensorModel);
+}
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
deleted file mode 100644
index f6b408c7ec9bb68544e5b5f24019bde710954c93..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_odom_2D.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/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;
-}
-
-SensorBasePtr SensorOdom2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server)
-{
- // Eigen::VectorXs _extrinsics_po = Eigen::Vector3s(0,0,0);
- Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos", "[0,0,0]");
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
- SensorOdom2DPtr odo;
- IntrinsicsOdom2D params;
- params.k_disp_to_disp = _server.getParam<double>(_unique_name + "/intrinsic/k_disp_to_disp", "1");
- params.k_rot_to_rot = _server.getParam<double>(_unique_name + "/intrinsic/k_rot_to_rot", "1");
- odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
- odo->setName(_unique_name);
- return odo;
-}
-
-}
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D)
-} // namespace wolf
-#include "core/sensor/autoconf_sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR_AUTO("ODOM 2D", SensorOdom2D)
-} // namespace wolf
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..574e4277e9181ee768b9bb08df1ba3f4d5082dd0
--- /dev/null
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -0,0 +1,56 @@
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_angle.h"
+
+namespace wolf {
+
+SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics) :
+ SensorBase("SensorOdom2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
+ 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::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics) :
+ SensorOdom2d(_extrinsics, *_intrinsics)
+{
+ //
+}
+
+SensorOdom2d::~SensorOdom2d()
+{
+ //
+}
+
+double SensorOdom2d::getDispVarToDispNoiseFactor() const
+{
+ return k_disp_to_disp_;
+}
+
+double SensorOdom2d::getRotVarToRotNoiseFactor() const
+{
+ return k_rot_to_rot_;
+}
+
+Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
+{
+ assert(_data.size() == 2);
+ double d = fabs(_data(0));
+ double r = fabs(_data(1));
+
+ double dvar = k_disp_to_disp_ * d;
+ double rvar = k_rot_to_rot_ * r;
+
+ return (Vector2d() << dvar, rvar).finished().asDiagonal();
+}
+
+}
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorOdom2d);
+WOLF_REGISTER_SENSOR_AUTO(SensorOdom2d);
+} // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
deleted file mode 100644
index cf6431489efb45c0e0bf4599899bc9c850ebf902..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 "core/sensor/sensor_odom_3D.h"
-
-#include "core/state_block/state_block.h"
-#include "core/state_block/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 "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D)
-}
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b261029d677a40c7c8b821b1e4708251d4bd38ca
--- /dev/null
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -0,0 +1,63 @@
+/**
+ * \file sensor_odom_3d.cpp
+ *
+ * Created on: Oct 7, 2016
+ * \author: jsola
+ */
+
+#include "core/sensor/sensor_odom_3d.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& _intrinsics) :
+ SensorBase("SensorOdom3d", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+ 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::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
+ setNoiseCov(noise_cov_); // sets also noise_std_
+}
+
+SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr _intrinsics) :
+ SensorOdom3d(_extrinsics_pq, *_intrinsics)
+{
+ //
+}
+
+SensorOdom3d::~SensorOdom3d()
+{
+ //
+}
+
+Eigen::Matrix6d SensorOdom3d::computeCovFromMotion (const VectorXd& _data) const
+{
+ double d = _data.head<3>().norm();
+ double r;
+ if (_data.size() == 6)
+ r = _data.tail<3>().norm();
+ else
+ r = 2 * acos(_data(6)); // arc cos of real part of quaternion
+
+ double dvar = min_disp_var_ + k_disp_to_disp_ * d;
+ double rvar = min_rot_var_ + k_disp_to_rot_ * d + k_rot_to_rot_ * r;
+
+ return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorOdom3d);
+WOLF_REGISTER_SENSOR_AUTO(SensorOdom3d);
+}
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c308bed7130e146599e857170655488d52df3521
--- /dev/null
+++ b/src/sensor/sensor_pose.cpp
@@ -0,0 +1,45 @@
+/**
+ * \file sensor_pose.cpp
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+#include "core/sensor/sensor_pose.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
+ SensorBase("SensorPose", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+ std_p_(_intrinsics.std_p),
+ std_o_(_intrinsics.std_o)
+{
+ assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
+
+ noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
+ setNoiseCov(noise_cov_); // sets also noise_std_
+}
+
+SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :
+ SensorPose(_extrinsics_pq, *_intrinsics)
+{
+ //
+}
+
+SensorPose::~SensorPose()
+{
+ //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorPose);
+WOLF_REGISTER_SENSOR_AUTO(SensorPose);
+}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index f6d2914da4ff77e594ee0b2722627897f6fb23e5..99dfb8729c9bc5cb05f00758494571de9d52fc23 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -5,19 +5,47 @@
namespace wolf {
-SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
- wolf_problem_(_wolf_problem)
+SolverManager::SolverManager(const ProblemPtr& _problem) :
+ SolverManager(_problem, std::make_shared<ParamsSolver>())
+{
+}
+
+SolverManager::SolverManager(const ProblemPtr& _problem,
+ const ParamsSolverPtr& _params) :
+ n_solve_(0),
+ n_cov_(0),
+ acc_duration_total_(0),
+ acc_duration_solver_(0),
+ acc_duration_update_(0),
+ acc_duration_state_(0),
+ acc_duration_cov_(0),
+ max_duration_total_(0),
+ max_duration_solver_(0),
+ max_duration_update_(0),
+ max_duration_state_(0),
+ max_duration_cov_(0),
+ wolf_problem_(_problem),
+ params_(_params)
+{
+ assert(_problem != nullptr && "Passed a nullptr ProblemPtr.");
+}
+
+SolverManager::~SolverManager()
{
- assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
}
void SolverManager::update()
{
// Consume notification maps
- auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
- auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
+ std::map<StateBlockPtr,Notification> sb_notification_map;
+ std::map<FactorBasePtr,Notification> fac_notification_map;
+ wolf_problem_->consumeNotifications(sb_notification_map, fac_notification_map);
+
+ #ifdef _WOLF_DEBUG
+ assert(check("before update()"));
+ #endif
- // REMOVE CONSTRAINTS
+ // REMOVE FACTORS
for (auto fac_notification_it = fac_notification_map.begin();
fac_notification_it != fac_notification_map.end();
/* nothing, next is handled within the for */)
@@ -34,82 +62,74 @@ void SolverManager::update()
// ADD/REMOVE STATE BLOCS
while ( !sb_notification_map.empty() )
{
- StateBlockPtr state_ptr = sb_notification_map.begin()->first;
-
+ // add
if (sb_notification_map.begin()->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 to add an already added !");
- }
- }
+ addStateBlock(sb_notification_map.begin()->first);
+
+ // remove
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
+ removeStateBlock(sb_notification_map.begin()->first);
+
+ // remove notification
sb_notification_map.erase(sb_notification_map.begin());
}
- // ADD CONSTRAINTS
+ // ADD FACTORS
while (!fac_notification_map.empty())
{
assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
// add factor
addFactor(fac_notification_map.begin()->first);
+
// remove notification
fac_notification_map.erase(fac_notification_map.begin());
}
// UPDATE STATE BLOCKS (state, fix or local parameterization)
+ std::set<StateBlockPtr> new_floating_state_blocks;
for (auto state_pair : state_blocks_)
{
auto state_ptr = state_pair.first;
- // state update
- if (state_ptr->stateUpdated())
+ // Check for "floating" state blocks (not involved in any factor -> not observable problem)
+ if (state_blocks_2_factors_.at(state_ptr).empty())
{
- 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();
+ WOLF_DEBUG("SolverManager::update(): StateBlock ", state_ptr, " is 'Floating' (not involved in any factor). Storing it apart.");
+ new_floating_state_blocks.insert(state_ptr);
+ continue;
}
+
+ // state update
+ if (state_ptr->stateUpdated())
+ updateStateBlockState(state_ptr);
+
// 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();
- }
}
+
+ // REMOVE NEWLY DETECTED "floating" STATE BLOCKS (will be added next update() call)
+ for (auto state_ptr : new_floating_state_blocks)
+ {
+ removeStateBlock(state_ptr);
+ floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()!
+ }
+ // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be set in addStateBlock)
+ for (auto state_ptr : floating_state_blocks_)
+ {
+ state_ptr->resetStateUpdated();
+ state_ptr->resetFixUpdated();
+ state_ptr->resetLocalParamUpdated();
+ }
+
+ #ifdef _WOLF_DEBUG
+ assert(check("after update()"));
+ #endif
}
wolf::ProblemPtr SolverManager::getProblem()
@@ -117,57 +137,496 @@ wolf::ProblemPtr SolverManager::getProblem()
return wolf_problem_;
}
+std::string SolverManager::solve()
+{
+ return solve(params_->verbose);
+}
+
std::string SolverManager::solve(const ReportVerbosity report_level)
{
+ auto start_total = std::chrono::high_resolution_clock::now();
+ n_solve_++;
+
// update problem
+ auto start_update = std::chrono::high_resolution_clock::now();
update();
+ auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_update);
+ acc_duration_update_ += duration_update;
+ max_duration_update_ = std::max(max_duration_update_,duration_update);
- std::string report = solveImpl(report_level);
+ // call derived solver
+ auto start_solver = std::chrono::high_resolution_clock::now();
+ std::string report = solveDerived(report_level);
+ auto duration_solver = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_solver);
+ acc_duration_solver_ += duration_solver;
+ max_duration_solver_ = std::max(max_duration_solver_,duration_solver);
// update StateBlocks with optimized state value.
+ auto start_state = std::chrono::high_resolution_clock::now();
/// @todo whatif someone has changed the state notification during opti ??
/// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
- std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
- it_end = state_blocks_.end();
- for (; it != it_end; ++it)
+ //std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
+ // it_end = state_blocks_.end();
+ for (auto& stateblock_statevector : state_blocks_)
{
// Avoid usuless copies
- if (!it->first->isFixed())
- it->first->setState(it->second, false); // false = do not raise the flag state_updated_
+ if (!stateblock_statevector.first->isFixed())
+ stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
}
+ auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
+ acc_duration_state_ += duration_state;
+ max_duration_state_ = std::max(max_duration_state_,duration_state);
+
+ auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total);
+ acc_duration_total_ += duration_total;
+ max_duration_total_ = std::max(max_duration_total_,duration_total);
return report;
}
-Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
+
+bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks)
+{
+ auto start_cov = std::chrono::high_resolution_clock::now();
+ n_cov_++;
+
+ auto ret = computeCovariancesDerived(blocks);
+
+ auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+ acc_duration_cov_ += duration_cov;
+ max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+ return ret;
+}
+
+bool SolverManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+{
+ auto start_cov = std::chrono::high_resolution_clock::now();
+ n_cov_++;
+
+ auto ret = computeCovariancesDerived(st_list);
+
+ auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+ acc_duration_cov_ += duration_cov;
+ max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+ return ret;
+}
+
+void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
+{
+ // Warning if adding an already added
+ if (factors_.count(fac_ptr) != 0)
+ {
+ WOLF_WARN("Tried to add the factor ", fac_ptr->id(), " that was already added !");
+ return;
+ }
+
+ /* HANDLING 'TRUNCATED' NOTIFICATION
+ * This happens in multi-threading if update() is called while emplacing/removing nodes
+ *
+ * ADD factor without ADD state block constrained by the factor
+ *
+ * Notification is put back on problem notifications, it will be added next update() call.
+ */
+ for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ if (state_blocks_.count(st) == 0)
+ {
+ // Check if it was stored as a 'floating' state block
+ if (floating_state_blocks_.count(st) == 1)
+ {
+ WOLF_DEBUG("SolverManager::addFactor(): Factor ", fac_ptr->id(), " involves state block ", st, " stored as 'floating'. Adding the state block to the solver.");
+ floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()!
+ addStateBlock(st);
+ }
+ else
+ {
+ WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
+ // put back notification in problem (will be added next update() call) and do nothing
+ wolf_problem_->notifyFactor(fac_ptr, ADD);
+ return;
+ }
+ }
+
+ // factors
+ factors_.insert(fac_ptr);
+
+ // state-factor map
+ for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ {
+ assert(state_blocks_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
+ assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
+ state_blocks_2_factors_.at(st).push_back(fac_ptr);
+ }
+
+ // derived
+ addFactorDerived(fac_ptr);
+}
+
+void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
+{
+ // Warning if removing a missing factor
+ if (factors_.count(fac_ptr) == 0)
+ {
+ WOLF_WARN("Tried to remove factor ", fac_ptr->id(), " that is missing !");
+ return;
+ }
+
+ // derived
+ removeFactorDerived(fac_ptr);
+
+ // factors
+ factors_.erase(fac_ptr);
+
+ // state-factor map
+ for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ {
+ assert(state_blocks_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
+ assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
+ state_blocks_2_factors_.at(st).remove(fac_ptr);
+ }
+}
+
+void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
+{
+ // Warning if adding an already added state block
+ if (state_blocks_.count(state_ptr) != 0)
+ {
+ WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that was already added !");
+ return;
+ }
+ // Warning if adding a floating state block
+ if (floating_state_blocks_.count(state_ptr) != 0)
+ {
+ WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that is stored as floating !");
+ return;
+ }
+
+ assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
+ assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
+ assert(state_ptr->isValid() && "SolverManager::addStateBlock state block state not valid (local parameterization)");
+
+ // stateblock maps
+ state_blocks_.emplace(state_ptr, state_ptr->getState());
+ state_blocks_2_factors_.emplace(state_ptr, FactorBasePtrList());
+
+ // derived
+ addStateBlockDerived(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();
+}
+
+void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
+{
+ // check if stored as 'floating' state block
+ if (floating_state_blocks_.count(state_ptr) == 1)
+ {
+ floating_state_blocks_.erase(state_ptr);
+ return;
+ }
+ // Warning if removing a missing state block
+ if (state_blocks_.count(state_ptr) == 0)
+ {
+ WOLF_WARN("Tried to remove the StateBlock ", state_ptr, " that was not added !");
+ return;
+ }
+
+ assert(state_blocks_.count(state_ptr) != 0 && "SolverManager::removeStateBlock missing state block");
+ assert(state_blocks_2_factors_.count(state_ptr) != 0 && "SolverManager::removeStateBlock missing state block");
+
+ /* HANDLING 'TRUNCATED' NOTIFICATION
+ * This happens in multi-threading if update() is called while emplacing/removing nodes
+ *
+ * REMOVE state block without REMOVE factor that constrains it
+ *
+ * Notification is put back on problem notifications, it will be removed next update() call.
+ */
+ if (!state_blocks_2_factors_.at(state_ptr).empty())
+ {
+ WOLF_WARN("SolverManager::removeStateBlock(): StateBlock ", state_ptr, " is notified to REMOVE but ", state_blocks_2_factors_.at(state_ptr).size(), " involved factors still not removed. Skipping, will be removed later.");
+ // put back notification in problem (will be removed next update() call) and do nothing
+ for (auto fac : state_blocks_2_factors_.at(state_ptr))
+ WOLF_INFO(fac->id());
+ wolf_problem_->notifyStateBlock(state_ptr, REMOVE);
+ return;
+ }
+
+ assert(state_blocks_2_factors_.at(state_ptr).empty() && "SolverManager::removeStateBlock removing state block before removing all factors involved");
+
+ // derived
+ removeStateBlockDerived(state_ptr);
+
+ // stateblock maps
+ state_blocks_.erase(state_ptr);
+ state_blocks_2_factors_.erase(state_ptr);
+}
+
+void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
+{
+ // derived
+ updateStateBlockStatusDerived(state_ptr);
+
+ // reset flag
+ state_ptr->resetFixUpdated();
+}
+
+void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
+{
+ assert(state_ptr && "SolverManager::updateStateBlockState null state block");
+ assert(state_blocks_.count(state_ptr) == 1 && "SolverManager::updateStateBlockState unregistered state block");
+ assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)");
+ assert(state_ptr->getState().size() == getAssociatedMemBlock(state_ptr).size());
+
+ Eigen::VectorXd new_state = state_ptr->getState();
+ std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
+ // reset flag
+ state_ptr->resetStateUpdated();
+}
+
+void SolverManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+{
+ // derived
+ updateStateBlockLocalParametrizationDerived(state_ptr);
+
+ // reset flag
+ state_ptr->resetLocalParamUpdated();
+}
+
+Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
{
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
+ {
+ WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+ }
return it->second;
}
-Scalar* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
+const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const
{
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
- throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+ {
+ WOLF_ERROR("Tried to retrieve the memory block const ptr of an unregistered StateBlock: ", state_ptr);
+ throw std::runtime_error("Tried to retrieve the memory block const ptr of an unregistered StateBlock !");
+ }
+ return it->second.data();
+}
+
+double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
+{
+ auto it = state_blocks_.find(state_ptr);
+ if (it == state_blocks_.end())
+ {
+ WOLF_ERROR("Tried to retrieve the memory block ptr of an unregistered StateBlock: ", state_ptr);
+ throw std::runtime_error("Tried to retrieve the memory block ptr of an unregistered StateBlock !");
+ }
return it->second.data();
}
-bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
+SolverManager::ReportVerbosity SolverManager::getVerbosity() const
+{
+ return params_->verbose;
+}
+
+bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
+{
+ return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+}
+
+bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
+{
+ return floating_state_blocks_.count(state_ptr) == 1;
+}
+
+bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
+{
+ return factors_.count(fac_ptr) == 1 and isFactorRegisteredDerived(fac_ptr);
+}
+
+bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->isFixed();
+
+ return isStateBlockFixedDerived(st);
+}
+
+bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->getLocalParametrization() == local_param;
+
+ return hasThisLocalParametrizationDerived(st, local_param);
+};
+
+bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->hasLocalParametrization();
+
+ return hasLocalParametrizationDerived(st);
+};
+
+int SolverManager::numFactors() const
+{
+ return factors_.size();
+}
+
+int SolverManager::numStateBlocks() const
+{
+ return state_blocks_.size() + floating_state_blocks_.size();
+}
+
+int SolverManager::numStateBlocksFloating() const
+{
+ return floating_state_blocks_.size();
+}
+
+double SolverManager::getPeriod() const
+{
+ return params_->period;
+}
+
+bool SolverManager::check(std::string prefix) const
{
- return state_blocks_.find(state_ptr) != state_blocks_.end() && isStateBlockRegisteredDerived(state_ptr);
+ bool ok = true;
+
+ // state blocks
+ if (state_blocks_.size() != state_blocks_2_factors_.size())
+ {
+ WOLF_ERROR("SolverManager::check: mismatching number of state blocks (state_blocks_, state_blocks_2_factors_) - in ", prefix);
+ ok = false;
+ }
+ auto sb_vec_it = state_blocks_.begin();
+ auto sb_fac_it = state_blocks_2_factors_.begin();
+ while (sb_vec_it != state_blocks_.end())
+ {
+ // same state block in both maps
+ if (sb_vec_it->first != sb_fac_it->first)
+ {
+ WOLF_ERROR("SolverManager::check: mismatching state blocks ", sb_vec_it->first, " and ", sb_fac_it->first, " - in ", prefix);
+ ok = false;
+ }
+
+ // no factors involving state block
+ if (sb_fac_it->second.empty())
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix);
+ ok = false;
+ }
+ else
+ {
+ // factor involving state block in factors_
+ for (auto fac : sb_fac_it->second)
+ {
+ if (factors_.count(fac) == 0)
+ {
+ WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
+ ok = false;
+ }
+ }
+ }
+
+ // can't be in both state_blocks_ and floating_state_blocks_
+ if (floating_state_blocks_.count(sb_fac_it->first) == 1)
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix);
+ ok = false;
+ }
+
+ sb_vec_it++;
+ sb_fac_it++;
+ }
+
+ // factors
+ for (auto fac : factors_)
+ {
+ // involved sb stored
+ for (auto sb : fac->getStateBlockPtrVector())
+ {
+ if (state_blocks_.count(sb) == 0)
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb, " inolved in factor ", fac->id(), " missing in state_blocks_ map - in ", prefix);
+ ok = false;
+ }
+ }
+ }
+
+ // CHECK DERIVED ----------------------
+ ok &= checkDerived(prefix);
+
+ // CHECK IF DERIVED IS UP TO DATE ----------------------
+ // state blocks registered in derived solver
+ for (auto sb : state_blocks_)
+ if (!isStateBlockRegisteredDerived(sb.first))
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+ ok = false;
+ }
+
+ // factors registered in derived solver
+ for (auto fac : factors_)
+ if (!isFactorRegisteredDerived(fac))
+ {
+ WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+ ok = false;
+ }
+
+ // numbers
+ if (numStateBlocksDerived() != state_blocks_.size())
+ {
+ WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+ ok = false;
+ }
+ if (numFactorsDerived() != numFactors())
+ {
+ WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+ ok = false;
+ }
+
+ return ok;
}
-bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr)
+void SolverManager::printProfiling(std::ostream& _stream) const
{
- return isFactorRegisteredDerived(fac_ptr);
+ _stream <<"\nSolverManager:"
+ <<"\nTotal values:"
+ << "\n\tSolving state: " << 1e-6*acc_duration_total_.count() << " s - executions: " << n_solve_
+ << "\n\t\tUpdate problem: " << 1e-6*acc_duration_update_.count() << " s" << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"
+ << "\n\t\tSolver: " << 1e-6*acc_duration_solver_.count() << " s" << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"
+ << "\n\t\tUpdate state: " << 1e-6*acc_duration_state_.count() << " s" << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"
+ << "\n\tSolving covariance: " << 1e-6*acc_duration_cov_.count() << " s - executions: " << n_cov_
+ <<"\nAverage:"
+ << "\n\tSolving state: " << 1e-3*acc_duration_total_.count() / n_solve_ << " ms"
+ << "\n\t\tUpdate problem: " << 1e-3*acc_duration_update_.count() / n_solve_ << " ms"
+ << "\n\t\tSolver: " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms"
+ << "\n\t\tUpdate state: " << 1e-3*acc_duration_state_.count() / n_solve_ << " ms"
+ << "\n\tSolving covariance: " << 1e-3*acc_duration_cov_.count() / n_cov_ << " ms"
+ <<"\nMax values:"
+ << "\n\tSolving state: " << 1e-3*max_duration_total_.count() << " ms"
+ << "\n\t\tUpdate problem: " << 1e-3*max_duration_update_.count() << " ms"
+ << "\n\t\tSolver: " << 1e-3*max_duration_solver_.count() << " ms"
+ << "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms"
+ << "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl;
+
}
} // namespace wolf
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 05d89142a35cc740c69c29712565d132d69fbe93..f94353d93dff9a0425e984372dde89b0116c8b4d 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -1,4 +1,4 @@
-#include "core/ceres_wrapper/ceres_manager.h"
+#include "../../include/core/ceres_wrapper/solver_ceres.h"
SolverManager::SolverManager()
{
@@ -61,7 +61,7 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
void SolverManager::addFactor(FactorBasePtr _corr_ptr)
{
- //TODO MatrixXs J; Vector e;
+ //TODO MatrixXd J; Vector e;
// getResidualsAndJacobian(_corr_ptr, J, e);
// solverQR->addFactor(_corr_ptr, J, e);
@@ -94,10 +94,10 @@ void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
- case ST_POINT_1D:
+ 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);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1d*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_VECTOR:
@@ -106,7 +106,7 @@ void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
- case ST_POINT_3D:
+ 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);
@@ -152,10 +152,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
switch (_corrPtr->getFactorType())
{
- case FAC_GPS_FIX_2D:
+ case FAC_GPS_FIX_2d:
{
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorGPS2D,
+ FactorGPS2d* specific_ptr = (FactorGPS2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorGPS2d,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -169,10 +169,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case FAC_ODOM_2D_COMPLEX_ANGLE:
+ case FAC_ODOM_2d_COMPLEX_ANGLE:
{
- FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
+ FactorOdom2dComplexAngle* specific_ptr = (FactorOdom2dComplexAngle*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2dComplexAngle,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -186,10 +186,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case FAC_ODOM_2D:
+ case FAC_ODOM_2d:
{
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2D,
+ FactorOdom2d* specific_ptr = (FactorOdom2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2d,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -203,10 +203,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case FAC_CORNER_2D:
+ case FAC_CORNER_2d:
{
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorCorner2D,
+ FactorCorner2d* specific_ptr = (FactorCorner2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorCorner2d,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce743db05620f2e5bea90fd6cb8512c9669d394f
--- /dev/null
+++ b/src/state_block/has_state_blocks.cpp
@@ -0,0 +1,92 @@
+
+#include "core/state_block/has_state_blocks.h"
+
+namespace wolf
+{
+
+StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ state_block_map_.emplace(_sb_type, _sb);
+ if (!isInStructure(_sb_type))
+ appendToStructure(_sb_type);
+
+ // conditionally register to problem
+ if(_problem)
+ _problem->notifyStateBlock(_sb, ADD);
+
+ return _sb;
+}
+
+void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) const
+{
+ if (_problem != nullptr)
+ {
+ for (auto pair_key_sbp : getStateBlockMap())
+ if (pair_key_sbp.second != nullptr)
+ _problem->notifyStateBlock(pair_key_sbp.second, ADD);
+ }
+}
+
+void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
+{
+ for (const char key : getStructure()) // note: key is a char
+ {
+ auto sbp = getStateBlock(key);
+ if (sbp != nullptr)
+ {
+ if (_problem != nullptr)
+ {
+ _problem->notifyStateBlock(sbp,REMOVE);
+ }
+ removeStateBlock(key);
+ }
+ }
+}
+
+void HasStateBlocks::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
+
+void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ if (_metric && _state_blocks)
+ {
+ for (const auto &key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ if (sb)
+ _stream << _tabs << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << " @ " << sb << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est")
+ << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )"
+ << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (const auto &key : getStructure())
+ {
+ const auto &sb = getStateBlock(key);
+ if (sb)
+ _stream << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] @ " << sb;
+ }
+ _stream << std::endl;
+ }
+}
+
+}
diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
index f46a93f9c488d0b899e94df156f3faff04a63554..aee3f704126bf4ba81e9658b3c337ded088c78a5 100644
--- a/src/state_block/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.cpp
@@ -22,3 +22,14 @@ unsigned int LocalParametrizationBase::getGlobalSize() const
}
} // namespace wolf
+
+bool wolf::LocalParametrizationBase::plus(const Eigen::VectorXd &_x,
+ const Eigen::VectorXd &_delta,
+ Eigen::VectorXd &_x_plus_delta) const
+{
+ Eigen::Map<const Eigen::VectorXd> x(_x.data(), _x.size());
+ Eigen::Map<const Eigen::VectorXd> delta(_delta.data(), _delta.size());
+ Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size());
+
+ return plus(x, delta, x_plus_delta);
+}
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index 9fdc26e1c0a799d7e669c54755f1ed55b834268e..846b14a49934c70c0d3879efd03f454ffab74605 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -22,9 +22,9 @@ 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
+bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _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.");
@@ -32,18 +32,18 @@ bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXs>& _h
using namespace Eigen;
- _h_plus_delta = ( exp_q(_delta) * Quaternions(_h.data()) ).coeffs();
+ _h_plus_delta = ( exp_q(_delta) * Quaterniond(_h.data()) ).coeffs();
return true;
}
-bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixRowXd>& _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;
+ Eigen::Vector4d hh = _h/2;
_jacobian << hh(3), hh(2), -hh(1),
-hh(2), hh(3), hh(0),
hh(1), -hh(0), hh(3),
@@ -51,14 +51,18 @@ bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::Ve
return true;
}
-bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXs>& _h1,
- Eigen::Map<const Eigen::VectorXs>& _h2,
- Eigen::Map<Eigen::VectorXs>& _h2_minus_h1)
+bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _h1,
+ Eigen::Map<const Eigen::VectorXd>& _h2,
+ Eigen::Map<Eigen::VectorXd>& _h2_minus_h1)
{
- using Eigen::Quaternions;
- _h2_minus_h1 = log_q(Quaternions(_h2.data()) * Quaternions(_h1.data()).conjugate());
+ using Eigen::Quaterniond;
+ _h2_minus_h1 = log_q(Quaterniond(_h2.data()) * Quaterniond(_h1.data()).conjugate());
return true;
}
+bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, double tolerance)
+{
+ return _state.size() == global_size_;
+}
} // namespace wolf
diff --git a/src/state_block/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp
index d8bcd6a0d1f9ffa02d77d754d389a4a711596137..727fa0de8f737dee986eb69e99581ad49b343355 100644
--- a/src/state_block/local_parametrization_quaternion.cpp
+++ b/src/state_block/local_parametrization_quaternion.cpp
@@ -8,9 +8,9 @@ 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
+bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -21,19 +21,19 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::Vect
using namespace Eigen;
- _q_plus_delta_theta = ( Quaternions(_q.data()) * exp_q(_delta_theta) ).coeffs();
+ _q_plus_delta_theta = ( Quaterniond(_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
+bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<Eigen::MatrixRowXd>& _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;
+ Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), -qq(2), qq(1),
qq(2), qq(3), -qq(0),
-qq(1), qq(0), qq(3),
@@ -43,16 +43,16 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const
}
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)
+bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
+ Eigen::Map<const Eigen::VectorXd>& _q2,
+ Eigen::Map<Eigen::VectorXd>& _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()));
+ using Eigen::Quaterniond;
+ _q2_minus_q1 = log_q(Quaterniond(_q1.data()).conjugate() * Quaterniond(_q2.data()));
return true;
}
@@ -60,9 +60,9 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec
////////// 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
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -73,19 +73,19 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::Vec
using namespace Eigen;
- _q_plus_delta_theta = ( exp_q(_delta_theta) * Quaternions(_q.data()) ).coeffs();
+ _q_plus_delta_theta = ( exp_q(_delta_theta) * Quaterniond(_q.data()) ).coeffs();
return true;
}
template <>
-bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<Eigen::MatrixRowXd>& _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;
+ Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), qq(2), -qq(1),
-qq(2), qq(3), qq(0),
qq(1), -qq(0), qq(3),
@@ -95,16 +95,16 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const
}
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)
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
+ Eigen::Map<const Eigen::VectorXd>& _q2,
+ Eigen::Map<Eigen::VectorXd>& _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());
+ using Eigen::Quaterniond;
+ _q2_minus_q1 = log_q(Quaterniond(_q2.data()) * Quaterniond(_q1.data()).conjugate());
return true;
}
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index e37f70ff44a22de9873ab1dad510a4ba1b1232d9..0280e2657a3e5bb0bd0dcc184b6800844ee68ef8 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -2,7 +2,7 @@
namespace wolf
{
-void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify)
+void StateBlock::setState(const Eigen::VectorXd& _state, const bool _notify)
{
assert(_state.size() == state_.size());
{
@@ -26,14 +26,52 @@ void StateBlock::setFixed(bool _fixed)
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());
-//}
+void StateBlock::perturb(double amplitude)
+{
+ using namespace Eigen;
+ VectorXd perturbation(VectorXd::Random(getLocalSize()) * amplitude);
+ this->plus(perturbation);
+}
+
+void StateBlock::plus(const Eigen::VectorXd &_dv)
+{
+ if (local_param_ptr_ == nullptr)
+ setState(getState() + _dv);
+ else
+ {
+ Eigen::VectorXd state(getSize());
+ local_param_ptr_->plus(getState(), _dv, state);
+ setState(state);
+ }
+}
+
+}
+
+#include "core/state_block/factory_state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_homogeneous_3d.h"
+
+namespace wolf{
+WOLF_REGISTER_STATEBLOCK(StateBlock);
+WOLF_REGISTER_STATEBLOCK(StateQuaternion);
+WOLF_REGISTER_STATEBLOCK(StateAngle);
+WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d);
+
+StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 1)
+ return StateAngle::create(_state, _fixed);
+ if (_state.size() == 4)
+ return StateQuaternion::create(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
+
+ return nullptr;
+}
+//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
+namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO = \
+ FactoryStateBlock::registerCreator("O", wolf::create_orientation); }
}
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04658a0fb6ccc9fd20412e650709d0d9320e169c
--- /dev/null
+++ b/src/state_block/state_composite.cpp
@@ -0,0 +1,975 @@
+
+
+
+#include "core/state_block/state_composite.h"
+#include "core/state_block/state_block.h"
+
+namespace wolf{
+
+////// VECTOR COMPOSITE //////////
+
+VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ auto size_it = _sizes.begin();
+ for ( const auto& key : _structure)
+ {
+ const auto& size = *size_it;
+
+ this->emplace(key, VectorXd(size).setZero());
+
+ size_it ++;
+ }
+}
+
+VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& key : _structure)
+ {
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _s)
+{
+ for (const auto& key : _s)
+ {
+ this->emplace(key,VectorXd(0));
+ }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors)
+{
+ assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch");
+
+ auto vector_it = _vectors.begin();
+
+ for (const auto& key : _structure)
+ {
+ this->emplace(key, *vector_it);
+ vector_it++;
+ }
+}
+
+
+Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
+{
+ // traverse once with unordered_map access
+ std::vector<const VectorXd*> vp;
+ unsigned int size = 0;
+ for (const auto& key : _structure)
+ {
+ vp.push_back(&(this->at(key)));
+ size += vp.back()->size();
+ }
+
+ Eigen::VectorXd x(size);
+
+ // traverse once linearly
+ unsigned int index = 0;
+ for (const auto& blkp : vp)
+ {
+ x.segment(index, blkp->size()) = *blkp;
+ index += blkp->size();
+ }
+ return x;
+}
+
+bool VectorComposite::includesStructure(const StateStructure &_structure) const
+{
+ for (auto key : _structure)
+ if (count(key) == 0)
+ return false;
+ return true;
+}
+
+std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
+{
+ for (const auto &pair_key_vec : _x)
+ {
+ const auto &key = pair_key_vec.first;
+ const auto &vec = pair_key_vec.second;
+ _os << key << ": (" << vec.transpose() << "); ";
+ }
+ return _os;
+}
+
+wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
+{
+ wolf::VectorComposite xpy;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+
+ xpy.emplace(i, _x.at(i) + _y.at(i));
+ }
+ return xpy;
+}
+
+VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y)
+{
+ VectorComposite xpy;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+
+ xpy.emplace(i, _x.at(i) - _y.at(i));
+ }
+ return xpy;
+}
+
+VectorComposite operator -(const wolf::VectorComposite &_x)
+{
+ wolf::VectorComposite m;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+ m.emplace(i, - _x.at(i));
+ }
+ return m;
+}
+
+void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& key : _structure)
+ {
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
+void VectorComposite::setZero()
+{
+ for (auto& pair_key_vec : *this)
+ pair_key_vec.second.setZero();
+}
+
+////// MATRIX COMPOSITE //////////
+
+bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk)
+{
+ // check rows
+ if (size_rows_.count(_row) == 0)
+ size_rows_[_row] = _mat_blk.rows();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // check cols
+ if (size_cols_.count(_col) == 0)
+ size_cols_[_col] = _mat_blk.cols();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // now it's safe to use [] operator. the first one is a function-like call to [] -- just weird for this->[]
+ this->operator[](_row)[_col] = _mat_blk;
+ return true;
+}
+
+bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const
+{
+ const auto &row_it = this->find(_row);
+ if(row_it != this->end())
+ return false;
+
+ const auto &col_it = row_it->second.find(_col);
+ if(col_it != row_it->second.end())
+ return false;
+
+ _mat_blk = col_it->second;
+
+ return true;
+}
+
+Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
+{
+ const auto &row_it = this->find(_row);
+ assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const
+{
+ const auto &row_it = this->find(_row);
+ assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) const
+{
+ MatrixComposite MN;
+ for (const auto &pair_i_Mi : (*this))
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto &pair_k_Nk : _N)
+ {
+ const auto &k = pair_k_Nk.first;
+ const auto &Nk = pair_k_Nk.second;
+
+ for (const auto &pair_j_Nkj : Nk)
+ {
+ const auto &j = pair_j_Nkj.first;
+ const auto &Nkj = pair_j_Nkj.second;
+ const auto &Mik = Mi.at(k);
+
+ if (MN.count(i, j) == 0)
+ MN.emplace(i, j, Mik * Nkj);
+ else
+ MN.at(i, j) += Mik * Nkj;
+ }
+ }
+ }
+ return MN;
+}
+
+wolf::MatrixComposite MatrixComposite::operator +(const MatrixComposite &_N) const
+{
+ MatrixComposite MpN;
+ for (const auto &pair_i_Mi : *this)
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto& pair_j_Mij : Mi)
+ {
+ const auto& j = pair_j_Mij.first;
+ const auto& Mij = pair_j_Mij.second;
+ const auto& Nij = _N.at(i,j);
+
+ MpN.emplace(i, j, Mij + Nij);
+ }
+ }
+ return MpN;
+}
+
+wolf::MatrixComposite MatrixComposite::operator -(const MatrixComposite &_N) const
+{
+ MatrixComposite MpN;
+ for (const auto &pair_i_Mi : *this)
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto& pair_j_Mij : Mi)
+ {
+ const auto& j = pair_j_Mij.first;
+ const auto& Mij = pair_j_Mij.second;
+ const auto& Nij = _N.at(i,j);
+
+ MpN.emplace(i, j, Mij - Nij);
+ }
+ }
+ return MpN;
+}
+
+MatrixComposite MatrixComposite::operator - (void) const
+{
+ MatrixComposite m;
+
+ for (const auto& pair_rkey_row : *this)
+ {
+ m.unordered_map<char,unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<char, MatrixXd>());
+ for (const auto& pair_ckey_blk : pair_rkey_row.second)
+ {
+ m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second);
+ }
+ }
+ return m;
+}
+
+
+wolf::VectorComposite MatrixComposite::operator *(const VectorComposite &_x) const
+{
+ VectorComposite y;
+ for (const auto &pair_rkey_row : *this)
+ {
+ const auto &rkey = pair_rkey_row.first;
+ const auto &row = pair_rkey_row.second;
+
+ for (const auto &pair_ckey_mat : row)
+ {
+ const auto &ckey = pair_ckey_mat.first;
+ const auto &J_r_c = pair_ckey_mat.second;
+
+ const auto& it = y.find(rkey);
+ if (it != y.end())
+ it->second += J_r_c * _x.at(ckey);
+ else
+ y.emplace(rkey, J_r_c * _x.at(ckey));
+ }
+ }
+ return y;
+}
+
+MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov)
+{
+
+
+ // simplify names of operands
+ const auto& J = *this;
+ const auto& Q = _Cov;
+
+ MatrixComposite S; // S = J * Q * J.tr
+
+
+ /* Covariance propagation
+ *
+ * 1. General formula for the output block S(i,j)
+ *
+ * S_ij = sum_n (J_in * sum_k(Q_nk * J_jk^T)) (A)
+ *
+ * which develops as
+ *
+ * S_ij = sum_n (J_in * QJt_nj)
+ *
+ * with:
+ *
+ * QJt_nj = sum_k (Q_nk * J_jk^T) (B)
+ *
+ *
+ * 2. Algorithm to accomplish the above:
+ *
+ * for i = 1 : S.rows = J.rows (1) // iterate for i and j
+ * {
+ * J_i = J[i]
+ * for j = i : S.cols = J.rows (2) // j starts at i: do not loop over the symmetrical blocks
+ * {
+ * S_ij = 0 (3) // start formula (A) for S_ij
+ * for n = 1 : Q.rows (4)
+ * {
+ * J_in = J[i][n]
+ * QJt_nj = 0 (5) // start formula (B) for QJt_nj
+ * for k = 1 : Q.cols (6)
+ * {
+ * J_jk = J[j][k]
+ * QJt_nj += Q_nk * J_jk^T (7) // sum on QJt_nj
+ * }
+ * S_ij += J_in * QJt_nj (8) // sum on S_ij
+ * }
+ * S[i][j] = S_ij // write block in output composite
+ * if (i != j)
+ * S[j][i] = S_ij^T (9) // write symmetrical block in output composite
+ * }
+ * }
+ */
+
+ // Iterate on the output matrix S first, row i, col j.
+ for (const auto& pair_i_Si : J) // (1)
+ {
+ const auto& i = pair_i_Si.first;
+ const auto& J_i = pair_i_Si.second;
+
+ double i_size = J_i.begin()->second.rows();
+
+ for (const auto& pair_j_Sj : J) // (2)
+ {
+ const auto& j = pair_j_Sj.first;
+ const auto& J_j = pair_j_Sj.second;
+
+ double j_size = J_j.begin()->second.rows();
+
+ MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3)
+
+ for (const auto& pair_n_Qn : Q) // (4)
+ {
+ const auto& n = pair_n_Qn.first;
+ const auto& Q_n = pair_n_Qn.second;
+
+ double n_size = Q_n.begin()->second.rows();
+
+ const auto& J_in = J_i.at(n);
+
+ MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5)
+
+ for (const auto& pair_k_Qnk : Q_n) // (6)
+ {
+ const auto& k = pair_k_Qnk.first;
+ const auto& Q_nk = pair_k_Qnk.second;
+
+ const auto& J_jk = J_j.at(k);
+
+ QJt_nj += Q_nk * J_jk.transpose(); // (7)
+ }
+
+ S_ij += J_in * QJt_nj; // (8)
+ }
+
+ S.emplace(i,j,S_ij);
+ if (j == i)
+ break; // avoid computing the symmetrical block!
+ else
+ S.emplace(j, i, S_ij.transpose()); // (9)
+ }
+ }
+
+ return S;
+}
+
+MatrixComposite MatrixComposite::operator * (double scalar_right) const
+{
+ MatrixComposite S(*this);
+ for (const auto& pair_rkey_row : *this)
+ {
+ const auto& rkey = pair_rkey_row.first;
+ for (const auto& pair_ckey_block : pair_rkey_row.second)
+ {
+ const auto ckey = pair_ckey_block.first;
+ S[rkey][ckey] *= scalar_right;
+ }
+ }
+ return S;
+}
+
+MatrixComposite operator * (double scalar_left, const MatrixComposite& M)
+{
+ MatrixComposite S;
+ S = M * scalar_left;
+ return S;
+}
+
+MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const
+{
+
+ std::unordered_map < char, unsigned int> indices_rows;
+ std::unordered_map < char, unsigned int> indices_cols;
+ unsigned int rows, cols;
+
+ sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols);
+
+ MatrixXd M ( MatrixXd::Zero(rows, cols) );
+
+ for (const auto& pair_row_rband : (*this))
+ {
+ const auto& row = pair_row_rband.first;
+ const auto& rband = pair_row_rband.second;
+ for (const auto& pair_col_blk : rband)
+ {
+ const auto& col = pair_col_blk.first;
+ const auto& blk = pair_col_blk.second;
+
+ const unsigned int & blk_rows = size_rows_.at(row);
+ const unsigned int & blk_cols = size_cols_.at(col);
+
+ assert(blk.rows() == blk_rows && "MatrixComposite block size mismatch! Wrong number of rows.");
+ assert(blk.cols() == blk_cols && "MatrixComposite block size mismatch! Wrong number of cols.");
+
+ M.block(indices_rows.at(row), indices_cols.at(col), blk_rows, blk_cols) = blk;
+
+ }
+ }
+
+ return M;
+}
+
+unsigned int MatrixComposite::rows() const
+{
+ unsigned int rows = 0;
+ for (const auto& pair_row_size : this->size_rows_)
+ rows += pair_row_size.second;
+ return rows;
+}
+
+unsigned int MatrixComposite::cols() const
+{
+ unsigned int cols = 0;
+ for (const auto& pair_col_size : this->size_rows_)
+ cols += pair_col_size.second;
+ return cols;
+}
+
+void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
+ const StateStructure &_struct_cols,
+ std::unordered_map<char, unsigned int> &_indices_rows,
+ std::unordered_map<char, unsigned int> &_indices_cols,
+ unsigned int &_rows,
+ unsigned int &_cols) const
+{
+ _rows = 0;
+ _cols = 0;
+ for (const auto& row : _struct_rows)
+ {
+ _indices_rows[row] = _rows;
+ _rows += size_rows_.at(row);
+ }
+ for (const auto& col : _struct_cols)
+ {
+ _indices_cols[col] = _cols;
+ _cols += size_cols_.at(col);
+ }
+}
+
+MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure)
+{
+ for (const auto& rkey : _row_structure)
+ for (const auto& ckey : _col_structure)
+ this->emplace(rkey, ckey, MatrixXd(0,0));
+}
+
+MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes)
+{
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey : _row_structure)
+ {
+ auto csize_it = _col_sizes.begin();
+ for (const auto& ckey : _col_structure)
+ {
+ this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
+
+ csize_it ++;
+ }
+ rsize_it++;
+ }
+}
+
+MatrixComposite::MatrixComposite (const MatrixXd& _m,
+ const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes)
+{
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ SizeEigen rindex = 0, cindex;
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey : _row_structure)
+ {
+ assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows");
+
+ cindex = 0;
+ auto csize_it = _col_sizes.begin();
+
+ for (const auto& ckey : _col_structure)
+ {
+ assert(_m.cols() >= cindex + *csize_it && "Provided matrix has insufficient number of columns");
+
+ this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it));
+
+ cindex += *csize_it;
+ csize_it ++;
+ }
+
+ assert(_m.cols() == cindex && "Provided matrix has too many columns");
+
+ rindex += *rsize_it;
+ rsize_it++;
+ }
+
+ assert(_m.rows() == rindex && "Provided matrix has too many rows");
+}
+
+MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure, const std::list<int>& _col_sizes)
+{
+ MatrixComposite m;
+
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey : _row_structure)
+ {
+ auto csize_it = _col_sizes.begin();
+ for (const auto& ckey : _col_structure)
+ {
+ m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it));
+
+ csize_it ++;
+ }
+ rsize_it++;
+ }
+ return m;
+}
+
+MatrixComposite MatrixComposite::identity (const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ MatrixComposite m;
+
+ assert (_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!");
+
+ auto rsize_it = _sizes.begin();
+ auto rkey_it = _structure.begin();
+
+ while (rkey_it != _structure.end())
+ {
+
+ const auto& rkey = *rkey_it;
+ const auto rsize = *rsize_it;
+
+ m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block
+
+ auto csize_it = rsize_it;
+ auto ckey_it = rkey_it;
+
+ csize_it ++;
+ ckey_it++;
+
+ while (ckey_it != _structure.end())
+ {
+ const auto& ckey = *ckey_it;
+ const auto& csize = *csize_it;
+
+ m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
+ m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block
+
+ csize_it ++;
+ ckey_it++;
+ }
+
+ rsize_it ++;
+ rkey_it++;
+ }
+ return m;
+}
+
+bool MatrixComposite::check ( ) const
+{
+ bool size_ok = true;
+
+ // first see matrix itself, check that sizes are OK
+ for (const auto& pair_rkey_row : *this)
+ {
+ const auto& rkey = pair_rkey_row.first;
+ const auto& row = pair_rkey_row.second;
+ for (const auto& pair_ckey_blk : row)
+ {
+ const auto& ckey = pair_ckey_blk.first;
+ const auto& blk = pair_ckey_blk.second;
+
+ if (size_rows_.count(rkey) != 0)
+ {
+ if( size_rows_.at(rkey) != blk.rows())
+ {
+ WOLF_ERROR("row size for row ", rkey, " has wrong size");
+ size_ok = false;
+ }
+ }
+ else
+ {
+ WOLF_ERROR("row size for row ", rkey, " does not exist");
+ size_ok = false;
+ }
+ if (size_cols_.count(ckey) != 0)
+ {
+ if( size_cols_.at(ckey) != blk.cols())
+ {
+ WOLF_ERROR("col size for col ", rkey, " has wrong size");
+ size_ok = false;
+ }
+ }
+ else
+ {
+ WOLF_ERROR("col size for col ", ckey, " does not exist");
+ size_ok = false;
+ }
+ }
+ }
+ return size_ok;
+}
+
+std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M)
+{
+ for (const auto &pair_rkey_row : _M)
+ {
+ const auto rkey = pair_rkey_row.first;
+
+ for (const auto &pair_ckey_mat : pair_rkey_row.second)
+ {
+ const auto &ckey = pair_ckey_mat.first;
+
+ _os << "\n[" << rkey << "," << ckey << "]: \n" << pair_ckey_mat.second;
+ }
+ }
+ return _os;
+}
+
+
+
+////// STATE BLOCK COMPOSITE //////////
+
+const StateBlockMap& StateBlockComposite::getStateBlockMap() const
+{
+ return state_block_map_;
+}
+
+void StateBlockComposite::fix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->fix();
+}
+
+void StateBlockComposite::unfix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->unfix();
+}
+
+bool StateBlockComposite::isFixed() const
+{
+ bool fixed = true;
+ for (auto pair_key_sbp : state_block_map_)
+ {
+ if (pair_key_sbp.second)
+ fixed &= pair_key_sbp.second->isFixed();
+ }
+ return fixed;
+}
+
+unsigned int StateBlockComposite::remove(const char &_sb_type)
+{
+ return state_block_map_.erase(_sb_type);
+}
+
+bool StateBlockComposite::has(const StateBlockPtr &_sb) const
+{
+ bool found = false;
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ found = found || (pair_key_sb.second == _sb);
+ }
+ return found;
+}
+
+StateBlockPtr StateBlockComposite::at(const char &_sb_type) const
+{
+ auto it = state_block_map_.find(_sb_type);
+ if (it != state_block_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+void StateBlockComposite::set(const char& _sb_type, const StateBlockPtr &_sb)
+{
+ auto it = state_block_map_.find(_sb_type);
+ assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
+
+ it->second = _sb;
+}
+
+void StateBlockComposite::set(const char& _key, const VectorXd &_vec)
+{
+ auto it = state_block_map_.find(_key);
+ assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
+
+ assert ( it->second->getSize() == _vec.size() && "Provided vector size mismatch with associated state block");
+
+ it->second->setState(_vec);
+}
+
+void StateBlockComposite::setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors)
+{
+ assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match");
+
+ auto vec_it = _vectors.begin();
+ for (const auto key : _structure)
+ {
+ set (key, *vec_it);
+ vec_it++;
+ }
+}
+
+bool StateBlockComposite::key(const StateBlockPtr &_sb, char &_key) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key ='0';
+ return false;
+ }
+}
+
+char StateBlockComposite::key(const StateBlockPtr& _sb) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ return it->first;
+ else
+ return '0';
+}
+
+
+StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
+{
+ const auto& it = std::find_if(state_block_map_.begin(),
+ state_block_map_.end(),
+ [_sb](const std::pair<char, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+StateBlockPtr StateBlockComposite::add(const char &_sb_type, const StateBlockPtr &_sb)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+ state_block_map_.emplace(_sb_type, _sb);
+
+ return _sb;
+}
+
+void StateBlockComposite::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
+
+void StateBlockComposite::plus(const VectorComposite &_dx)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
+ const auto& dv = _dx.at(key);
+
+ sb->plus(dv);
+ }
+}
+
+VectorComposite StateBlockComposite::getState() const
+{
+ VectorComposite v;
+ getState(v);
+ return v;
+}
+
+bool StateBlockComposite::getState(VectorComposite &_state) const
+{
+ for (auto &pair_key_sb : state_block_map_)
+ {
+ _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+ }
+ return true;
+}
+
+void StateBlockComposite::setState(const VectorComposite &_state)
+{
+ for (const auto &pair_key_sb : _state)
+ {
+ state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
+ }
+}
+
+void StateBlockComposite::setIdentity(bool _notify)
+{
+ for (const auto& pair_key_sb : getStateBlockMap())
+ {
+ pair_key_sb.second->setIdentity(_notify);
+ }
+}
+
+void StateBlockComposite::setZero(bool _notify)
+{
+ setIdentity(_notify);
+}
+
+VectorComposite StateBlockComposite::identity() const
+{
+ VectorComposite x;
+ for (const auto& pair_key_sb : getStateBlockMap())
+ x.emplace(pair_key_sb.first, pair_key_sb.second->identity());
+ return x;
+}
+
+VectorComposite StateBlockComposite::zero() const
+{
+ return identity();
+}
+
+
+SizeEigen StateBlockComposite::stateSize() const
+{
+ SizeEigen size = 0;
+ for (const auto& pair_key_sb : state_block_map_)
+ size += pair_key_sb.second->getSize();
+ return size;
+}
+
+SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const
+{
+ SizeEigen size = 0;
+ for (const auto& key : _structure)
+ {
+ size += state_block_map_.at(key)->getSize();
+ }
+ return size;
+}
+
+VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) const
+{
+ VectorXd x(stateSize(_structure));
+ SizeEigen index = 0;
+ SizeEigen size;
+ for (const auto& key : _structure)
+ {
+ const auto& sb = state_block_map_.at(key);
+ size = sb->getSize();
+ x.segment(index,size) = sb->getState();
+ index += size;
+ }
+ return x;
+}
+
+void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd &_vector) const
+{
+ _vector.resize(stateSize(_structure));
+ SizeEigen index = 0;
+ SizeEigen size;
+ for (const auto& key : _structure)
+ {
+ const auto& sb = state_block_map_.at(key);
+ size = sb->getSize();
+ _vector.segment(index,size) = sb->getState();
+ index += size;
+ }
+}
+
+} // namespace wolf
+
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index a2395ffae0c1512ca4157fc335bd02ed62855d6a..4a64613a3445bfead7565f38ae5527f01f2fde5c 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -3,13 +3,10 @@
namespace wolf {
-TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
- NodeBase("TRAJECTORY", "Base"),
- frame_structure_(_frame_structure),
- last_key_frame_ptr_(nullptr),
- last_key_or_aux_frame_ptr_(nullptr)
+TrajectoryBase::TrajectoryBase() :
+ NodeBase("TRAJECTORY", "TrajectoryBase")
{
-// WOLF_DEBUG("constructed T");
+ frame_map_ = FrameMap();
}
TrajectoryBase::~TrajectoryBase()
@@ -20,110 +17,106 @@ TrajectoryBase::~TrajectoryBase()
FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
// add to list
- frame_list_.push_back(_frame_ptr);
+ assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
- if (_frame_ptr->isKeyOrAux())
- {
- // sort
- sortFrame(_frame_ptr);
-
- // update last_estimated and last_key
- updateLastFrames();
- }
+ frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
return _frame_ptr;
}
-void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
+void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
- for(auto fr_ptr : getFrameList())
- fr_ptr->getFactorList(_fac_list);
+ // add to list
+ // frame_map_.erase(_frame_ptr);
+ frame_map_.erase(_frame_ptr->getTimeStamp());
}
-void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
+void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
{
- moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
+ for(auto fr_ptr : *this)
+ fr_ptr->getFactorList(_fac_list);
}
-void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
+
+FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
{
- if (*_place != _frm_ptr)
+ FrameBasePtr closest_kf = nullptr;
+ //If frame_map_ is empty then closestFrameToTimeStamp is meaningless
+ if(not frame_map_.empty())
{
- frame_list_.remove(_frm_ptr);
- frame_list_.insert(_place, _frm_ptr);
- updateLastFrames();
+ //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
+ //In the std they fulfill the following property:
+ // lb is the first element such that ts <= lb, alternatively the smallest element that is NOT less than ts.
+ // lb definition is NOT the ACTUAL lower bound but the following position so, lb = lb_true + 1.
+
+ auto lower_bound = frame_map_.lower_bound(_ts);
+ if((lower_bound != this->end() and lower_bound->first == _ts) or lower_bound == this->begin() ) closest_kf = lower_bound->second;
+ else
+ {
+ auto upper_bound = lower_bound;
+ //I find it easier to reason if lb < ts < ub. Remember that we have got rid of the
+ //equality case and the out of bounds cases so this inequality is complete (it is not misssing cases).
+ //Therefore, we need to decrease the lower_bound to the previous element
+ lower_bound = std::prev(lower_bound);
+
+ //If ub points to end() it means that the last frame is still less than _ts, therefore certainly
+ //it will be the closest one
+ if(upper_bound == this->end()) closest_kf = lower_bound->second;
+ else
+ {
+ //Easy stuff just calculate the distance return minimum
+ auto lb_diff = fabs(lower_bound->first - _ts);
+ auto ub_diff = fabs(upper_bound->first - _ts);
+ if(lb_diff < ub_diff)
+ {
+ closest_kf = lower_bound->second;
+ }
+ else
+ {
+ closest_kf = upper_bound->second;
+ }
+ }
+ }
}
+ return closest_kf;
}
-FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
+void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
{
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
- if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
- return frm_rit.base();
- return getFrameList().begin();
+ _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl;
}
-
-void TrajectoryBase::updateLastFrames()
+void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- bool last_estimated_set = false;
-
- // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
- if ((*frm_rit)->isKeyOrAux())
- {
- if (!last_estimated_set)
- {
- last_key_or_aux_frame_ptr_ = (*frm_rit);
- last_estimated_set = true;
- }
- if ((*frm_rit)->isKey())
- {
- last_key_frame_ptr_ = (*frm_rit);
- break;
- }
- }
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 1)
+ for (auto F : *this)
+ if (F)
+ F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
+CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
{
- // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
- FrameBasePtr closest_kf = nullptr;
- Scalar min_dt = 1e9;
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
- for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.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;
-}
+ if (_verbose)
+ {
+ _stream << _tabs << "Trj @ " << _trj_ptr.get() << std::endl;
+ }
-FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
+ // check pointer to Problem
+ inconsistency_explanation << "Trj->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getTrajectory() [" << getProblem()->getTrajectory().get() << "] -> Trj [" << _trj_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
- FrameBasePtr closest_kf = nullptr;
- Scalar min_dt = 1e9;
-
- for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
- if ((*frm_rit)->isKeyOrAux())
- {
- Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
- if (dt < min_dt)
- {
- min_dt = dt;
- closest_kf = *frm_rit;
- }
- else
- break;
- }
- return closest_kf;
+ auto trj_ptr = std::static_pointer_cast<TrajectoryBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto F : *this)
+ F->check(_log, F, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
}
-
} // namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7d10c3167b5fe23aa279808e743bc4f8b4abb3da
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -0,0 +1,49 @@
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
+{
+ int n_f = getProblem()->getTrajectory()->getFrameMap().size();
+ bool remove_first = (n_f > params_sw_->n_frames);
+ int n_fix = (n_f >= params_sw_->n_frames ?
+ params_sw_->n_fix_first_frames :
+ n_f - (params_sw_->n_frames - params_sw_->n_fix_first_frames));
+
+ auto frame = (remove_first ?
+ getProblem()->getTrajectory()->getFirstFrame()->getNextFrame() :
+ getProblem()->getTrajectory()->getFirstFrame());
+ int fixed_frames = 0;
+
+ // Fix n_fix first frames
+ while (fixed_frames < n_fix)
+ {
+ if (not frame)
+ break;
+ if (not frame->isFixed())
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow fixing frame ", frame->id());
+ frame->fix();
+ }
+ frame = frame->getNextFrame();
+ fixed_frames++;
+ }
+
+ // Remove first frame
+ if (remove_first)
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
+ getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_empty_parent);
+ }
+}
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindow);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindow);
+} // namespace wolf
+
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..35c20da4585856df0e18739dce5553c74efb4807
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -0,0 +1,80 @@
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/capture/capture_motion.h"
+#include "core/processor/processor_motion.h"
+
+namespace wolf
+{
+
+TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) :
+ TreeManagerSlidingWindow(_params),
+ params_swdr_(_params),
+ count_frames_(0)
+{
+ NodeBase::setType("TreeManagerSlidingWindowDualRate");
+}
+
+void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
+{
+ int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+
+ // recent segment not complete
+ if (n_f <= params_swdr_->n_frames_recent)
+ return;
+
+ // REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames
+ if (count_frames_ != 0)
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
+ FrameBasePtr remove_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
+ params_swdr_->n_frames_recent)->second;
+ FrameBasePtr keep_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
+ params_swdr_->n_frames_recent - 1)->second;
+
+ // compose motion captures for all processors motion
+ for (auto is_motion_pair : getProblem()->getProcessorIsMotionMap())
+ {
+ auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion_pair.second);
+ if (proc_motion == nullptr)
+ {
+ // FIXME: IsMotion::mergeCaptures pure virtual in IsMotion without need of casting
+ WOLF_INFO("TreeManagerSlidingWindowDualRate::keyFrameCallback: IsMotion ",
+ std::dynamic_pointer_cast<ProcessorBase>(is_motion_pair.second)->getName(),
+ " couldn't be casted to ProcessorMotion. Not merging its captures...");
+ continue;
+ }
+
+ auto cap_prev = std::dynamic_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor()));
+ auto cap_next = std::dynamic_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor()));
+
+ // merge captures (if exist)
+ if (cap_prev and cap_next)
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow merging capture ", cap_prev->id(), " with ", cap_next->id());
+ assert(cap_next->getOriginCapture() == cap_prev);
+ proc_motion->mergeCaptures(cap_prev, cap_next);
+ }
+ }
+
+ // remove frame
+ remove_recent_frame->remove(params_swdr_->viral_remove_empty_parent);
+ }
+ // Call tree manager sliding window
+ // It will remove oldest frame if tfirst recent frame has been kept
+ TreeManagerSlidingWindow::keyFrameCallback(_frame);
+
+ // iterate counter
+ count_frames_++;
+ if (count_frames_ == params_swdr_->rate_old_frames)
+ count_frames_ = 0;
+}
+
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowDualRate);
+} // namespace wolf
+
diff --git a/src/utils/check_log.cpp b/src/utils/check_log.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06e58733aa3d99b9cffa312a9a88bdb223d15eb5
--- /dev/null
+++ b/src/utils/check_log.cpp
@@ -0,0 +1,29 @@
+#include "core/utils/check_log.h"
+
+using namespace wolf;
+
+CheckLog::CheckLog()
+{
+ is_consistent_ = true;
+ explanation_ = "";
+}
+CheckLog::CheckLog(bool _consistent, std::string _explanation)
+{
+ is_consistent_ = _consistent;
+ if (not _consistent)
+ explanation_ = _explanation;
+ else
+ explanation_ = "";
+}
+void CheckLog::compose(CheckLog l)
+{
+ is_consistent_ = is_consistent_ and l.is_consistent_;
+ explanation_ = explanation_ + l.explanation_;
+}
+void CheckLog::assertTrue(bool _condition, std::stringstream& _stream)
+{
+ auto cl = CheckLog(_condition, _stream.str());
+ this->compose(cl);
+ // Clear inconsistency_explanation
+ std::stringstream().swap(_stream);
+}
diff --git a/src/utils/converter_utils.cpp b/src/utils/converter_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f445c572dd03b2fa10aae48bb34f4a80798ed41
--- /dev/null
+++ b/src/utils/converter_utils.cpp
@@ -0,0 +1,132 @@
+#include "core/utils/converter_utils.h"
+
+// Eigen
+#include <array>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+
+// STD
+#include <iostream>
+#include <list>
+#include <stack>
+#include <vector>
+
+namespace utils {
+std::vector<std::string> splitter(std::string val) {
+ std::vector<std::string> cont = std::vector<std::string>();
+ std::stringstream ss(val);
+ std::string token;
+ while (std::getline(ss, token, ',')) {
+ cont.push_back(token);
+ }
+ return cont;
+}
+std::vector<std::string> getMatches(std::string val, std::regex exp) {
+ std::smatch res;
+ auto v = std::vector<std::string>();
+ std::string::const_iterator searchStart(val.cbegin());
+ while (std::regex_search(searchStart, val.cend(), res, exp)) {
+ v.push_back(res[0]);
+ searchStart = res.suffix().first;
+ }
+ return v;
+}
+std::array<std::string, 2> splitMatrixStringRepresentation(std::string matrix) {
+ std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:-?[0-9]*(?:\\.[0-9]+)?,?)+)\\]");
+ std::regex rgxStatic("\\[((?:(?:-?[0-9]*)(?:\\.[0-9]+)?,?)+)\\]");
+ std::smatch matches;
+ std::smatch matchesStatic;
+ std::array<std::string, 2> values = {{"[]", "[]"}};
+ if (std::regex_match(matrix, matches, rgx)) {
+ values[0] = "[" + matches[1].str() + "]";
+ values[1] = "[" + matches[2].str() + "]";
+ } else if (std::regex_match(matrix, matchesStatic, rgxStatic)) {
+ values[1] = "[" + matchesStatic[1].str() + "]";
+ } else {
+ throw std::runtime_error(
+ "Invalid string representation of a Matrix. Correct format is "
+ "[([num,num],)?(num(,num)*)?]. String provided: " +
+ matrix);
+ }
+ return values;
+}
+std::vector<std::string> pairSplitter(std::string val) {
+ std::regex exp("\\{[^\\{:]:.*?\\}");
+ return getMatches(val, exp);
+}
+std::string splitMapStringRepresentation(std::string str_map) {
+ std::smatch mmatches;
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ std::string result = "";
+ if (std::regex_match(str_map, mmatches, rgxM)) {
+ result = mmatches[1].str();
+ } else {
+ throw std::runtime_error(
+ "Invalid string representation of a Map. Correct format is "
+ "[({id:value})?(,{id:value})*]. String provided: " +
+ str_map);
+ }
+ return result;
+}
+std::vector<std::string> parseList(std::string val) {
+ std::stack<char> limiters;
+ std::stack<std::string> word_stack;
+ std::string current_word;
+ std::vector<std::string> words;
+ std::vector<char> chars(val.begin(), val.end());
+ for (const char ¤t : chars) {
+ if (current == '[') {
+ limiters.push(current);
+ word_stack.push(current_word);
+ current_word = "";
+ } else if (current == ']') {
+ if (limiters.empty())
+ throw std::runtime_error("Unmatched delimiter");
+ if (limiters.top() == '[') {
+ if (limiters.size() > 1) {
+ if (word_stack.empty())
+ word_stack.push("");
+ current_word = word_stack.top() + "[" + current_word + "]";
+ word_stack.pop();
+ } else if (limiters.size() == 1 and current_word != "")
+ words.push_back(current_word);
+ else
+ current_word += current;
+ limiters.pop();
+ } else
+ throw std::runtime_error("Unmatched delimiter");
+ } else if (current == '{') {
+ limiters.push(current);
+ word_stack.push(current_word);
+ current_word = "";
+ } else if (current == '}') {
+ if (limiters.top() == '{') {
+ if (limiters.size() > 1) {
+ if (word_stack.empty())
+ word_stack.push("");
+ current_word = word_stack.top() + "{" + current_word + "}";
+ word_stack.pop();
+ } else if (limiters.size() == 1)
+ words.push_back(current_word);
+ else
+ current_word += current;
+ limiters.pop();
+ } else
+ throw std::runtime_error("Unmatched delimiter");
+ } else if (current == ',') {
+ if (limiters.size() == 1 and current_word != "") {
+ words.push_back(current_word);
+ current_word = "";
+ } else if (limiters.size() > 1)
+ current_word += current;
+ } else {
+ if (limiters.empty())
+ throw std::runtime_error("Found non-delimited text");
+ current_word += current;
+ }
+ }
+ if (not limiters.empty())
+ throw std::runtime_error("Unclosed delimiter [] or {}");
+ return words;
+}
+} // namespace utils
\ No newline at end of file
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2876701b84d3acabf5e4e9c761a49570bffc2fe
--- /dev/null
+++ b/src/utils/graph_search.cpp
@@ -0,0 +1,119 @@
+#include "core/utils/graph_search.h"
+
+using namespace wolf;
+
+GraphSearch::GraphSearch() :
+ parents_()
+{
+
+}
+
+GraphSearch::~GraphSearch()
+{
+
+}
+
+FactorBasePtrList GraphSearch::computeShortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist)
+{
+ //WOLF_INFO("GraphSearch::computeShortestPath: from frame ", frm1->id(), " to frame ", frm2->id());
+
+ std::set<FrameBasePtr> frm_neigs({frm1});
+ parents_[frm1] = std::pair<FactorBasePtr,FrameBasePtr>(nullptr, nullptr);
+ unsigned int depth = 0;
+
+ //WOLF_INFO(frm1->id());
+
+ while (not frm_neigs.empty())
+ {
+ frm_neigs = getNeighborFrames(frm_neigs);
+ depth++;
+
+ //if (not frm_neigs.empty())
+ //{
+ // std::string frm_neigs_str(depth, '.');
+ // for (auto frm : frm_neigs)
+ // frm_neigs_str += std::to_string(frm->id()) + std::string(" ");
+ // WOLF_INFO(frm_neigs_str);
+ //}
+
+ // finish
+ if (frm_neigs.count(frm2) != 0)
+ {
+ //WOLF_INFO("Frame ", frm2->id(), " found!");
+
+ assert(parents_.count(frm2) != 0);
+ FactorBasePtrList factor_path;
+ auto frm_it = frm2;
+
+ while (frm_it != frm1)
+ {
+ factor_path.push_back(parents_.at(frm_it).first);
+ frm_it = parents_.at(frm_it).second;
+ }
+
+ return factor_path;
+ }
+
+ // stop
+ if (max_graph_dist > 0 and depth == max_graph_dist)
+ break;
+ }
+ //WOLF_INFO("Path to frame ", frm2->id(), " NOT found!");
+
+ return FactorBasePtrList();
+}
+
+std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePtr>& frms)
+{
+ std::set<FrameBasePtr> frm_neigs;
+
+ for (auto frm : frms)
+ {
+ // get constrained by factors
+ FactorBasePtrList facs_by = frm->getConstrainedByList();
+
+ // Iterate over all factors_by
+ for (auto && fac_by : facs_by)
+ {
+ //WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
+ //WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
+ if (fac_by and
+ fac_by->getFrame() and
+ parents_.count(fac_by->getFrame()) == 0)
+ {
+ //WOLF_INFO("registering");
+ frm_neigs.insert(fac_by->getFrame());
+ parents_[fac_by->getFrame()] = std::pair<FactorBasePtr,FrameBasePtr>(fac_by, frm);
+ }
+ }
+
+ // get the factors of this frame
+ FactorBasePtrList facs_own;
+ frm->getFactorList(facs_own);
+
+ // Iterate over all factors_own
+ for (auto && fac_own : facs_own)
+ {
+ //WOLF_INFO_COND(fac_own, "fac_own: ", fac_own->id());
+ //WOLF_INFO_COND(fac_own->getFrameOtherList().empty(), "fac_own->getFrameOtherList() is empty");
+ if (fac_own and not fac_own->getFrameOtherList().empty())
+ for (auto frm_other_w : fac_own->getFrameOtherList())
+ {
+ auto frm_other = frm_other_w.lock();
+ //WOLF_INFO_COND(frm_other, "frm_other ", frm_other->id());
+ if (frm_other and
+ parents_.count(frm_other) == 0)
+ {
+ //WOLF_INFO("registering");
+ frm_neigs.insert(frm_other);
+ parents_[frm_other] = std::pair<FactorBasePtr,FrameBasePtr>(fac_own, frm);
+ }
+ }
+ }
+ }
+
+ return frm_neigs;
+}
+
diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fdf6984b6a3de57fa8a8ae062232a685897209f
--- /dev/null
+++ b/src/utils/loader.cpp
@@ -0,0 +1,37 @@
+#include "core/utils/loader.h"
+
+#include <dlfcn.h>
+#include <stdexcept>
+
+Loader::Loader(std::string _file)
+{
+ path_ = _file;
+}
+LoaderRaw::LoaderRaw(std::string _file) : Loader(_file)
+{
+ //
+}
+LoaderRaw::~LoaderRaw()
+{
+ close();
+}
+void LoaderRaw::load()
+{
+ resource_ = dlopen(path_.c_str(), RTLD_LAZY);
+ if (not resource_)
+ throw std::runtime_error("Couldn't load resource with path " + path_ + "\n" + "Error info: " + dlerror());
+}
+void LoaderRaw::close()
+{
+ if (resource_)
+ dlclose(resource_);
+}
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// resource_ = new ClassLoader(path_);
+// }
+// void close(){
+// delete resource_;
+// }
+// };
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6344466792706c9e499695721f95e781089ec07e
--- /dev/null
+++ b/src/utils/params_server.cpp
@@ -0,0 +1,28 @@
+#include "core/utils/params_server.h"
+
+using namespace wolf;
+
+ParamsServer::ParamsServer()
+{
+ params_ = std::map<std::string, std::string>();
+}
+ParamsServer::ParamsServer(std::map<std::string, std::string> _params)
+{
+ params_ = _params;
+}
+
+void ParamsServer::print()
+{
+ for (auto it : params_)
+ std::cout << it.first << "~~" << it.second << std::endl;
+}
+
+void ParamsServer::addParam(std::string _key, std::string _value)
+{
+ params_.insert(std::pair<std::string, std::string>(_key, _value));
+}
+
+void ParamsServer::addParams(std::map<std::string, std::string> _params)
+{
+ params_.insert(_params.begin(), _params.end());
+}
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f07cc4d559c911e9cfc9e4ecb2797417c9b21201
--- /dev/null
+++ b/src/yaml/parser_yaml.cpp
@@ -0,0 +1,584 @@
+#include "core/yaml/parser_yaml.h"
+
+#include <string>
+#include <vector>
+#include <list>
+#include <stack>
+#include <regex>
+#include <map>
+#include <iostream>
+#include <algorithm>
+#include <numeric>
+
+using namespace wolf;
+namespace {
+ //====== START OF FORWARD DECLARATION ========
+ std::string parseAtomicNode(YAML::Node);
+ std::string fetchMapEntry(YAML::Node);
+ std::string mapToString(std::map<std::string,std::string>);
+ //====== END OF FORWARD DECLARATION ========
+
+/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
+ * @param n the node representing a map
+ * @return std::map<std::string, std::string> populated with the key,value pairs in n
+ */
+std::map<std::string, std::string> fetchAsMap(YAML::Node _n){
+ assert(_n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
+ auto m = std::map<std::string, std::string>();
+ for(const auto& kv : _n){
+ std::string key = kv.first.as<std::string>();
+ switch (kv.second.Type()) {
+ case YAML::NodeType::Scalar : {
+ std::string value = kv.second.Scalar();
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ case YAML::NodeType::Sequence : {
+ std::string aux = parseAtomicNode(kv.second);
+ m.insert(std::pair<std::string,std::string>(key, aux));
+ break;
+ }
+ case YAML::NodeType::Map : {
+ std::string value = fetchMapEntry(kv.second);
+ std::regex r("^\\$.*");
+ if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ default:
+ assert(1 == 0 && "Unsupported node Type at fetchAsMap");
+ break;
+ }
+ }
+ return m;
+}
+ std::string fetchMapEntry(YAML::Node _n){
+ switch (_n.Type()) {
+ case YAML::NodeType::Scalar: {
+ return _n.Scalar();
+ break;
+ }
+ case YAML::NodeType::Sequence: {
+ return parseAtomicNode(_n);
+ break;
+ }
+ case YAML::NodeType::Map: {
+ return mapToString(fetchAsMap(_n));
+ break;
+ }
+ default: {
+ assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
+ return "";
+ break;
+ }
+ }
+ }
+ /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
+ * @param map_ just a std::map<std::string,std::string>
+ * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
+ */
+ std::string mapToString(std::map<std::string,std::string> _map){
+ std::string result = "";
+ auto v = std::vector<std::string>();
+ std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
+ auto concat = [](std::string ac, std::string str)-> std::string {
+ return ac + str + ",";
+ };
+ std::string aux = "";
+ std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
+ if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
+ else accumulated = "";
+ return "[" + accumulated + "]";
+ }
+ /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
+ * @param n a vector of YAML::Node that represents a YAML::Sequence
+ * @return <b>{std::string}</b> representing the YAML sequence
+ */
+ std::string parseAtomicNode(YAML::Node _n){
+ std::string aux = "";
+ bool first = true;
+ std::string separator = "";
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return _n.Scalar();
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n){
+ aux += separator + parseAtomicNode(it);
+ if(first){
+ separator = ",";
+ first = false;
+ }
+ }
+ return "[" + aux + "]";
+ break;
+ case YAML::NodeType::Map:
+ return mapToString(fetchAsMap(_n));
+ break;
+ default:
+ return "";
+ break;
+ }
+ }
+
+ /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
+ * Scalar, Sequence or Map.
+ * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
+ * @param n node to be test for atomicity
+ */
+ bool isAtomic(std::string _key, YAML::Node _n){
+ assert(_n.Type() != YAML::NodeType::Undefined && _n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
+ std::regex r("^\\$.*");
+ bool is_atomic = true;
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return true;
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n) {
+ switch(it.Type()){
+ case YAML::NodeType::Map:
+ for(const auto& kv : it){
+ is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
+ }
+ break;
+ default:
+ is_atomic = is_atomic and isAtomic("", it);
+ break;
+ }
+ }
+ return is_atomic;
+ break;
+ case YAML::NodeType::Map:
+ is_atomic = std::regex_match(_key, r);
+ return is_atomic;
+ break;
+ default:
+ throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(_n.Type()));
+ return false;
+ break;
+ }
+ return false;
+ }
+}
+ParserYaml::ParserYaml(std::string _file, std::string path_root, bool freely_parse)
+{
+ params_ = std::map<std::string, std::string>();
+ active_name_ = "";
+ paramsSens_ = std::vector<ParamsInitSensor>();
+ paramsProc_ = std::vector<ParamsInitProcessor>();
+ subscriber_managers_ = std::vector<SubscriberManager>();
+ publisher_managers_ = std::vector<PublisherManager>();
+ parsing_file_ = std::stack<std::string>();
+ file_ = _file;
+ if (path_root != "")
+ {
+ std::regex r(".*/ *$");
+ if (not std::regex_match(path_root, r))
+ path_root_ = path_root + "/";
+ else
+ path_root_ = path_root;
+ }
+ if (not freely_parse)
+ parse();
+ else
+ parse_freely();
+}
+
+std::string ParserYaml::generatePath(std::string _file)
+{
+ std::regex r("^/.*");
+ if (std::regex_match(_file, r))
+ {
+ return _file;
+ }
+ else
+ {
+ return path_root_ + _file;
+ }
+}
+YAML::Node ParserYaml::loadYaml(std::string _file)
+{
+ try
+ {
+ // std::cout << "Parsing " << generatePath(_file) << std::endl;
+ WOLF_INFO("Parsing file: ", generatePath(_file));
+ return YAML::LoadFile(generatePath(_file));
+ }
+ catch (YAML::BadFile& e)
+ {
+ throw std::runtime_error("Couldn't load file " + generatePath(_file) + ". Tried to open it from " +
+ parsing_file_.top());
+ }
+}
+std::string ParserYaml::tagsToString(std::vector<std::string>& _tags)
+{
+ std::string hdr = "";
+ for (auto it : _tags)
+ {
+ hdr = hdr + "/" + it;
+ }
+ return hdr;
+}
+void ParserYaml::walkTree(std::string _file)
+{
+ YAML::Node n;
+ n = loadYaml(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ std::vector<std::string> hdrs = std::vector<std::string>();
+ walkTreeR(n, hdrs, "");
+ parsing_file_.pop();
+}
+void ParserYaml::walkTree(std::string _file, std::vector<std::string>& _tags)
+{
+ YAML::Node n;
+ n = loadYaml(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, "");
+ parsing_file_.pop();
+}
+void ParserYaml::walkTree(std::string _file, std::vector<std::string>& _tags, std::string hdr)
+{
+ YAML::Node n;
+ n = loadYaml(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, hdr);
+ parsing_file_.pop();
+}
+void ParserYaml::walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string hdr)
+{
+ switch (_n.Type())
+ {
+ case YAML::NodeType::Scalar:
+ {
+ std::regex r("^@.*");
+ if (std::regex_match(_n.Scalar(), r))
+ {
+ std::string str = _n.Scalar();
+ walkTree(str.substr(1, str.size() - 1), _tags, hdr);
+ }
+ else
+ {
+ insert_register(hdr, _n.Scalar());
+ }
+ break;
+ }
+ case YAML::NodeType::Sequence:
+ {
+ if (isAtomic("", _n))
+ {
+ insert_register(hdr, parseAtomicNode(_n));
+ }
+ else
+ {
+ for (const auto& kv : _n)
+ {
+ walkTreeR(kv, _tags, hdr);
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Map:
+ {
+ for (const auto& kv : _n)
+ {
+ if (isAtomic(kv.first.as<std::string>(), _n))
+ {
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
+ key = key.substr(1, key.size() - 1);
+ insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
+ }
+ else
+ {
+ /*
+ If key=="follow" then the parser will assume that the value is a path and will parse
+ the (expected) yaml file at the specified path. Note that this does not increase the header depth.
+ The following example shows how the header remains unafected:
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - follow: "@some_path" |
+ - var: 1.2 |
+ Resulting map:
+ cov_det -> 1
+ my_value-> 23
+ var: 1.2
+ Instead of:
+ cov_det -> 1
+ follow/my_value-> 23
+ var: 1.2
+ Which would result from the following yaml files
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - $follow: "@some_path" |
+ - var: 1.2 |
+ */
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
+ std::regex rr("follow");
+ if (not std::regex_match(kv.first.as<std::string>(), rr))
+ {
+ _tags.push_back(kv.first.as<std::string>());
+ if (_tags.size() == 2)
+ updateActiveName(kv.first.as<std::string>());
+ walkTreeR(kv.second, _tags, hdr + "/" + kv.first.as<std::string>());
+ _tags.pop_back();
+ if (_tags.size() == 1)
+ updateActiveName("");
+ }
+ else
+ {
+ walkTree(kv.second.as<std::string>(), _tags, hdr);
+ }
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Null:
+ break;
+ default:
+ assert(1 == 0 && "Unsupported node Type at walkTreeR.");
+ break;
+ }
+}
+void ParserYaml::updateActiveName(std::string _tag)
+{
+ active_name_ = _tag;
+}
+/*
+** @brief This function is responsible for parsing the first level of the YAML file.
+** The first level here can be thought as the entry point of the YAML file. Since we impose a certain structure
+** this function is responsible for enforcing said structure.
+**
+ */
+void ParserYaml::parseFirstLevel(YAML::Node _n, std::string _file)
+{
+
+ YAML::Node n_config = _n["config"];
+ // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
+ if (n_config.Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find config node. Please make sure that your YAML file " +
+ generatePath(_file) + " starts with 'config:'");
+ if (n_config["problem"].Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " +
+ generatePath(_file) + " has a 'problem' entry");
+ problem = n_config["problem"];
+ std::vector<std::map<std::string, std::string>> map_container;
+ try
+ {
+ for (const auto& kv : n_config["sensors"])
+ {
+ ParamsInitSensor pSensor = { kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv };
+ paramsSens_.push_back(pSensor);
+ map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() } }));
+ }
+ insert_register("sensors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing sensors @" + generatePath(_file) +
+ ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["processors"])
+ {
+ ParamsInitProcessor pProc = {
+ kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv
+ };
+ paramsProc_.push_back(pProc);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("processors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing processors @" + generatePath(_file) +
+ ". Please make sure that each processor has 'type', 'name', 'plugin' and "
+ "'sensor_name' entries.");
+ }
+ try
+ {
+ for (const auto& kv : n_config["ROS subscriber"])
+ {
+ SubscriberManager pSubscriberManager = {
+ kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv
+ };
+ subscriber_managers_.push_back(pSubscriberManager);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing subscriber @" + generatePath(_file) +
+ ". Please make sure that each manager has 'package', 'type', 'topic' and "
+ "'sensor_name' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["ROS publisher"])
+ {
+ PublisherManager pPublisherManager = {
+ kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
+ };
+ publisher_managers_.push_back(pPublisherManager);
+ map_container.push_back(std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "period", kv["period"].Scalar() } }));
+ }
+ insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
+ ". Please make sure that each manager has 'package', 'type', 'topic' and 'period' entries.");
+ }
+}
+std::map<std::string, std::string> ParserYaml::getParams()
+{
+ std::map<std::string, std::string> rtn = params_;
+ return rtn;
+}
+void ParserYaml::parse()
+{
+ parsing_file_.push(generatePath(file_));
+
+ YAML::Node n;
+ n = loadYaml(generatePath(file_));
+
+ parseFirstLevel(n, file_);
+
+
+ if (problem.Type() != YAML::NodeType::Undefined)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(problem, tags, "problem");
+ }
+ for (auto it : paramsSens_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("sensor");
+ walkTreeR(it.n_, tags, "sensor/" + it.name_);
+ }
+ for (auto it : paramsProc_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("processor");
+ walkTreeR(it.n_, tags, "processor/" + it.name_);
+ }
+ for (auto it : subscriber_managers_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("ROS subscriber");
+ walkTreeR(it.n_, tags, "ROS subscriber/" + it.type_ + " - " + it.topic_);
+ }
+ for (auto it : publisher_managers_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("ROS publisher");
+ walkTreeR(it.n_, tags, "ROS publisher/" + it.type_ + " - " + it.topic_);
+ }
+ std::list<std::string> plugins;
+ std::list<std::string> packages_subscriber, packages_publisher;
+ for (const auto& it : paramsSens_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : paramsProc_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : subscriber_managers_)
+ packages_subscriber.push_back(it.package_);
+ for (const auto& it : publisher_managers_)
+ packages_publisher.push_back(it.package_);
+ plugins.sort();
+ plugins.unique();
+ packages_subscriber.sort();
+ packages_subscriber.unique();
+ packages_publisher.sort();
+ packages_publisher.unique();
+ insert_register("plugins", wolf::converter<std::string>::convert(plugins));
+ insert_register("packages_subscriber", wolf::converter<std::string>::convert(packages_subscriber));
+ insert_register("packages_publisher", wolf::converter<std::string>::convert(packages_publisher));
+
+ // YAML::Node n;
+ // n = loadYaml(generatePath(file_));
+
+ if (n.Type() == YAML::NodeType::Map)
+ {
+ for (auto it : n)
+ {
+ auto node = it.second;
+ std::vector<std::string> tags = std::vector<std::string>();
+ if (it.first.as<std::string>() != "config")
+ walkTreeR(node, tags, it.first.as<std::string>());
+ else
+ {
+ for (auto itt : node)
+ {
+ std::string node_key = itt.first.as<std::string>();
+ if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
+ node_key != "ROS subscriber" and node_key != "ROS publisher")
+ {
+ std::regex rr("follow");
+ if (not std::regex_match(node_key, rr))
+ {
+ walkTreeR(itt.second, tags, node_key);
+ }
+ else
+ {
+ walkTree(itt.second.as<std::string>(), tags, "");
+ }
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(n, tags, "");
+ }
+ parsing_file_.pop();
+}
+/*
+** @brief This function gives the ability to run the parser without enforcing the wolf YAML structure.
+ */
+void ParserYaml::parse_freely()
+{
+ parsing_file_.push(generatePath(file_));
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(loadYaml(file_), tags, "");
+ parsing_file_.pop();
+}
+void ParserYaml::insert_register(std::string _key, std::string _value)
+{
+ if (_key.substr(0, 1) == "/")
+ _key = _key.substr(1, _key.size() - 1);
+ auto inserted_it = params_.insert(std::pair<std::string, std::string>(_key, _value));
+ if (not inserted_it.second)
+ WOLF_WARN("Skipping key '",
+ _key,
+ "' with value '",
+ _value,
+ "'. There already exists the register: (",
+ inserted_it.first->first,
+ ",",
+ inserted_it.first->second,
+ ")");
+}
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
deleted file mode 100644
index da21efc2ddb6e0acb08a7e509999f0b2195d81a4..0000000000000000000000000000000000000000
--- a/src/yaml/processor_image_yaml.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/**
- * \file processor_image_yaml.cpp
- *
- * Created on: May 21, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-#include "core/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-#include "core/processor/processor_params_image.h"
-
-namespace wolf
-{
-namespace
-{
-static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _filename_dot_yaml)
-{
- using std::string;
- using YAML::Node;
-
- std::shared_ptr<ProcessorParamsTrackerFeatureImage> p = std::make_shared<ProcessorParamsTrackerFeatureImage>();
-
- Node params = YAML::LoadFile(_filename_dot_yaml);
-
- if (!params.IsNull())
- {
- Node dd_yaml = params["vision_utils"];
- p->yaml_file_params_vision_utils = dd_yaml["YAML file params"].as<std::string>();
- // Check if relative path
- if (!p->yaml_file_params_vision_utils.empty())
- {
- if (p->yaml_file_params_vision_utils[0] != '/')
- {
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::cout << "Wolf root: " << wolf_root << std::endl;
- std::string abs_path = wolf_root + "/src/examples/" + p->yaml_file_params_vision_utils;
- p->yaml_file_params_vision_utils = abs_path;
- }
- }
-
- Node alg = params["algorithm"];
- p->max_new_features = alg["maximum new features"].as<unsigned int>();
- p->min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>();
- p->min_response_for_new_features = alg["minimum response for new features"].as<float>();
- p->time_tolerance= alg["time tolerance"].as<Scalar>();
- p->distance= alg["distance"].as<Scalar>();
-
- Node noi = params["noise"];
- p->pixel_noise_std = noi["pixel noise std"].as<Scalar>();
- p->pixel_noise_var = p->pixel_noise_std * p->pixel_noise_std;
- }
-
- return p;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_image_feature_par = ProcessorParamsFactory::get().registerCreator("IMAGE FEATURE", createProcessorParamsImage);
-const bool WOLF_UNUSED registered_prc_image_landmark_par = ProcessorParamsFactory::get().registerCreator("IMAGE LANDMARK", createProcessorParamsImage);
-
-}
-}
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
deleted file mode 100644
index ff564779517aa946365510c4a046c1cbc91de053..0000000000000000000000000000000000000000
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/**
- * \file processor_odom_3D_yaml.cpp
- *
- * Created on: Oct 25, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-#include "core/processor/processor_odom_3D.h"
-#include "core/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["processor type"].as<std::string>() == "ODOM 3D")
- {
- YAML::Node kf_vote = config["keyframe vote"];
-
- ProcessorParamsOdom3DPtr params = std::make_shared<ProcessorParamsOdom3D>();
-
- params->time_tolerance = config["time tolerance"] .as<Scalar>();
- params->max_time_span = kf_vote["max time span"] .as<Scalar>();
- params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >();
- params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>();
- params->angle_turned = kf_vote["angle turned"] .as<Scalar>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No processor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("ODOM 3D", createProcessorOdom3DParams);
-
-} // namespace [unnamed]
-
-} // namespace wolf
-
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd168ee20b162426a85d7b1b78e508f2c00a8fe1
--- /dev/null
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -0,0 +1,55 @@
+/**
+ * \file processor_odom_3d_yaml.cpp
+ *
+ * Created on: Oct 25, 2016
+ * \author: jsola
+ */
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/processor/processor_odom_3d.h"
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "ProcessorOdom3d")
+ {
+ ParamsProcessorOdom3dPtr params = std::make_shared<ParamsProcessorOdom3d>();
+
+ params->time_tolerance = config["time_tolerance"] .as<double>();
+ params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"] .as<double>();
+
+ YAML::Node keyframe_vote = config["keyframe_vote"];
+
+ params->voting_active = keyframe_vote["voting_active"] .as<bool>();
+ params->max_time_span = keyframe_vote["max_time_span"] .as<double>();
+ params->max_buff_length = keyframe_vote["max_buff_length"] .as<SizeEigen>();
+ params->dist_traveled = keyframe_vote["dist_traveled"] .as<double>();
+ params->angle_turned = keyframe_vote["angle_turned"] .as<double>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No processor type found." << std::endl;
+ return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_prc_odom_3d = FactoryParamsProcessor::registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
+
+} // namespace [unnamed]
+
+} // namespace wolf
+
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a33b7e3c7d1cf0b73374aa10510083bf9e47f9fd
--- /dev/null
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -0,0 +1,48 @@
+/**
+ * \file sensor_odom_2d_yaml.cpp
+ *
+ * Created on: Aug 3, 2019
+ * \author: jsola
+ */
+
+
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/sensor/sensor_odom_2d.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsSensorBasePtr createParamsSensorOdom2d(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "SensorOdom2d")
+ {
+ auto params = std::make_shared<ParamsSensorOdom2d>();
+
+ params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>();
+ params->k_rot_to_rot = config["k_rot_to_rot"] .as<double>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No sensor type found." << std::endl;
+ return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_odom_2d_intr = FactoryParamsSensor::registerCreator("SensorOdom2d", createParamsSensorOdom2d);
+
+} // namespace [unnamed]
+
+} // namespace wolf
+
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
deleted file mode 100644
index 3756953b46cf952c900dca37569f8d51118509e4..0000000000000000000000000000000000000000
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/**
- * \file sensor_odom_3D_yaml.cpp
- *
- * Created on: Oct 25, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["sensor type"].as<std::string>() == "ODOM 3D")
- {
- YAML::Node variances = config["motion variances"];
-
- IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>();
-
- params->k_disp_to_disp = variances["disp_to_disp"] .as<Scalar>();
- params->k_disp_to_rot = variances["disp_to_rot"] .as<Scalar>();
- params->k_rot_to_rot = variances["rot_to_rot"] .as<Scalar>();
- params->min_disp_var = variances["min_disp_var"] .as<Scalar>();
- params->min_rot_var = variances["min_rot_var"] .as<Scalar>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No sensor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_odom_3D_intr = IntrinsicsFactory::get().registerCreator("ODOM 3D", createIntrinsicsOdom3D);
-
-} // namespace [unnamed]
-
-} // namespace wolf
-
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edba0bd17d21b86508e01a407b4dea5bf41f91bd
--- /dev/null
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -0,0 +1,50 @@
+/**
+ * \file sensor_odom_3d_yaml.cpp
+ *
+ * Created on: Oct 25, 2016
+ * \author: jsola
+ */
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsSensorBasePtr createParamsSensorOdom3d(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "SensorOdom3d")
+ {
+ ParamsSensorOdom3dPtr params = std::make_shared<ParamsSensorOdom3d>();
+
+ params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>();
+ params->k_disp_to_rot = config["k_disp_to_rot"] .as<double>();
+ params->k_rot_to_rot = config["k_rot_to_rot"] .as<double>();
+ params->min_disp_var = config["min_disp_var"] .as<double>();
+ params->min_rot_var = config["min_rot_var"] .as<double>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No sensor type found." << std::endl;
+ return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorOdom3d", createParamsSensorOdom3d);
+
+} // namespace [unnamed]
+
+} // namespace wolf
+
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4b9521595c7d5ac100e7d6dd72b266dc884d7336
--- /dev/null
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -0,0 +1,47 @@
+/**
+ * \file sensor_pose_yaml.cpp
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/sensor/sensor_pose.h"
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "SensorPose")
+ {
+ ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
+
+ params->std_p = config["std_p"].as<double>();
+ params->std_o = config["std_o"].as<double>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No sensor type found." << std::endl;
+ return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorPose", createParamsSensorPose);
+
+} // namespace [unnamed]
+
+} // namespace wolf
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c0d47dea04b20e26ed26cf685b68f169a073bdab..46964ac57b3430b8113edbb7ee23d4923fc0d50b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,11 +28,15 @@ include_directories(${GTEST_INCLUDE_DIRS})
# #
# Create a specific test executable for gtest_example #
wolf_add_gtest(gtest_example gtest_example.cpp) #
-target_link_libraries(gtest_example ${PROJECT_NAME}) #
+target_link_libraries(gtest_example ${PLUGIN_NAME}) #
# #
###########################################################
-
-
+set(SRC_DUMMY
+ dummy/processor_tracker_feature_dummy.cpp
+ dummy/processor_tracker_landmark_dummy.cpp
+ )
+add_library(dummy SHARED ${SRC_DUMMY})
+target_link_libraries(dummy ${PLUGIN_NAME})
################# ADD YOUR TESTS BELOW ####################
# #
# ==== IN ALPHABETICAL ORDER! ==== #
@@ -42,150 +46,256 @@ target_link_libraries(gtest_example ${PROJECT_NAME}) #
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-target_link_libraries(gtest_capture_base ${PROJECT_NAME})
+target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
-# CaptureBase class test
-#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
-#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME})
+# Converter from String to various types used by the parameters server
+wolf_add_gtest(gtest_converter gtest_converter.cpp)
+target_link_libraries(gtest_converter ${PLUGIN_NAME})
+
+# FactorBase class test
+wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
+target_link_libraries(gtest_factor_base ${PLUGIN_NAME})
# FactorAutodiff class test
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
-target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
+target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
-# Converter from String to various types used by the parameters server
-wolf_add_gtest(gtest_converter gtest_converter.cpp)
-target_link_libraries(gtest_converter ${PROJECT_NAME})
+# FactoryStateBlock factory test
+wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
+target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
+target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME})
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-target_link_libraries(gtest_emplace ${PROJECT_NAME})
+target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
# FeatureBase classes test
wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-target_link_libraries(gtest_feature_base ${PROJECT_NAME})
+target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
# FrameBase classes test
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-target_link_libraries(gtest_frame_base ${PROJECT_NAME})
+target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
+
+# GraphSearch class test
+wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
+target_link_libraries(gtest_graph_search ${PLUGIN_NAME})
+
+# HasStateBlocks classes test
+wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
+target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
+
+# IsMotion classes test
+wolf_add_gtest(gtest_is_motion gtest_is_motion.cpp)
+target_link_libraries(gtest_is_motion ${PLUGIN_NAME})
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-target_link_libraries(gtest_local_param ${PROJECT_NAME})
+target_link_libraries(gtest_local_param ${PLUGIN_NAME})
+
+# Logging test
+wolf_add_gtest(gtest_logging gtest_logging.cpp)
+target_link_libraries(gtest_logging ${PLUGIN_NAME})
# MotionBuffer class test
wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
+target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME})
# PackKFBuffer
wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
-target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME})
+target_link_libraries(gtest_pack_KF_buffer ${PLUGIN_NAME} dummy)
# Parameters server
wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
-target_link_libraries(gtest_param_server ${PROJECT_NAME})
+target_link_libraries(gtest_param_server ${PLUGIN_NAME})
# YAML parser
wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
-target_link_libraries(gtest_parser_yaml ${PROJECT_NAME})
+target_link_libraries(gtest_parser_yaml ${PLUGIN_NAME})
# Problem class test
wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PROJECT_NAME})
+target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy)
# ProcessorBase class test
wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PROJECT_NAME})
+target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
# ProcessorMotion class test
wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
+target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
# SE3 test
wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
+target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
-target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
+target_link_libraries(gtest_sensor_base ${PLUGIN_NAME})
# shared_from_this test
wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
+target_link_libraries(gtest_shared_from_this ${PLUGIN_NAME})
# SolverManager test
wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
-target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
+target_link_libraries(gtest_solver_manager ${PLUGIN_NAME})
+
+# SolverManagerMultithread test
+wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp)
+target_link_libraries(gtest_solver_manager_multithread ${PLUGIN_NAME})
+
+# StateBlock class test
+wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
+target_link_libraries(gtest_state_block ${PLUGIN_NAME})
+
+# StateBlock class test
+wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
+target_link_libraries(gtest_state_composite ${PLUGIN_NAME})
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
-target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
+target_link_libraries(gtest_time_stamp ${PLUGIN_NAME})
# TrackMatrix class test
wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
-target_link_libraries(gtest_track_matrix ${PROJECT_NAME})
+target_link_libraries(gtest_track_matrix ${PLUGIN_NAME})
# TrajectoryBase class test
wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
-target_link_libraries(gtest_trajectory ${PROJECT_NAME})
+target_link_libraries(gtest_trajectory ${PLUGIN_NAME})
-# ------- Now Derived classes ----------
+# TreeManager class test
+wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
+target_link_libraries(gtest_tree_manager ${PLUGIN_NAME})
-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)
+# ------- Now Derived classes ----------
# FactorAbs(P/O/V) classes test
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
-target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
+target_link_libraries(gtest_factor_absolute ${PLUGIN_NAME})
+
+# FactorAutodiffDistance3d test
+wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
+target_link_libraries(gtest_factor_autodiff_distance_3d ${PLUGIN_NAME})
+
+# FactorBlockDifference class test
+wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
+target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME})
+
+# FactorDiffDrive class test
+wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
+target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
+
+# FactorOdom2dAutodiff class test
+wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
+target_link_libraries(gtest_factor_odom_2d_autodiff ${PLUGIN_NAME})
-# 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})
+# FactorPose2d class test
+wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
+target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME})
-# FactorOdom3D class test
-wolf_add_gtest(gtest_factor_odom_3D gtest_factor_odom_3D.cpp)
-target_link_libraries(gtest_factor_odom_3D ${PROJECT_NAME})
+# FactorPose3d class test
+wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
+target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME})
-# FactorPose2D class test
-wolf_add_gtest(gtest_factor_pose_2D gtest_factor_pose_2D.cpp)
-target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
+# FactorRelativePose2d class test
+wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
+target_link_libraries(gtest_factor_relative_pose_2d ${PLUGIN_NAME})
-# FactorPose3D class test
-wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
-target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
+# FactorRelativePose2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
+target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAME})
+# FactorRelativePose3d class test
+wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
+target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
+
+# FactorVelocityLocalDirection3d class test
+wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
+target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME})
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
+target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
+
+# Map yaml save/load test
+wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
+target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
# Parameter prior test
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PROJECT_NAME})
+target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
+
+# ProcessorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_processor_fix_wing_model gtest_processor_fix_wing_model.cpp)
+target_link_libraries(gtest_processor_fix_wing_model ${PLUGIN_NAME})
+
+# ProcessorFixWingModel class test
+wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
+target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
+# ProcessorLoopClosure class test
+wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
+target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
# ProcessorFrameNearestNeighborFilter class test
-wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
-target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
+# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
+# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PLUGIN_NAME})
+# ProcessorMotion in 2d
+wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
+target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
-# 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_processor_odom_3d gtest_processor_odom_3d.cpp)
+target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
-# ProcessorOdom3D class test
-wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp)
-target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
+# FactorPose3dWithExtrinsics class test
+wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+target_link_libraries(gtest_processor_and_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
+
+
+# ProcessorTrackerFeatureDummy class test
+wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
+target_link_libraries(gtest_processor_tracker_feature_dummy ${PLUGIN_NAME} dummy)
+
+# ProcessorTrackerLandmarkDummy class test
+wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
+target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dummy)
+
+# SensorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
+target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
+
+# SensorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
+target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
+
+IF (Ceres_FOUND)
+ # SolverCeres test
+ wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
+ target_link_libraries(gtest_solver_ceres ${PLUGIN_NAME})
+
+ # SolverCeresMultithread test
+ wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
+ target_link_libraries(gtest_solver_ceres_multithread ${PLUGIN_NAME})
+ENDIF(Ceres_FOUND)
-# ------- Now Core classes Serialization ----------
+# TreeManagerSlidingWindow class test
+wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
+target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME})
-add_subdirectory(serialization)
+# TreeManagerSlidingWindowDualRate class test
+wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
+target_link_libraries(gtest_tree_manager_sliding_window_dual_rate ${PLUGIN_NAME})
+# yaml conversions
+wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
+target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME})
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2d174702710418cb6ff17ff55fd586be872ec68
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -0,0 +1,45 @@
+#ifndef FACTOR_DUMMY_ZERO_1_H_
+#define FACTOR_DUMMY_ZERO_1_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorDummyZero1);
+
+//class
+class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
+{
+ using Base = FactorAutodiff<FactorDummyZero1, 1, 1>;
+ public:
+ FactorDummyZero1(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb_ptr) :
+ Base("FactorDummy1",
+ TOP_OTHER,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb_ptr)
+ {
+ //
+ }
+
+ ~FactorDummyZero1() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _st1,
+ T* _residuals) const
+ {
+ _residuals[0] = _st1[0];
+ return true;
+ }
+};
+
+} // namespace wolf
+
+#endif
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_12.h
new file mode 100644
index 0000000000000000000000000000000000000000..978592b6124abc815d4f0f2a897e0ef78e70d95b
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_12.h
@@ -0,0 +1,155 @@
+#ifndef FACTOR_DUMMY_ZERO_12_H_
+#define FACTOR_DUMMY_ZERO_12_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+//class
+template <unsigned int ID>
+class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
+{
+ using Base = FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>;
+ static const unsigned int id = ID;
+ public:
+ FactorDummyZero12(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const StateBlockPtr& _sb3_ptr,
+ const StateBlockPtr& _sb4_ptr,
+ const StateBlockPtr& _sb5_ptr,
+ const StateBlockPtr& _sb6_ptr,
+ const StateBlockPtr& _sb7_ptr,
+ const StateBlockPtr& _sb8_ptr,
+ const StateBlockPtr& _sb9_ptr,
+ const StateBlockPtr& _sb10_ptr,
+ const StateBlockPtr& _sb11_ptr,
+ const StateBlockPtr& _sb12_ptr) :
+ Base("FactorDummy12",
+ TOP_OTHER,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb1_ptr,
+ _sb2_ptr,
+ _sb3_ptr,
+ _sb4_ptr,
+ _sb5_ptr,
+ _sb6_ptr,
+ _sb7_ptr,
+ _sb8_ptr,
+ _sb9_ptr,
+ _sb10_ptr,
+ _sb11_ptr,
+ _sb12_ptr)
+ {
+ assert(id > 0 && id <= 12);
+ }
+
+ ~FactorDummyZero12() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _st1,
+ const T* const _st2,
+ const T* const _st3,
+ const T* const _st4,
+ const T* const _st5,
+ const T* const _st6,
+ const T* const _st7,
+ const T* const _st8,
+ const T* const _st9,
+ const T* const _st10,
+ const T* const _st11,
+ const T* const _st12,
+ T* _residuals) const
+ {
+ Eigen::Map<Eigen::Matrix<T,ID,1>> residuals(_residuals);
+ switch (id)
+ {
+ case 1:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st1(_st1);
+ residuals = st1;
+ break;
+ }
+ case 2:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st2(_st2);
+ residuals = st2;
+ break;
+ }
+ case 3:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st3(_st3);
+ residuals = st3;
+ break;
+ }
+ case 4:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st4(_st4);
+ residuals = st4;
+ break;
+ }
+ case 5:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st5(_st5);
+ residuals = st5;
+ break;
+ }
+ case 6:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st6(_st6);
+ residuals = st6;
+ break;
+ }
+ case 7:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st7(_st7);
+ residuals = st7;
+ break;
+ }
+ case 8:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st8(_st8);
+ residuals = st8;
+ break;
+ }
+ case 9:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st9(_st9);
+ residuals = st9;
+ break;
+ }
+ case 10:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st10(_st10);
+ residuals = st10;
+ break;
+ }
+ case 11:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st11(_st11);
+ residuals = st11;
+ break;
+ }
+ case 12:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st12(_st12);
+ residuals = st12;
+ break;
+ }
+ default:
+ throw std::runtime_error("ID not found");
+ }
+ return true;
+ }
+};
+
+} // namespace wolf
+
+#endif
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf81f302567c80022f7c1c4abaf0e92f5341771f
--- /dev/null
+++ b/test/dummy/factor_feature_dummy.h
@@ -0,0 +1,82 @@
+#ifndef FACTOR_FEATURE_DUMMY_H
+#define FACTOR_FEATURE_DUMMY_H
+
+#include "core/factor/factor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorFeatureDummy);
+
+class FactorFeatureDummy : public FactorBase
+{
+ public:
+ FactorFeatureDummy(const FeatureBasePtr& _feature_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorFeatureDummy() override = default;
+
+ /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
+ **/
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override {return true;};
+
+ /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ **/
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+
+ /** \brief Returns the jacobians computation method
+ **/
+ JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
+
+ /** \brief Returns a vector of pointers to the states in which this factor depends
+ **/
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+
+ /** \brief Returns the factor residual size
+ **/
+ unsigned int getSize() const override {return 0;}
+
+ /** \brief Returns the factor states sizes
+ **/
+ 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 FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ TOP_OTHER,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ _feature_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
+{
+ //
+}
+
+inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr)
+{
+ return std::make_shared<FactorFeatureDummy>(_feature_ptr,
+ std::static_pointer_cast<FeatureBase>(_correspondant_ptr),
+ _processor_ptr);
+}
+
+} // namespace wolf
+
+#endif // FACTOR_FEATURE_DUMMY_H
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..5f6f21124d25ff194b042241e136f755761c4411
--- /dev/null
+++ b/test/dummy/factor_landmark_dummy.h
@@ -0,0 +1,92 @@
+#ifndef FACTOR_LANDMARK_DUMMY_H
+#define FACTOR_LANDMARK_DUMMY_H
+
+#include "core/factor/factor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorLandmarkDummy);
+
+class FactorLandmarkDummy : public FactorBase
+{
+ public:
+ FactorLandmarkDummy(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorLandmarkDummy() override = default;
+
+ /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
+ **/
+ bool evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override {return true;};
+
+ /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ **/
+ void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& residual_,
+ std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+
+ /** \brief Returns the jacobians computation method
+ **/
+ JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
+
+ /** \brief Returns a vector of pointers to the states in which this factor depends
+ **/
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockPtr>(0);
+ }
+
+ /** \brief Returns the factor residual size
+ **/
+ unsigned int getSize() const override {return 0;}
+
+ /** \brief Returns the factor states sizes
+ **/
+ 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 FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ TOP_OTHER,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
+{
+ //
+}
+
+inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr)
+{
+ return std::make_shared<FactorLandmarkDummy>(_feature_ptr,
+ std::static_pointer_cast<LandmarkBase>(_correspondant_ptr),
+ _processor_ptr);
+}
+
+} // namespace wolf
+
+#endif // FACTOR_LANDMARK_DUMMY_H
diff --git a/include/core/factor/factor_odom_2D.h b/test/dummy/factor_odom_2d_autodiff.h
similarity index 50%
rename from include/core/factor/factor_odom_2D.h
rename to test/dummy/factor_odom_2d_autodiff.h
index e5cfd57a69329ca9c82c9019f29d4ee591b27f77..e765ed760472534f4de3024530ed07df7e15f0d1 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/test/dummy/factor_odom_2d_autodiff.h
@@ -1,52 +1,56 @@
-#ifndef FACTOR_ODOM_2D_THETA_H_
-#define FACTOR_ODOM_2D_THETA_H_
+#ifndef FACTOR_ODOM_2d_AUTODIFF_H_
+#define FACTOR_ODOM_2d_AUTODIFF_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorOdom2D);
+WOLF_PTR_TYPEDEFS(FactorOdom2dAutodiff);
//class
-class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
+class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 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",
+ FactorOdom2dAutodiff(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1>("FactorOdom2dAutodiff",
+ TOP_OTHER,
+ _ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function, _status,
_frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
- virtual ~FactorOdom2D() = default;
+ ~FactorOdom2dAutodiff() override = 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);
- }
+// 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,
+inline bool FactorOdom2dAutodiff::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, T* _residuals) const
{
@@ -64,30 +68,27 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
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 );
+ er = expected_measurement - getMeasurement();
+ er(2) = pi2pi(er(2));
// Residuals
- res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getMeasurementSquareRootInformationUpper() * 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;
+// Eigen::MatrixXd J(3,6);
+// J.row(0) = ((Jet<double, 6>)(er(0))).v;
+// J.row(1) = ((Jet<double, 6>)(er(1))).v;
+// J.row(2) = ((Jet<double, 6>)(er(2))).v;
+// J.row(0) = ((Jet<double, 6>)(res(0))).v;
+// J.row(1) = ((Jet<double, 6>)(res(1))).v;
+// J.row(2) = ((Jet<double, 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;
+// 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;
// }
////////////////////////////////////////////////////////
@@ -96,4 +97,4 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
} // namespace wolf
-#endif
+#endif // FACTOR_ODOM_2d_AUTODIFF_H_
diff --git a/test/dummy/processor_is_motion_dummy.h b/test/dummy/processor_is_motion_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..dca4808c90602d550156a9e486514480b2c7f6b1
--- /dev/null
+++ b/test/dummy/processor_is_motion_dummy.h
@@ -0,0 +1,76 @@
+/*
+ * processor_is_motion_dummy.h
+ *
+ * Created on: Mar 8, 2021
+ * Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+
+#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorIsMotionDummy);
+
+struct ParamsProcessorIsMotionDummy : public ParamsProcessorBase, public ParamsIsMotion
+{
+ std::string state_structure = "PO";
+
+ ParamsProcessorIsMotionDummy() = default;
+ ParamsProcessorIsMotionDummy(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server),
+ ParamsIsMotion(_unique_name, _server)
+ {
+
+ };
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorIsMotionDummy);
+
+class ProcessorIsMotionDummy : public ProcessorBase, public IsMotion
+{
+ public:
+ ProcessorIsMotionDummy(ParamsProcessorIsMotionDummyPtr _params) :
+ ProcessorBase("ProcessorIsMotionDummy", 2, _params),
+ IsMotion(_params->state_structure, _params)
+ {}
+ ~ProcessorIsMotionDummy(){};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorIsMotionDummy, ParamsProcessorIsMotionDummy);
+
+ void configure(SensorBasePtr _sensor) override {};
+ void processCapture(CaptureBasePtr) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
+ bool triggerInCapture(CaptureBasePtr) const override { return false; };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override { return false; };
+ bool storeKeyFrame(FrameBasePtr) override { return false; };
+ bool storeCapture(CaptureBasePtr) override { return false; };
+ bool voteForKeyFrame() const override { return false; };
+ TimeStamp getTimeStamp() const override {return TimeStamp(0);};
+
+ VectorComposite getState(const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorIsMotionDummy);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorIsMotionDummy);
+} // namespace wolf
+
+#endif /* TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ */
diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..388f2f0dd5f13624d21115a9351680ceb92ce743
--- /dev/null
+++ b/test/dummy/processor_loop_closure_dummy.h
@@ -0,0 +1,93 @@
+#ifndef TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
+
+#include "core/processor/processor_loop_closure.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
+
+// dummy class:
+class ProcessorLoopClosureDummy : public ProcessorLoopClosure
+{
+ public:
+ ProcessorLoopClosureDummy(ParamsProcessorLoopClosurePtr _params) :
+ ProcessorLoopClosure("ProcessorLoopClosureDummy", 2, _params)
+ {
+ }
+
+ protected:
+ bool voteFindLoopClosures(CaptureBasePtr cap) override { return true;};
+ bool validateLoopClosure(MatchLoopClosurePtr match) override { return true;};
+
+ void emplaceFeatures(CaptureBasePtr cap) override
+ {
+ // feature = frame pose
+ FeatureBase::emplace<FeatureBase>(cap,
+ "FeatureLoopClosureDummy",
+ cap->getFrame()->getState().vector("PO"),
+ MatrixXd::Identity(3,3));
+ }
+
+ std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
+ {
+ std::map<double,MatchLoopClosurePtr> match_lc_map;
+
+ auto old_frame = _capture->getFrame()->getPreviousFrame();
+ while (old_frame)
+ {
+ // match if features (frames psoe) are close enough
+ for (auto cap : old_frame->getCaptureList())
+ for (auto feat : cap->getFeatureList())
+ if (feat->getType() == "FeatureLoopClosureDummy" and
+ (feat->getMeasurement() - _capture->getFeatureList().front()->getMeasurement()).norm() < 1e-3)
+ {
+ MatchLoopClosurePtr match = std::make_shared<MatchLoopClosure>();
+ match->capture_reference = cap;
+ match->capture_target = _capture;
+ match->normalized_score = 1;
+
+ while (match_lc_map.count(match->normalized_score))
+ match->normalized_score -= 1e-9;
+
+ match_lc_map.emplace(match->normalized_score, match);
+ }
+
+ old_frame = old_frame->getPreviousFrame();
+ }
+
+ return match_lc_map;
+ }
+
+ void emplaceFactors(MatchLoopClosurePtr match) override
+ {
+ FeatureBasePtr feat_2;
+ for (auto feat : match->capture_target->getFeatureList())
+ if (feat->getType() == "FeatureLoopClosureDummy")
+ {
+ feat_2 = feat;
+ break;
+ }
+
+ FactorBase::emplace<FactorRelativePose2d>(feat_2, feat_2,
+ match->capture_reference->getFrame(),
+ shared_from_this(),
+ false,
+ TOP_LOOP);
+ }
+
+ public:
+ unsigned int getNStoredFrames()
+ {
+ return buffer_pack_kf_.getContainer().size();
+ }
+
+ unsigned int getNStoredCaptures()
+ {
+ return buffer_capture_.getContainer().size();
+ }
+};
+
+
+#endif /* TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_ */
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8485d267034febde4f08911108c0773fd67ab892
--- /dev/null
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -0,0 +1,103 @@
+/**
+ * \file processor_tracker_feature_dummy.cpp
+ *
+ * Created on: Apr 11, 2016
+ * \author: jvallve
+ */
+
+#include "processor_tracker_feature_dummy.h"
+#include "core/feature/feature_base.h"
+
+namespace wolf
+{
+
+unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences)
+{
+ WOLF_INFO("tracking " , _features_in.size() , " features...");
+
+ auto count = 0;
+ for (auto feat_in : _features_in)
+ {
+ if (count < params_tracker_feature_dummy_->n_tracks_lost) // lose first ntrackslost tracks
+ {
+ WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost!");
+ count++;
+ }
+ else
+ {
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "FEATURE DUMMY",
+ feat_in->getMeasurement(),
+ feat_in->getMeasurementCovariance());
+ _features_out.push_back(ftr);
+ _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+
+ WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
+ }
+ }
+
+ return _features_out.size();
+}
+
+bool ProcessorTrackerFeatureDummy::voteForKeyFrame() const
+{
+ 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 incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
+}
+
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
+{
+ unsigned int max_features = _max_new_features;
+ WOLF_INFO("Detecting " , _max_new_features , " new features..." );
+
+ if (max_features == -1)
+ {
+ max_features = 10;
+ WOLF_INFO("max_features unlimited, setting it to " , max_features);
+ }
+ WOLF_INFO("Detecting " , _max_new_features , " new features..." );
+
+ // detecting new features
+ for (unsigned int i = 0; i < max_features; i++)
+ {
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "FeatureDummy",
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1));
+ _features_out.push_back(ftr);
+
+ WOLF_INFO("feature " , ftr->id() , " detected!" );
+ }
+
+ WOLF_INFO(_features_out.size() , " features detected!");
+
+ return _features_out.size();
+}
+
+FactorBasePtr ProcessorTrackerFeatureDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr)
+{
+ WOLF_INFO( "emplacing factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id(),
+ " with origin feature " , _feature_other_ptr->id() );
+
+ return FactorBase::emplace<FactorFeatureDummy>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this());
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerFeatureDummy)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerFeatureDummy)
+} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..12705325ac8b8b086b10ec1f0a1a13870b76b702
--- /dev/null
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -0,0 +1,134 @@
+/**
+ * \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 "core/common/wolf.h"
+#include "core/processor/processor_tracker_feature.h"
+#include "factor_feature_dummy.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureDummy);
+
+struct ParamsProcessorTrackerFeatureDummy : public ParamsProcessorTrackerFeature
+{
+ unsigned int n_tracks_lost; ///< number of tracks lost each time track is called (the first ones)
+
+ ParamsProcessorTrackerFeatureDummy() = default;
+ ParamsProcessorTrackerFeatureDummy(std::string _unique_name,
+ const wolf::ParamsServer & _server):
+ ParamsProcessorTrackerFeature(_unique_name, _server)
+ {
+ n_tracks_lost = _server.getParam<unsigned int>(prefix + _unique_name + "/n_tracks_lost");
+ }
+};
+
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
+
+//Class
+class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
+{
+
+ public:
+ ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature);
+ ~ProcessorTrackerFeatureDummy() override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ParamsProcessorTrackerFeatureDummy);
+
+ void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ ParamsProcessorTrackerFeatureDummyPtr params_tracker_feature_dummy_;
+
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
+ * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * \return the number of features tracked
+ */
+ unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) override;
+
+ /** \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
+ */
+ bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) 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!
+ */
+ bool voteForKeyFrame() const override;
+
+ /** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * 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_.
+ */
+ unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the parent Feature
+ * \param _feature_other_ptr pointer to the other feature constrained.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr) override;
+};
+
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
+ ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", "PO", 0, _params_tracker_feature_dummy),
+ params_tracker_feature_dummy_(_params_tracker_feature_dummy)
+{
+ //
+}
+
+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/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d36efd3f804cd71ff38fc7b835dc1b5230c59885
--- /dev/null
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -0,0 +1,127 @@
+/**
+ * \file processor_tracker_landmark_dummy.cpp
+ *
+ * Created on: Apr 12, 2016
+ * \author: jvallve
+ */
+
+#include "processor_tracker_landmark_dummy.h"
+#include "core/landmark/landmark_base.h"
+#include "factor_landmark_dummy.h"
+
+namespace wolf
+{
+
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
+ ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params_tracker_landmark_dummy),
+ params_tracker_landmark_dummy_(_params_tracker_landmark_dummy)
+{
+ //
+
+}
+
+ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
+{
+ //
+}
+
+unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_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 landmark_idx_non_visible_ features
+ auto prev_found = matches_landmark_from_last_.size();
+ if (prev_found == 0) prev_found = _landmarks_in.size();
+ auto count = 0;
+ for (auto landmark_in_ptr : _landmarks_in)
+ {
+ if (count < prev_found - params_tracker_landmark_dummy_->n_landmarks_lost) // lose last n_landmarks_lost landmarks
+ {
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "DUMMY FEATURE",
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1));
+ _features_out.push_back(ftr);
+ _feature_landmark_correspondences[ftr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
+ std::cout << "\t\tlandmark " << landmark_in_ptr->id() << " found!" << std::endl;
+ }
+ else
+ {
+ WOLF_INFO("landmark: " , landmark_in_ptr->id() , " lost!");
+ }
+ count++;
+ }
+ return _features_out.size();
+}
+
+bool ProcessorTrackerLandmarkDummy::voteForKeyFrame() const
+{
+ std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
+ bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_landmark_dummy_->min_features_for_keyframe;
+ std::cout << (vote ? "Vote ": "Not vote ") << "for KF" << std::endl;
+ return vote;
+}
+
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_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);
+ }
+ WOLF_INFO("Detecting " , _max_features , " new features..." );
+
+ // detecting new features
+ for (unsigned int i = 0; i < max_features; i++)
+ {
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "DUMMY FEATURE",
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1));
+ _features_out.push_back(ftr);
+
+ WOLF_INFO("feature " , ftr->id() , " detected!" );
+ }
+
+ WOLF_INFO(_features_out.size() , " features detected!");
+
+ return _features_out.size();
+}
+
+LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _feature_ptr)
+{
+ //std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
+ return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkBase",
+ std::make_shared<StateBlock>(2),
+ std::make_shared<StateBlock>(1));
+}
+
+FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr)
+{
+ std::cout << "\tProcessorTrackerLandmarkDummy::emplaceFactor" << std::endl;
+ std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
+ std::cout << "\t\tlandmark "<< _landmark_ptr->id() << std::endl;
+ return FactorBase::emplace<FactorLandmarkDummy>(_feature_ptr, _feature_ptr, _landmark_ptr, shared_from_this());
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkDummy)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerLandmarkDummy)
+} // namespace wolf
+
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..58561c896a4764202654853cf852f7afb0cc954e
--- /dev/null
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -0,0 +1,108 @@
+/**
+ * \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 "core/processor/processor_tracker_landmark.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmarkDummy);
+
+struct ParamsProcessorTrackerLandmarkDummy : public ParamsProcessorTrackerLandmark
+{
+ unsigned int n_landmarks_lost; ///< number of landmarks lost each time findLandmarks is called (the last ones)
+
+ ParamsProcessorTrackerLandmarkDummy() = default;
+ ParamsProcessorTrackerLandmarkDummy(std::string _unique_name, const wolf::ParamsServer & _server):
+ ParamsProcessorTrackerLandmark(_unique_name, _server)
+ {
+ n_landmarks_lost = _server.getParam<unsigned int>(prefix + _unique_name + "/n_landmarks_lost");
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
+
+class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
+{
+ public:
+ ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
+ ~ProcessorTrackerLandmarkDummy() override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ParamsProcessorTrackerLandmarkDummy);
+
+ void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ ParamsProcessorTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
+
+ /** \brief Find provided landmarks as features in the provided capture
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture 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
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * \return the number of landmarks found
+ */
+ unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) 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!
+ */
+ bool voteForKeyFrame() const override;
+
+ /** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * 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_.
+ */
+ unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
+
+ /** \brief Emplace one landmark
+ *
+ * Implement in derived classes to build the type of landmark you need for this tracker.
+ */
+ LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) override;
+
+ /** \brief Emplace 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.
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr) override;
+};
+
+} // namespace wolf
+
+#endif /* PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ */
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf45c71c94167f934504e5f97f91080d4d011ce8
--- /dev/null
+++ b/test/dummy/solver_manager_dummy.h
@@ -0,0 +1,108 @@
+/*
+ * solver_manager_dummy.h
+ *
+ * Created on: May 27, 2020
+ * Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+#define TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+
+#include "core/solver/solver_manager.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(SolverManagerDummy);
+class SolverManagerDummy : public SolverManager
+{
+ public:
+ std::set<FactorBasePtr> factors_derived_;
+ std::map<StateBlockPtr,bool> state_block_fixed_;
+ std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
+
+ SolverManagerDummy(const ProblemPtr& wolf_problem) :
+ SolverManager(wolf_problem)
+ {
+ };
+
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override
+ {
+ return state_block_fixed_.at(st);
+ };
+
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
+ {
+ return state_block_local_param_.at(st) == local_param;
+ };
+
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
+ {
+ return state_block_local_param_.at(st) != nullptr;
+ };
+
+ virtual int numStateBlocksDerived() const override
+ {
+ return state_block_fixed_.size();
+ }
+
+ virtual int numFactorsDerived() const override
+ {
+ return factors_derived_.size();
+ };
+
+ bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override {return false;};
+ bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override {return false;};
+
+
+ // The following are dummy implementations
+ bool hasConverged() override { return true; }
+ SizeStd iterations() override { return 1; }
+ double initialCost() override { return double(1); }
+ double finalCost() override { return double(0); }
+ double totalTime() override { return double(0); }
+
+ protected:
+
+ bool checkDerived(std::string prefix="") const override {return true;}
+ std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
+ void addFactorDerived(const FactorBasePtr& fac_ptr) override
+ {
+ factors_derived_.insert(fac_ptr);
+ };
+ void removeFactorDerived(const FactorBasePtr& fac_ptr) override
+ {
+ factors_derived_.erase(fac_ptr);
+ };
+ void addStateBlockDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ void removeStateBlockDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_.erase(state_ptr);
+ state_block_local_param_.erase(state_ptr);
+ };
+ void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ };
+ void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override
+ {
+ return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1;
+ };
+
+ bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
+ {
+ return factors_derived_.count(fac_ptr) == 1;
+ };
+};
+
+}
+
+#endif /* TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_ */
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e7e70f2ee476404cb96b070e72f280c7a570a6e
--- /dev/null
+++ b/test/dummy/tree_manager_dummy.h
@@ -0,0 +1,61 @@
+#ifndef INCLUDE_TREE_MANAGER_DUMMY_H_
+#define INCLUDE_TREE_MANAGER_DUMMY_H_
+
+#include "core/tree_manager/tree_manager_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy)
+struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
+{
+ ParamsTreeManagerDummy() = default;
+ ParamsTreeManagerDummy(std::string _unique_name, const ParamsServer& _server):
+ ParamsTreeManagerBase(_unique_name, _server)
+ {
+ toy_param = _server.getParam<double>(prefix + "/toy_param");
+ }
+
+ ~ParamsTreeManagerDummy() override = default;
+
+ bool toy_param;
+
+ std::string print() const override
+ {
+ return ParamsTreeManagerBase::print() + "\n"
+ + "toy_param: " + std::to_string(toy_param) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(TreeManagerDummy)
+
+class TreeManagerDummy : public TreeManagerBase
+{
+ public:
+ TreeManagerDummy(ParamsTreeManagerBasePtr _params) :
+ TreeManagerBase("TreeManagerDummy", _params),
+ n_KF_(0)
+ {};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerDummy, ParamsTreeManagerBase)
+
+ ~TreeManagerDummy() override{}
+
+ void keyFrameCallback(FrameBasePtr _KF) override
+ {
+ n_KF_++;
+ };
+
+ int n_KF_;
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerDummy)
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerDummy)
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_DUMMY_H_ */
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index a6146b6615adcd9bea24d3c12fab7d70ee013a12..a99f7a19551f3e450e6b4958ded294db8980c62c 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -7,160 +7,317 @@
#include "core/math/SE3.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
using namespace Eigen;
using namespace wolf;
-using namespace three_D;
+using namespace SE3;
TEST(SE3, exp_0)
{
- Vector6s tau = Vector6s::Zero();
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
+ Vector6d tau = Vector6d::Zero();
+ Vector7d pose; pose << 0,0,0, 0,0,0,1;
- ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp(tau), 1e-8);
}
TEST(SE3, log_I)
{
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
- Vector6s tau = Vector6s::Zero();
+ Vector7d pose; pose << 0,0,0, 0,0,0,1;
+ Vector6d tau = Vector6d::Zero();
- ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8);
}
TEST(SE3, exp_log)
{
- Vector6s tau = Vector6s::Random() / 5.0;
- Vector7s pose = exp_SE3(tau);
+ Vector6d tau = Vector6d::Random() / 5.0;
+ Vector7d pose = exp(tau);
- ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
- ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(exp(tau)), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp(log(pose)), 1e-8);
}
TEST(SE3, exp_of_multiple)
{
- Vector6s tau, tau2, tau3;
- Vector7s pose, pose2, pose3;
- tau = Vector6s::Random() / 5;
- pose << exp_SE3(tau);
+ Vector6d tau, tau2, tau3;
+ Vector7d pose, pose2, pose3;
+ tau = Vector6d::Random() / 5;
+ pose << exp(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);
+ ASSERT_MATRIX_APPROX(exp(tau), pose, 1e-8);
// 2
- ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(tau2), pose2, 1e-8);
// 3
- ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(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);
+ Vector6d tau, tau2, tau3;
+ Vector7d pose, pose2, pose3;
+ tau = Vector6d::Random() / 5;
+ pose << exp(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);
+ ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8);
// 2
- ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8);
+ ASSERT_MATRIX_APPROX(tau2, log(pose2), 1e-8);
// 3
- ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8);
+ ASSERT_MATRIX_APPROX(tau3, log(pose3), 1e-8);
+}
+
+TEST(SE3, composeBlocks)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc);
+
+ ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+}
+
+TEST(SE3, composeVectorBlocks)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1.coeffs(), p2, q2.coeffs(), pc, qc.coeffs());
+
+ ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+}
+
+TEST(SE3, composeEigenVectors)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+
+ Vector7d x1; x1 << p1, q1.coeffs();
+ Vector7d x2; x2 << p2, q2.coeffs();
+ Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
+
+ compose(x1, x2, xc);
+
+ ASSERT_MATRIX_APPROX(xc, xc_true, 1e-8);
+}
+
+TEST(SE3, composeVectorComposite)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+ VectorComposite x1, x2, xc("PO", {3,4});
+
+ x1.emplace('P', p1);
+ x1.emplace('O', q1.coeffs());
+ x2.emplace('P', p2);
+ x2.emplace('O', q2.coeffs());
+
+ compose(x1, x2, xc);
+
+ ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
+}
+
+TEST(SE3, composeVectorComposite_Jacobians)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ Matrix3d R1 = q1.matrix();
+ Matrix3d R2 = q2.matrix();
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+ VectorComposite x1, x2, xc("PO", {3,4});
+ MatrixComposite J_xc_x1, J_xc_x2;
+
+ x1.emplace('P', p1);
+ x1.emplace('O', q1.coeffs());
+ x2.emplace('P', p2);
+ x2.emplace('O', q2.coeffs());
+
+ // we'll do xc = x1 * x2 and obtain Jacobians
+ compose(x1, x2, xc, J_xc_x1, J_xc_x2);
+
+ ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
+
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), - R1 * skew(p2) , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose() , 1e-8);
+
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1 , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity() , 1e-8);
+}
+
+TEST(SE3, exp_0_Composite)
+{
+ // we check that exp(zero) = identity
+
+ Vector3d p; p.setZero();
+ Vector3d theta; theta.setZero();
+
+ VectorComposite tau;
+
+ tau.emplace('P', p);
+ tau.emplace('O', theta);
+
+ VectorComposite x = SE3::exp(tau);
+
+ ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
+
+}
+
+TEST(SE3, plus_0_Composite)
+{
+ // we check that x plus zero = x
+
+ Vector3d p; p.setRandom();
+ Vector4d q; q.setRandom().normalized();
+
+ VectorComposite x;
+ x.emplace('P', p);
+ x.emplace('O', q);
+
+ Vector3d dp; dp.setZero();
+ Vector3d dtheta; dtheta.setZero();
+
+ VectorComposite tau;
+ tau.emplace('P', dp);
+ tau.emplace('O', dtheta);
+
+ VectorComposite res = plus(x, tau);
+
+ ASSERT_MATRIX_APPROX(res.at('P'), p, 1e-8);
+ ASSERT_MATRIX_APPROX(res.at('O'), q, 1e-8);
}
TEST(SE3, interpolate_I_xyz)
{
- Vector7s p1, p2, p_pre;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 1,2,3, 0,0,0,1;
- Scalar t = 0.2;
+ double t = 0.2;
interpolate(p1, p2, t, p_pre);
- Vector7s p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
+ Vector7d 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;
+ Vector7d p1, p2, p_pre;
p1 << 1,2,3, 0,0,0,1;
p2 << 2,4,6, 0,0,0,1;
- Scalar t = 0.2;
+ double t = 0.2;
interpolate(p1, p2, t, p_pre);
- Vector7s p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
+ Vector7d 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;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 0,0,0, 1,0,0,0;
- Scalar t = 0.5;
+ double 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;
+ Vector7d 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;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 0,0,0, 0,1,0,0;
- Scalar t = 0.5;
+ double 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;
+ Vector7d 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;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 0,0,0, 0,0,1,0;
- Scalar t = 0.5;
+ double 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;
+ Vector7d 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;
+ Vector7d p1, p2, dp, p_pre, p_gt;
+ Vector3d da;
da.setRandom(); da /= 5;
@@ -168,7 +325,7 @@ TEST(SE3, interpolate_I_qxyz)
dp << 0,0,0, exp_q(da).coeffs();
p_gt = compose(p1, dp);
p2 = compose(p_gt, dp);
- Scalar t = 0.5;
+ double t = 0.5;
interpolate(p1, p2, t, p_pre);
@@ -177,20 +334,20 @@ TEST(SE3, interpolate_I_qxyz)
TEST(SE3, interpolate_half)
{
- Vector7s p1, p2, dp, p_pre, p_gt;
- Vector6s da;
+ Vector7d p1, p2, dp, p_pre, p_gt;
+ Vector6d da;
p1.setRandom(); p1.tail(4).normalize();
da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
+ dp << exp(da);
// compose double, then interpolate 1/2
p_gt = compose(p1, dp);
p2 = compose(p_gt, dp);
- Scalar t = 0.5;
+ double t = 0.5;
interpolate(p1, p2, t, p_pre);
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
@@ -198,13 +355,13 @@ TEST(SE3, interpolate_half)
TEST(SE3, interpolate_third)
{
- Vector7s p1, p2, dp, dp2, dp3, p_pre, p_gt;
- Vector6s da;
+ Vector7d p1, p2, dp, dp2, dp3, p_pre, p_gt;
+ Vector6d da;
p1.setRandom(); p1.tail(4).normalize();
da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
+ dp << exp(da);
dp2 = compose(dp, dp);
dp3 = compose(dp2, dp);
@@ -213,7 +370,7 @@ TEST(SE3, interpolate_third)
p_gt = compose(p1, dp);
p2 = compose(p1, dp3);
- Scalar t = 1.0/3.0;
+ double t = 1.0/3.0;
interpolate(p1, p2, t, p_pre);
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
@@ -221,11 +378,11 @@ TEST(SE3, interpolate_third)
TEST(SE3, toSE3_I)
{
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
- Matrix4s R;
+ Vector7d pose; pose << 0,0,0, 0,0,0,1;
+ Matrix4d R;
toSE3(pose, R);
- ASSERT_MATRIX_APPROX(R, Matrix4s::Identity(), 1e-8);
+ ASSERT_MATRIX_APPROX(R, Matrix4d::Identity(), 1e-8);
}
int main(int argc, char **argv)
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 8bb84231d13a800845d80703025e2c382d187f8b..3a0cbc4555c88bd40f0a656e2a73ff2a619654b6 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/capture/capture_base.h"
#include "core/state_block/state_angle.h"
@@ -59,7 +59,7 @@ 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
+ SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StateBlock>(2), std::make_shared<StateAngle>(), std::make_shared<StateBlock>(2), 2, true, true, true)); // last 3 'true' mark dynamic
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
@@ -76,32 +76,19 @@ TEST(CaptureBase, Dynamic_sensor_params)
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()));
- auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2s::Zero(), Matrix2s::Identity());
+ // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2d::Zero(), Matrix2d::Identity()));
+ auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
ASSERT_FALSE(C->getFeatureList().empty());
ASSERT_EQ(C->getFeatureList().front(), f);
}
-TEST(CaptureBase, addFeatureList)
+TEST(CaptureBase, print)
{
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()));
- auto f_first = FeatureBase::emplace<FeatureBase>(C, "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 // EDIT 02-2019: features have been copied
- ASSERT_EQ(C->getFeatureList().front(), f_first);
- ASSERT_EQ(C->getFeatureList().back(), f_last);
+ // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2d::Zero(), Matrix2d::Identity()));
+ auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
+
+ C->print(4, 1, 1, 1);
}
TEST(CaptureBase, process)
@@ -113,6 +100,93 @@ TEST(CaptureBase, process)
ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
}
+TEST(CaptureBase, move_from_F_to_KF)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+ auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 1);
+ ASSERT_EQ(F->getCaptureList().size(), 1);
+
+ C->move(KF);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 2);
+ ASSERT_EQ(F->getCaptureList().size(), 0);
+ ASSERT_TRUE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_F_to_null)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ FrameBasePtr F0 = nullptr;
+
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
+
+ ASSERT_EQ(F->getCaptureList().size(), 1);
+
+ C->move(F0);
+
+ ASSERT_EQ(F->getCaptureList().size(), 0);
+ ASSERT_FALSE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_null_to_KF)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+ auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+ auto C = std::make_shared<CaptureBase>("Dummy", 0.0);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 1);
+
+ C->move(KF);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 2);
+ ASSERT_TRUE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_null_to_F)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+ auto C = std::make_shared<CaptureBase>("Dummy", 0.0);
+
+ C->move(F);
+
+ ASSERT_EQ(F->getCaptureList().size(), 1);
+
+ ASSERT_FALSE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_KF_to_F)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+ auto C = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+ ASSERT_DEATH(C->move(F), "");
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
deleted file mode 100644
index 4ab28374d25f09b63cceb7fa709ddfa9367ec347..0000000000000000000000000000000000000000
--- a/test/gtest_ceres_manager.cpp
+++ /dev/null
@@ -1,635 +0,0 @@
-/*
- * gtest_ceres_manager.cpp
- *
- * Created on: Jun, 2018
- * Author: jvallve
- */
-
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
-
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/capture/capture_void.h"
-#include "core/factor/factor_pose_2D.h"
-#include "core/factor/factor_quaternion_absolute.h"
-#include "core/solver/solver_manager.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/state_block/local_parametrization_angle.h"
-#include "core/state_block/local_parametrization_quaternion.h"
-
-#include "ceres/ceres.h"
-
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-WOLF_PTR_TYPEDEFS(CeresManagerWrapper);
-class CeresManagerWrapper : public CeresManager
-{
- public:
- CeresManagerWrapper(const ProblemPtr& wolf_problem) :
- CeresManager(wolf_problem)
- {
- };
-
- bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st)
- {
- return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st)
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st)
- {
- return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- int numStateBlocks()
- {
- return ceres_problem_->NumParameterBlocks();
- };
-
- int numFactors()
- {
- return ceres_problem_->NumResidualBlocks();
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
- {
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
- state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
- ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
- };
-
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end();
- };
-
-};
-
-TEST(CeresManager, Create)
-{
- ProblemPtr P = Problem::create("PO", 2);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // check double ointers to branches
- ASSERT_EQ(P, ceres_manager_ptr->getProblem());
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, AddStateBlock)
-{
- ProblemPtr P = Problem::create("PO", 2);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create State block
- 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", 2);
- 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", 2);
- 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", 2);
- 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", 2);
- 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", 2);
- 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", 2);
- 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", 2);
- 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", 2);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,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", 2);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,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", 2);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,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", 2);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
-
- // remove factor
- P->removeFactor(c);
-
- ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
-
- // update solver
- ceres_manager_ptr->update();
-
- // check factor
- ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
-
- // run ceres manager check
- ceres_manager_ptr->check();
-}
-
-TEST(CeresManager, DoubleRemoveFactor)
-{
- ProblemPtr P = Problem::create("PO", 2);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,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", 2);
- 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", 2);
- 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", 2);
- 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", 3);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, 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", 3);
- CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, 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_converter.cpp b/test/gtest_converter.cpp
index 69bb7ce355646daadaf3b91a1e816e0f0c163587..d068e024a3f9145f347a8645d7ba52dedd036c38 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
using namespace std;
@@ -20,6 +20,13 @@ TEST(Converter, ParseToVector)
ASSERT_EQ(v[8],11);
vector<string> vs {"a","b","c"};
ASSERT_EQ(converter<string>::convert(vs), "[a,b,c]");
+ string v2 = "[first,second,third,fourth]";
+ vector<string> vv = converter<vector<string>>::convert(v2);
+ ASSERT_EQ(vv.size(),4);
+ ASSERT_EQ(vv[0],"first");
+ ASSERT_EQ(vv[1],"second");
+ ASSERT_EQ(vv[2],"third");
+ ASSERT_EQ(vv[3],"fourth");
}
TEST(Converter, ParseToEigenMatrix)
@@ -32,6 +39,14 @@ TEST(Converter, ParseToEigenMatrix)
EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v2);}()), std::runtime_error);
string v3 = "[[3],3,4,5,6,7,8,9,10,11]";
EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v3);}()), std::runtime_error);
+ string v4 = "[[3,3],3,4,5,6,7,8,9,10,11]";
+ auto v41 = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>();
+ v41 = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v4);
+ ASSERT_EQ(v41(0,0), 3);
+ ASSERT_EQ(v41(1,1), 7);
+ ASSERT_EQ(v41(2,2), 11);
+ string v5 = "[[3,3],3,4,5,6,7,[8,9],10,11]";
+ EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v5);}()), std::runtime_error);
}
TEST(Converter, ParseToMap)
{
@@ -42,7 +57,24 @@ TEST(Converter, ParseToMap)
map<string, vector<int>> m = {{"x",vector<int>{1,2}}, {"y",vector<int>{}}, {"z",vector<int>{3}}};
ASSERT_EQ(converter<string>::convert(m), "[{x:[1,2]},{y:[]},{z:[3]}]");
}
+TEST(Converter, extraTests)
+{
+ string str = "[{x:1},{y:[[{x:[1,2]},{y:[]},{z:[3]}]]},{z:[1,2,3,4,5]}]";
+ auto v = converter<vector<string>>::convert(str);
+ ASSERT_EQ(v[0], "{x:1}");
+ ASSERT_EQ(v[1], "{y:[[{x:[1,2]},{y:[]},{z:[3]}]]}");
+ ASSERT_EQ(v[2], "{z:[1,2,3,4,5]}");
+ string str2 = "[]";
+ auto v2 = converter<vector<string>>::convert(str2);
+ // EXPECT_EQ(v2.size(), 1);
+ EXPECT_TRUE(v2.empty());
+}
+TEST(Converter, noGeneralConvert)
+{
+ class DUMMY{};
+ EXPECT_THROW(([=]{converter<DUMMY>::convert("Should fail");}()), std::runtime_error);
+}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index c7c68b4f3a2df00fab257fb4f47646d429f361be..036bc2a4b80ef4eeb1a38b146096ff1f1cd81487 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -1,14 +1,14 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/eigen_predicates.h"
TEST(TestEigenPredicates, TestEigenDynPredZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd 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.);
+ A = Eigen::MatrixXd::Zero(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
+ C = Eigen::MatrixXd::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));
@@ -19,11 +19,11 @@ TEST(TestEigenPredicates, TestEigenDynPredZero)
TEST(TestEigenPredicates, TestEigenFixPredZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Zero();
- B = Eigen::Matrix4s::Random();
- C = Eigen::Matrix4s::Ones() * (wolf::Constants::EPS/2.);
+ A = Eigen::Matrix4d::Zero();
+ B = Eigen::Matrix4d::Random();
+ C = Eigen::Matrix4d::Ones() * (wolf::Constants::EPS/2.);
EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
@@ -34,10 +34,10 @@ TEST(TestEigenPredicates, TestEigenFixPredZero)
TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
+ A = Eigen::MatrixXd::Random(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
C = A;
EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
@@ -48,10 +48,10 @@ TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
+ A = Eigen::Matrix4d::Random();
+ B = Eigen::Matrix4d::Random();
C = A;
EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
@@ -62,10 +62,10 @@ TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
+ A = Eigen::MatrixXd::Random(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
C = A;
EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
@@ -76,10 +76,10 @@ TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
+ A = Eigen::Matrix4d::Random();
+ B = Eigen::Matrix4d::Random();
C = A;
EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
@@ -90,10 +90,10 @@ TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
+ A = Eigen::MatrixXd::Random(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
C = A;
EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
@@ -104,10 +104,10 @@ TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
+ A = Eigen::Matrix4d::Random();
+ B = Eigen::Matrix4d::Random();
C = A;
EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
@@ -118,13 +118,13 @@ TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
{
- Eigen::Quaternions A, B, C;
+ Eigen::Quaterniond A, B, C;
/// @todo which version of Eigen provides this ?
-// A = Eigen::Quaternions::UnitRandom();
+// A = Eigen::Quaterniond::UnitRandom();
- A.coeffs() = Eigen::Vector4s::Random().normalized();
- B.coeffs() = Eigen::Vector4s::Random().normalized();
+ A.coeffs() = Eigen::Vector4d::Random().normalized();
+ B.coeffs() = Eigen::Vector4d::Random().normalized();
C = A;
EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS));
@@ -135,10 +135,10 @@ TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
{
- Eigen::Quaternions A, B;
+ Eigen::Quaterniond A, B;
- A = Eigen::Quaternions::Identity();
- B.coeffs() = Eigen::Vector4s::Random().normalized();
+ A = Eigen::Quaterniond::Identity();
+ B.coeffs() = Eigen::Vector4d::Random().normalized();
EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS));
EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS));
@@ -148,9 +148,9 @@ TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
TEST(TestEigenPredicates, TestEigenPredAngleIsApprox)
{
- wolf::Scalar a = M_PI;
- wolf::Scalar b = -M_PI;
- wolf::Scalar c = 0;
+ double a = M_PI;
+ double b = -M_PI;
+ double 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));
@@ -160,9 +160,9 @@ TEST(TestEigenPredicates, TestEigenPredAngleIsApprox)
TEST(TestEigenPredicates, TestEigenPredAngleIsZero)
{
- wolf::Scalar a = 0;
- wolf::Scalar b = M_PI;
- wolf::Scalar c = 2.*M_PI;
+ double a = 0;
+ double b = M_PI;
+ double c = 2.*M_PI;
EXPECT_TRUE(wolf::pred_angle_zero(a, wolf::Constants::EPS));
EXPECT_FALSE(wolf::pred_angle_zero(b, wolf::Constants::EPS));
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index eb4b405f46a0cf0b429171f1c8c51b050321fe4f..f790bc57f70c9ab2d77e4b6fb9b2bcb822912d8c 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -5,17 +5,18 @@
* Author: jcasals
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/feature/feature_odom_2D.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/processor/processor_odom_3d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/feature/feature_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "dummy/processor_tracker_feature_dummy.h"
#include <iostream>
@@ -42,19 +43,24 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
TEST(Emplace, Processor)
{
- ProblemPtr P = Problem::create("POV", 3);
+ ProblemPtr P = Problem::create("PO", 2);
auto sen = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
- auto prc = ProcessorOdom2D::emplace<ProcessorOdom2D>(sen, std::make_shared<ProcessorParamsOdom2D>());
+ auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ParamsProcessorOdom2d>());
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem());
ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor());
ASSERT_EQ(prc, sen->getProcessorList().front());
+
+ SensorBasePtr sen2 = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
+ ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ParamsProcessorOdom2d>());
+ ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor());
+ ASSERT_EQ(prc2, sen2->getProcessorList().front());
}
TEST(Emplace, Capture)
@@ -62,12 +68,12 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
}
@@ -76,19 +82,17 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
- auto cov = Eigen::MatrixXs(2,2);
- cov(0,0) = 1;
- cov(1,1) = 1;
- FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(5), cov);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXd(2), cov);
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
}
TEST(Emplace, Factor)
@@ -96,21 +100,19 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
- auto cov = Eigen::MatrixXs(2,2);
- cov(0,0) = 1;
- cov(1,1) = 1;
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
- auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
+ auto cnt = FactorBase::emplace<FactorRelativePose2d>(ftr, ftr, frm, nullptr, false, TOP_MOTION);
ASSERT_NE(nullptr, ftr->getFactorList().front().get());
}
@@ -118,29 +120,39 @@ TEST(Emplace, EmplaceDerived)
{
ProblemPtr P = Problem::create("POV", 3);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- // LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
- auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
- auto cov = Eigen::MatrixXs(2,2);
- cov(0,0) = 1;
- cov(1,1) = 1;
- auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov, frm);
- auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
- auto m = Eigen::Matrix<Scalar,9,6>();
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
+ auto m = Eigen::Matrix<double,9,6>();
for(int i = 0; i < 9; i++)
for(int j = 0; j < 6; j++)
m(i,j) = 1;
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
+ auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
}
TEST(Emplace, Nullpointer)
{
CaptureBase::emplace<wolf::CaptureBase>(nullptr, "DUMMY", 1.2, nullptr);
}
+TEST(Emplace, ReturnDerived)
+{
+ ProblemPtr P = Problem::create("POV", 3);
+
+ ASSERT_NE(P->getTrajectory(), nullptr);
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
+ auto cnt = FactorBase::emplace<FactorRelativePose2d>(ftr, ftr, frm, nullptr, false, TOP_MOTION);
+
+ FactorRelativePose2dPtr fac = FactorBase::emplace<FactorRelativePose2d>(ftr, ftr, frm, nullptr, false, TOP_MOTION);
+
+}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
-}
\ No newline at end of file
+}
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 0cadea4ffe640770290f82ea4b5e17542468d404..65d779ffd8d244bb795181fef71eb978f613ef6c 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
TEST(TestTest, DummyTestExample)
{
@@ -16,5 +16,7 @@ TEST(TestTest, DummyTestExample)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "TestTest.DummyTestExample"; // Test only this one
+ //::testing::GTEST_FLAG(filter) = "TestTest.*"; // Test only the tests in this group
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index da1cc8c050276e2e5f349773493027d0a96b0111..e67a2f7407f10330a3cf30c37ed1f35d8bcbeaf5 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -5,41 +5,41 @@
* \author: datchuth
*/
-#include "utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/utils/utils_gtest.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
#include "core/capture/capture_motion.h"
-#include "core/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
using std::cout;
using std::endl;
-Vector10s pose9toPose10(Vector9s _pose9)
+Vector10d pose9toPose10(Vector9d _pose9)
{
- return (Vector10s() << _pose9.head<3>() , v2q(_pose9.segment<3>(3)).coeffs(), _pose9.tail<3>()).finished();
+ return (Vector10d() << _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();
+Vector9d pose(Vector9d::Random());
+Vector10d pose10 = pose9toPose10(pose);
+Eigen::Matrix<double, 9, 9> data_cov = 0.2 * Eigen::Matrix<double,9,9>::Identity();
// perturbated priors
-Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
+Vector10d x0 = pose9toPose10(pose + Vector9d::Random()*0.25);
// Problem and solver
ProblemPtr problem_ptr = Problem::create("POV", 3);
-CeresManager ceres_mgr(problem_ptr);
+SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, problem_ptr->stateZero() );
-// Capture, feature and factor
-// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
+// Capture
+// auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, nullptr);
////////////////////////////////////////////////////////
/* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine
@@ -49,10 +49,8 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos
TEST(FactorBlockAbs, fac_block_abs_p)
{
- // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
- // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP());
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -61,7 +59,7 @@ TEST(FactorBlockAbs, fac_block_abs_p)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6);
@@ -69,17 +67,15 @@ TEST(FactorBlockAbs, fac_block_abs_p)
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>()));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
- // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
// Unfix frame 0, perturb frm0
frm0->unfix();
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
@@ -87,10 +83,8 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
TEST(FactorBlockAbs, fac_block_abs_v)
{
- // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
- auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
- // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV());
+ auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureVelocity", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getV(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -99,7 +93,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6);
@@ -107,10 +101,8 @@ TEST(FactorBlockAbs, fac_block_abs_v)
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)));
- auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
- // fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
- FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO());
+ auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureQuaternion", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
+ FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0, fea0->getFrame()->getO(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -119,7 +111,7 @@ TEST(FactorQuatAbs, fac_block_abs_o)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index efa61acc9cc027c965aca8f8f27ac7f28e2fde58..c0cbacdec10e1fddd49db23a3c536622626b21df 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -5,44 +5,372 @@
* Author: jvallve
*/
-#include "utils_gtest.h"
+#include "dummy/factor_odom_2d_autodiff.h"
+#include "dummy/factor_dummy_zero_1.h"
+#include "dummy/factor_dummy_zero_12.h"
-#include "core/sensor/sensor_odom_2D.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/sensor_odom_2d.h"
#include "core/capture/capture_void.h"
-#include "core/feature/feature_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/factor/factor_odom_2D_analytic.h"
+#include "core/feature/feature_odom_2d.h"
#include "core/factor/factor_autodiff.h"
+
using namespace wolf;
using namespace Eigen;
-TEST(CaptureAutodiff, ConstructorOdom2d)
+
+TEST(FactorAutodiff, AutodiffDummy1)
{
- 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)));
+ StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
+
+ auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
+
+ // COMPUTE JACOBIANS
+ std::vector<const double*> states_ptr({sb->getStateData()});
+
+ std::vector<Eigen::MatrixXd> J;
+ Eigen::VectorXd residuals(fac->getSize());
+ fac->evaluate(states_ptr, residuals, J);
+
+ ASSERT_MATRIX_APPROX(J[0], Eigen::Matrix1d::Ones(), wolf::Constants::EPS);
+}
+
+TEST(FactorAutodiff, AutodiffDummy12)
+{
+ StateBlockPtr sb1 = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateBlock>(Eigen::Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateBlock>(Eigen::Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateBlock>(Eigen::Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateBlock>(Eigen::Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateBlock>(Eigen::Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateBlock>(Eigen::Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateBlock>(Eigen::Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateBlock>(Eigen::Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateBlock>(Eigen::Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(12));
+
+ std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
+ std::vector<Eigen::MatrixXd> J;
+ Eigen::VectorXd residuals;
+ unsigned int i;
+ FactorBasePtr fac = nullptr;
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
+
+ // 1 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 2 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 3 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 4 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 5 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 6 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 7 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 8 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 9 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 10 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 11 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 12 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+}
+
+TEST(FactorAutodiff, EmplaceOdom2d)
+{
+
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1));
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2));
// SENSOR
- IntrinsicsOdom2D intrinsics_odo;
+ ParamsSensorOdom2d 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);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
// FACTOR
- // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(factor_ptr);
- // fr1_ptr->addConstrainedBy(factor_ptr);
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
ASSERT_TRUE(factor_ptr->getFeature());
ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
@@ -51,129 +379,118 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
ASSERT_TRUE(factor_ptr->getFrameOther());
}
-TEST(CaptureAutodiff, ResidualOdom2d)
+TEST(FactorAutodiff, ResidualOdom2d)
{
- Eigen::Vector3s f1_pose, f2_pose;
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(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>())));
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
- IntrinsicsOdom2D intrinsics_odo;
+ ParamsSensorOdom2d 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);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3s d;
+ Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(factor_ptr);
- // fr1_ptr->addConstrainedBy(factor_ptr);
-
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// 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();
+ Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
- std::vector<Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
+ std::vector<double*> 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());
+ Eigen::VectorXd 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)
+TEST(FactorAutodiff, JacobianOdom2d)
{
- Eigen::Vector3s f1_pose, f2_pose;
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(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>())));
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
- IntrinsicsOdom2D intrinsics_odo;
+ ParamsSensorOdom2d 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);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3s d;
+ Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(factor_ptr);
- // fr1_ptr->addConstrainedBy(factor_ptr);
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// 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();
+ const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXd 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<const double*> 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());
+ std::vector<Eigen::MatrixXd> Jauto;
+ Eigen::VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(states_ptr, residuals, Jauto);
- std::cout << Jauto.size() << std::endl;
+ ASSERT_EQ(Jauto.size(),4);
// ANALYTIC JACOBIANS
- Eigen::MatrixXs J0(3,2);
+ Eigen::MatrixXd 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);
+ Eigen::MatrixXd 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);
+ Eigen::MatrixXd 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);
+ Eigen::MatrixXd 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);
@@ -188,64 +505,55 @@ TEST(CaptureAutodiff, JacobianOdom2d)
// std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl;
}
-TEST(CaptureAutodiff, AutodiffVsAnalytic)
+TEST(FactorAutodiff, AutodiffVsAnalytic)
{
- Eigen::Vector3s f1_pose, f2_pose;
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(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>())));
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
- IntrinsicsOdom2D intrinsics_odo;
+ ParamsSensorOdom2d 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);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3s d;
+ Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- // FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(fac_autodiff_ptr);
- // fr1_ptr->addConstrainedBy(fac_autodiff_ptr);
- auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_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);
- auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr);
+ auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_analytic_ptr = FactorBase::emplace<FactorRelativePose2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false, TOP_MOTION);
// COMPUTE JACOBIANS
+ const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
- 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 double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
- 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());
+ std::vector<Eigen::MatrixXd> Jautodiff, Janalytic;
+ Eigen::VectorXd 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);
+ fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+ std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
+
+ ASSERT_EQ(Janalytic.size(), Jautodiff.size());
+ for (auto i = 0; i < Jautodiff.size(); i++)
+ ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
deleted file mode 100644
index c605a558d48b12a0f4dad46ad718f929f1bbc62c..0000000000000000000000000000000000000000
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/**
- * \file gtest_factor_autodiff_distance_3D.cpp
- *
- * Created on: Apr 10, 2018
- * \author: jsola
- */
-
-#include "core/factor/factor_autodiff_distance_3D.h"
-#include "core/problem/problem.h"
-#include "core/utils/logging.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/math/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", 3);
- ceres_manager = std::make_shared<CeresManager>(problem);
-
- F1 = problem->emplaceFrame (KEY, pose1, 1.0);
-
- F2 = problem->emplaceFrame (KEY, pose2, 2.0);
- C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
- f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = std::static_pointer_cast<FactorAutodiffDistance3D>(FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE));
-
- }
-
-};
-
-TEST_F(FactorAutodiffDistance3D_Test, ground_truth)
-{
- Scalar res;
-
- c2->operator ()(pos1.data(), pos2.data(), &res);
-
- ASSERT_NEAR(res, 0.0, 1e-5);
-}
-
-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-5);
-}
-
-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_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..477520c0f0199b6c3d2c6ea2eba031122cc11561
--- /dev/null
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -0,0 +1,110 @@
+/**
+ * \file gtest_factor_autodiff_distance_3d.cpp
+ *
+ * Created on: Apr 10, 2018
+ * \author: jsola
+ */
+
+#include "core/factor/factor_distance_3d.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/problem/problem.h"
+
+#include "core/math/rotations.h"
+
+#include "core/utils/utils_gtest.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+class FactorAutodiffDistance3d_Test : public testing::Test
+{
+ public:
+ Vector3d pos1, pos2;
+ Vector3d euler1, euler2;
+ Quaterniond quat1, quat2;
+ Vector4d vquat1, vquat2; // quaternions as vectors
+ Vector7d pose1, pose2;
+
+ FrameBasePtr F1, F2;
+ CaptureBasePtr C2;
+ FeatureBasePtr f2;
+ FactorDistance3dPtr c2;
+
+ Vector1d dist;
+ Matrix1d dist_cov;
+
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+
+ void SetUp() override
+ {
+ 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 = Vector1d(sqrt(2.0));
+ dist_cov = Matrix1d(0.01);
+
+ problem = Problem::create("PO", 3);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ F1 = problem->emplaceFrame (1.0, pose1);
+
+ F2 = problem->emplaceFrame (2.0, pose2);
+ C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
+ }
+
+};
+
+TEST_F(FactorAutodiffDistance3d_Test, ground_truth)
+{
+ double res;
+
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2->operator ()(pos1.data(), pos2.data(), &res);
+
+ ASSERT_NEAR(res, 0.0, 1e-5);
+}
+
+TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
+{
+ double measurement = 1.400;
+
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+
+ double res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
+
+ double res;
+ c2->operator ()(pos1.data(), pos2.data(), &res);
+
+ ASSERT_NEAR(res, res_expected, 1e-5);
+}
+
+TEST_F(FactorAutodiffDistance3d_Test, solve)
+{
+ double measurement = 1.400;
+
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET);
+
+ // Check distance between F1 and F2 positions -- must match the measurement
+ ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f861114e75bb24eceae0f747780bd89d2e1d3579
--- /dev/null
+++ b/test/gtest_factor_base.cpp
@@ -0,0 +1,135 @@
+/*
+ * gtest_factor_base.cpp
+ *
+ * Created on: Apr 2, 2020
+ * Author: jsola
+ */
+
+
+#include "core/utils/utils_gtest.h"
+#include "core/factor/factor_base.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+class FactorBaseTest : public testing::Test
+{
+ public:
+ FrameBasePtr F0,F1;
+ CaptureBasePtr C0,C1;
+ FeatureBasePtr f0,f1;
+ LandmarkBasePtr L0,L1;
+
+ void SetUp() override
+ {
+ F0 = std::make_shared<FrameBase>(0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(1.0, nullptr);
+ C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
+ C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
+ f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ f1 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ L0 = std::make_shared<LandmarkBase>("Lmk", nullptr);
+ L1 = std::make_shared<LandmarkBase>("Lmk", nullptr);
+ }
+// virtual void TearDown(){}
+};
+
+class FactorDummy : public FactorBase
+{
+ public:
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtr& _frame_other,
+ const CaptureBasePtr& _capture_other,
+ const FeatureBasePtr& _feature_other,
+ const LandmarkBasePtr& _landmark_other) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other,
+ _capture_other,
+ _feature_other,
+ _landmark_other,
+ nullptr,
+ false)
+ {
+ //
+ }
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ nullptr,
+ false)
+ {
+ //
+ }
+ ~FactorDummy() override = default;
+
+ bool evaluate(double const* const* _parameters,
+ double* _residuals,
+ double** _jacobians) const override {return true;}
+ void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& _residual,
+ std::vector<Eigen::MatrixXd>& _jacobians) const override {}
+ JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
+ std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
+ std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
+ unsigned int getSize() const override {return 0;}
+
+};
+
+TEST_F(FactorBaseTest, constructor_from_pointers)
+{
+ FactorDummy fac(f0,nullptr,C0,f1,nullptr);
+
+ ASSERT_TRUE(fac.getFrameOtherList().empty());
+
+ ASSERT_EQ(fac.getCaptureOtherList().size(), 1);
+
+ ASSERT_EQ(fac.getFeatureOtherList().size(), 1);
+
+ ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
+}
+
+TEST_F(FactorBaseTest, constructor_from_lists)
+{
+ FactorDummy fac(f0,{},{C0},{f0,f1},{});
+
+ ASSERT_TRUE(fac.getFrameOtherList().empty());
+
+ ASSERT_EQ(fac.getCaptureOtherList().size(), 1);
+
+ ASSERT_EQ(fac.getFeatureOtherList().size(), 2);
+
+ ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c314c74cefc9b901267b5b3e78c704c521a7f1b9
--- /dev/null
+++ b/test/gtest_factor_block_difference.cpp
@@ -0,0 +1,205 @@
+/**
+ * \file gtest_factor_block_difference.cpp
+ *
+ * Created on: Feb 29, 2020
+ * \author: mfourmy
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/problem/problem.h"
+#include "core/frame/frame_base.h"
+#include "core/capture/capture_base.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_block_difference.h"
+#include "core/factor/factor_block_absolute.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+
+const Vector6d zero6 = Vector6d::Zero();
+const Vector3d zero3 = zero6.head<3>();
+const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
+
+class FixtureFactorBlockDifference : public testing::Test
+{
+ public:
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ FrameBasePtr KF0_;
+ FrameBasePtr KF1_;
+ CaptureBasePtr Cap_;
+ FeatureBasePtr Feat_;
+ FactorBlockDifferencePtr Fac_;
+
+ protected:
+
+ void SetUp() override
+ {
+ // Problem and solver
+ problem_ = Problem::create("POV", 3);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+
+ TimeStamp t0(0);
+ TimeStamp t1(1);
+
+ // Vector10d x_origin = problem_->stateZero().vector("POV");
+ VectorComposite x_origin(problem_->stateZero().vector("POV"), "POV", {3, 4, 3});
+ // Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
+ VectorComposite cov_prior("POV", {Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))});
+ KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0, 0.1);
+
+ //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
+ //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
+ //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
+
+ KF1_ = problem_->emplaceFrame(t1, problem_->stateZero());
+
+ Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
+ }
+
+ void TearDown() override {}
+};
+
+TEST_F(FixtureFactorBlockDifference, CheckFactorType)
+{
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
+ ASSERT_EQ(Fac_->getType(), "FactorBlockDifference");
+}
+
+
+TEST_F(FixtureFactorBlockDifference, EqualP)
+{
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
+
+ // perturbate
+ KF0_->getP()->setState(Vector3d::Random());
+ KF1_->getP()->setState(Vector3d::Random());
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
+}
+
+TEST_F(FixtureFactorBlockDifference, EqualV)
+{
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
+
+ // perturbate
+ KF0_->getV()->setState(Vector3d::Random());
+ KF1_->getV()->setState(Vector3d::Random());
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8);
+}
+
+TEST_F(FixtureFactorBlockDifference, DiffP)
+{
+ Vector3d diff = Vector3d::Random();
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
+
+ // perturbate
+ KF0_->getP()->setState(Vector3d::Random());
+ KF1_->getP()->setState(Vector3d::Random());
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diff, 1e-8);
+}
+
+TEST_F(FixtureFactorBlockDifference, DiffV)
+{
+ Vector3d diff = Vector3d::Random();
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
+
+ // perturbate
+ KF0_->getV()->setState(Vector3d::Random());
+ KF1_->getV()->setState(Vector3d::Random());
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diff, 1e-8);
+}
+
+
+TEST_F(FixtureFactorBlockDifference, DiffPx)
+{
+ // Vector3d diff = Vector3d::Random();
+ Vector1d diff; diff << 1; // measurement still of the same size as the constrained state
+ Matrix1d cov_diff = 1e-4 * Matrix1d::Identity(); // measurement still of the same size as the constrained state
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 1, 0, 1);
+
+ // perturbate
+ KF0_->getP()->setState(Vector3d::Random());
+ KF1_->getP()->setState(Vector3d::Random());
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<1>() - KF0_->getP()->getState().head<1>(), diff.head<1>(), 1e-8);
+}
+
+TEST_F(FixtureFactorBlockDifference, DiffPxy)
+{
+ // Vector3d diff = Vector3d::Random();
+ Vector2d diff; diff << 1, 2; // measurement still of the same size as the constrained state
+ Matrix2d cov_diff = 1e-4 * Matrix2d::Identity(); // measurement still of the same size as the constrained state
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 2, 0, 2);
+
+ // perturbate
+ KF0_->getP()->setState(Vector3d::Random());
+ KF1_->getP()->setState(Vector3d::Random());
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<2>() - KF0_->getP()->getState().head<2>(), diff, 1e-8);
+}
+
+TEST_F(FixtureFactorBlockDifference, DiffPyz)
+{
+ // Vector3d diff = Vector3d::Random();
+ Vector2d diff; diff << 1, 2; // measurement still of the same size as the constrained state
+ Matrix2d cov_diff = 1e-4 * Matrix2d::Identity(); // measurement still of the same size as the constrained state
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 1, 2, 1, 2);
+
+ // perturbate
+ KF0_->getP()->setState(Vector3d::Random());
+ KF1_->getP()->setState(Vector3d::Random());
+
+ problem_->print(4,true,true,true);
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+ problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KF1_->getP()->getState().segment<2>(1) - KF0_->getP()->getState().segment<2>(1), diff, 1e-8);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "TestTest.DummyTestExample"; // Test only this one
+ //::testing::GTEST_FLAG(filter) = "TestTest.*"; // Test only the tests in this group
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2e7783be9bbb602c9e9e5376c1f13426234d4d9
--- /dev/null
+++ b/test/gtest_factor_diff_drive.cpp
@@ -0,0 +1,669 @@
+/**
+ * \file gtest_factor_diff_drive.cpp
+ *
+ * Created on: Jul 24, 2019
+ * \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/processor/processor_diff_drive.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/factor/factor_diff_drive.h"
+#include "core/frame/frame_base.h"
+#include "core/capture/capture_motion.h"
+#include "core/feature/feature_motion.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorDiffDrivePublic);
+
+class ProcessorDiffDrivePublic : public ProcessorDiffDrive
+{
+ using Base = ProcessorDiffDrive;
+ public:
+ ProcessorDiffDrivePublic(ParamsProcessorDiffDrivePtr _params_motion) :
+ ProcessorDiffDrive(_params_motion)
+ {
+ //
+ }
+ void configure(SensorBasePtr _sensor) override
+ {
+ Base::configure(_sensor);
+ }
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override
+ {
+ Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
+ }
+
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+ }
+
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+ }
+
+ VectorXd getCalibration (const CaptureBasePtr _capture) const override
+ {
+ return Base::getCalibration(_capture);
+ }
+
+ Eigen::VectorXd deltaZero() const override
+ {
+ return Base::deltaZero();
+ }
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override
+ {
+ return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
+ }
+
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+ {
+ return Base::emplaceFeature(_capture_own);
+ }
+
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin) override
+ {
+ return Base::emplaceFactor(_feature_motion, _capture_origin);
+ }
+
+ ParamsProcessorDiffDrivePtr getParams()
+ {
+ return params_prc_diff_drv_selfcal_;
+ }
+
+ double getRadPerTick()
+ {
+ return radians_per_tick_;
+ }
+
+};
+
+
+
+class FactorDiffDriveTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ TrajectoryBasePtr trajectory;
+ ParamsSensorDiffDrivePtr intr;
+ SensorDiffDrivePtr sensor;
+ ParamsProcessorDiffDrivePtr params;
+ ProcessorDiffDrivePublicPtr processor;
+ FrameBasePtr F0, F1;
+ CaptureMotionPtr C0, C1;
+ FeatureMotionPtr f1;
+ FactorDiffDrivePtr c1;
+
+ Vector2d data0, data1;
+ Matrix2d data_cov;
+ Vector3d delta0, delta1;
+ Matrix3d delta0_cov, delta1_cov;
+ Vector3d calib;
+ Vector3d residual;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ trajectory = problem->getTrajectory();
+
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3d extr(0, 0, 0);
+ auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
+ sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ calib = sensor->getIntrinsic()->getState();
+
+ // params and processor
+ params = std::make_shared<ParamsProcessorDiffDrive>();
+ params->angle_turned = 1;
+ params->dist_traveled = 2;
+ params->max_buff_length = 3;
+ auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
+ processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+ // frames
+ F0 = FrameBase::emplace<FrameBase>(trajectory,
+ 0.0,
+ "PO",
+ std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
+ F1 = FrameBase::emplace<FrameBase>(trajectory,
+ 1.0,
+ "PO",
+ std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
+
+ // captures
+ C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
+ 0.0,
+ sensor,
+ data0.Zero(),
+ data_cov,
+ nullptr);
+
+ data1 << 100/(2*M_PI), 100/(2*M_PI); // we go straight -- this can be changed later on
+ data_cov.setIdentity();
+ delta1 << 1,0,0;
+ delta1_cov.setIdentity();
+
+ C1 = CaptureBase::emplace<CaptureDiffDrive>(F1,
+ 1.0,
+ sensor,
+ data1.Zero(),
+ data_cov,
+ nullptr);
+
+
+ // feature
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta1,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
+
+ // final checks
+ startup_config_for_all_tests();
+ }
+
+ void TearDown() override
+ {
+ }
+
+ void startup_config_for_all_tests()
+ {
+ ASSERT_NE(sensor, nullptr);
+ ASSERT_FALSE(sensor->getIntrinsic()->isFixed());
+
+ ASSERT_NE(processor, nullptr);
+
+ ASSERT_NE(C0, nullptr);
+
+ ASSERT_NE(f1, nullptr);
+ ASSERT_MATRIX_APPROX(f1->getMeasurement(), delta1, 1e-6);
+ ASSERT_MATRIX_APPROX(f1->getMeasurementCovariance(), delta1_cov, 1e-6);
+ ASSERT_MATRIX_APPROX(f1->getJacobianCalibration(), Matrix3d::Identity(), 1e-6);
+ }
+
+};
+
+TEST_F(FactorDiffDriveTest, constructor)
+{
+ // plain constructor
+ auto c1_obj = FactorDiffDrive(f1, C0, processor, false);
+ ASSERT_EQ(c1_obj.getType(), "FactorDiffDrive");
+
+ // std: make_shared
+ c1 = std::make_shared<FactorDiffDrive>(f1,
+ C0,
+ processor,
+ false);
+
+ ASSERT_NE(c1, nullptr);
+ ASSERT_EQ(c1->getType(), "FactorDiffDrive");
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1,
+ f1,
+ C0,
+ processor,
+ false);
+
+ ASSERT_NE(c1, nullptr);
+ ASSERT_EQ(c1->getType(), "FactorDiffDrive");
+ ASSERT_EQ(c1->getCaptureOther()->getSensorIntrinsic(), sensor->getIntrinsic());
+
+}
+
+TEST_F(FactorDiffDriveTest, residual_zeros)
+{
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1,
+ f1,
+ C0,
+ processor,
+ false);
+
+ residual = c1->residual();
+
+ ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-12);
+}
+
+TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
+{
+ MatrixXd J_delta_calib(3,3);
+
+ // right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ VectorXd delta(3), delta2(3);
+ MatrixXd delta_cov(3,3);
+ double dt = 1.0;
+
+ data1(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
+
+ ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
+ F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false);
+
+ residual = c1->residual();
+
+ ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-12);
+}
+
+TEST_F(FactorDiffDriveTest, solve_F1)
+{
+ MatrixXd J_delta_calib(3,3);
+
+ // right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ VectorXd delta(3), delta2(3);
+ MatrixXd delta_cov(3,3);
+ double dt = 1.0;
+
+ data1(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
+
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
+ F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false);
+
+ residual = c1->residual();
+
+ // Fix all but F1 ; perturb F1 ; solve ; print and assert values of F1
+ F0->fix();
+ sensor->fixIntrinsics();
+ F1->perturb(0.1);
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+// WOLF_TRACE("\n", report);
+ problem->print(1,0,1,1);
+
+ ASSERT_MATRIX_APPROX(F1->getStateVector(), delta, 1e-6);
+
+}
+
+TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
+{
+ MatrixXd J_delta_calib(3,3);
+
+ // right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ VectorXd delta(3), delta2(3);
+ MatrixXd delta_cov(3,3);
+ double dt = 1.0;
+
+ data1(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
+
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
+ F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false);
+
+ residual = c1->residual();
+
+ // Fix all but S ; perturb Sensor ; solve ; print and assert values of Sensor
+ F0->fix();
+ F1->fix();
+ sensor->unfixIntrinsics();
+ sensor->perturb(0.1);
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+// WOLF_TRACE("\n", report);
+ problem->print(1,0,1,1);
+
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib, 1e-6);
+
+}
+
+TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
+{
+
+ /*
+ * The trajectory is an S-shape of two 90-deg arcs of radius 1.5m.
+ * Passage poses are:
+ * x0: Initial: 0 , 0 , 0
+ * x1: Half : 1.5, -1.5 , -pi/2
+ * x2: Final : 3 , -3 , 0
+ *
+ * x0 1.5 3
+ * *> ------+--------+------> X
+ * | *
+ * |
+ * | *
+ * |
+ * -1.5 + * x1
+ * | v
+ * | *
+ * |
+ * | * x2
+ * -3 + *>
+ * |
+ * v
+ * -Y
+ *
+ * Notes:
+ * - We make two arcs because the intrinsics are not observable with just one arc.
+ * - The deltas are pre-integrated with ground truth values of the intrinsics. They are perturbed before solving.
+ * - From x0 to x1 we integrate 6 identical deltas. There are 2 intermediate keyframes.
+ * - From x1 to x2 we integrate 6 identical deltas, turning to the other side as the first 6. There are 2 intermediate keyframes.
+ * - The total of deltas is 12.
+ * - The total of keyframes is 7.
+ * - Keyframes x0 and x2 are fixed to provide the boundary conditions.
+ * - The other 5 keyframes including x1 are estimated.
+ * - The sensor intrinsics are also estimated.
+ */
+
+ ProblemPtr problem = Problem::create("PO", 2);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3d extr(0, 0, 0);
+ auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ auto calib_preint = sensor->getIntrinsic()->getState();
+ Vector3d calib_gt(1,1,1);
+
+ // params and processor
+ ParamsProcessorDiffDrivePtr params = std::make_shared<ParamsProcessorDiffDrive>();
+ params->angle_turned = 99;
+ params->dist_traveled = 99;
+ params->max_buff_length = 99;
+ params->voting_active = true;
+ params->max_time_span = 1.5;
+ auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
+ auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+ TimeStamp t(0.0);
+ double dt = 1.0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ Vector3d x1(1.5, -1.5, -M_PI/2.0);
+ Vector3d x2(3.0, -3.0, 0.0);
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
+
+ // set prior at t=0 and processor origin
+ auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ processor->setOrigin(F0);
+
+ // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
+ int N = 6;
+ Vector3d data;
+ Matrix2d data_cov; data_cov.setIdentity();
+
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
+
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ // left turn 90 deg in N steps --> ends up in (3, -3, 0)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+ C->setData(data);
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ auto F2 = problem->getLastFrame();
+
+ // Fix boundaries
+ F0->fix();
+ F2->fix();
+
+ // Perturb S
+ sensor->getIntrinsic()->perturb(0.2);
+ Vector3d calib_pert = sensor->getIntrinsic()->getState();
+
+ WOLF_TRACE("\n ========== SOLVE =========");
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_TRACE("\n", report);
+
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
+ WOLF_TRACE("x2 : ", F2->getStateVector().transpose());
+ WOLF_TRACE("calib_preint : ", calib_preint.transpose());
+ WOLF_TRACE("calib_pert : ", calib_pert.transpose());
+ WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
+ WOLF_TRACE("calib_gt : ", calib_gt.transpose());
+
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState() , calib_gt , 1e-6 );
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO") , x1 , 1e-6 );
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO") , x2 , 1e-6 );
+
+ std::cout << "\n\n" << std::endl;
+
+}
+
+TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
+{
+ /*
+ * The trajectory is an S-shape of two 90-deg arcs of radius 1.5m.
+ * Passage poses are:
+ * x0: Initial: 0 , 0 , 0
+ * x1: Half : 1.5, -1.5 , -pi/2
+ * x2: Final : 3 , -3 , 0
+ *
+ * x0 1.5 3
+ * *> ------+--------+------> X
+ * | *
+ * |
+ * | *
+ * |
+ * -1.5 + * x1
+ * | v
+ * | *
+ * |
+ * | * x2
+ * -3 + *>
+ * |
+ * v
+ * -Y
+ *
+ * Notes:
+ * - We make two arcs because the intrinsics are not observable with just one arc.
+ * - The deltas are pre-integrated with wrong values of the intrinsics. The true values must be found by solving.
+ * - From x0 to x1 we integrate 6 identical deltas. There are 2 intermediate keyframes.
+ * - From x1 to x2 we integrate 6 identical deltas, turning to the other side as the first 6. There are 2 intermediate keyframes.
+ * - The total of deltas is 12.
+ * - The total of keyframes is 7.
+ * - Keyframes x0 and x2 are fixed to provide the boundary conditions.
+ * - The other 5 keyframes including x1 are estimated.
+ * - The sensor intrinsics are also estimated.
+ */
+
+
+
+ ProblemPtr problem = Problem::create("PO", 2);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 0.95;
+ intr->radius_right = 0.98;
+ intr->wheel_separation = 1.05;
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3d extr(0, 0, 0);
+ auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ auto calib_preint = sensor->getIntrinsic()->getState();
+ Vector3d calib_gt(1.0, 1.0, 1.0); // ground truth calib
+
+ // params and processor
+ ParamsProcessorDiffDrivePtr params = std::make_shared<ParamsProcessorDiffDrive>();
+ params->angle_turned = 99;
+ params->dist_traveled = 99;
+ params->max_buff_length = 99;
+ params->voting_active = true;
+ params->max_time_span = 1.5;
+ auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
+ auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+ TimeStamp t(0.0);
+ double dt = 1.0;
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ Vector3d x1(1.5, -1.5, -M_PI/2.0);
+ Vector3d x2(3.0, -3.0, 0.0);
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+
+ // set prior at t=0 and processor origin
+ auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ processor->setOrigin(F0);
+
+ // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
+ int N = 6;
+ Vector3d data;
+ Matrix2d data_cov; data_cov.setIdentity();
+
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
+
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ // left turn 90 deg in N steps --> ends up in (3, -3, 0)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+
+ C->setData(data);
+
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ auto F2 = problem->getLastFrame();
+
+// VectorComposite x2c(x2, "PO", {2,1});
+ F2->setState(x2, "PO"); // Impose known final state regardless of integrated value.
+
+ // Fix boundaries
+ F0->fix();
+ F2->fix();
+
+ WOLF_TRACE("\n ========== SOLVE =========");
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_TRACE("\n", report);
+
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
+ WOLF_TRACE("x2 : ", F2->getStateVector().transpose());
+ WOLF_TRACE("calib_preint : ", calib_preint.transpose());
+ WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
+ WOLF_TRACE("calib_GT : ", calib_gt.transpose());
+
+
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-2);
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO"), x1, 1e-2);
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO"), x2, 1e-6);
+
+}
+
+int main(int argc, char **argv)
+{
+testing::InitGoogleTest(&argc, argv);
+//::testing::GTEST_FLAG(filter) = "FactorDiffDrive.*";
+//::testing::GTEST_FLAG(filter) = "FactorDiffDrive.preintegrate_and_solve_sensor_intrinsics";
+return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_factor_odom_2d_autodiff.cpp b/test/gtest_factor_odom_2d_autodiff.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..45315ddf2136a927674903c884d5715f6b1ae631
--- /dev/null
+++ b/test/gtest_factor_odom_2d_autodiff.cpp
@@ -0,0 +1,119 @@
+#include "core/utils/utils_gtest.h"
+#include "dummy/factor_odom_2d_autodiff.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector3d::Zero());
+
+// Capture from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+
+TEST(FactorOdom2dAutodiff, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorOdom2dAutodiff, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorOdom2dAutodiff, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
deleted file mode 100644
index 09ae42b0c6f523f8a0ed98f49c570c82cbec9d7e..0000000000000000000000000000000000000000
--- a/test/gtest_factor_odom_3D.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/**
- * \file gtest_factor_odom_3D.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "core/factor/factor_odom_3D.h"
-#include "core/capture/capture_motion.h"
-
-#include "core/ceres_wrapper/ceres_manager.h"
-
-using namespace Eigen;
-using namespace wolf;
-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", 3);
-CeresManager ceres_mgr(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 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));
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "ODOM 3D", 1, nullptr, delta, 7, 6, nullptr);
-// FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "ODOM 3D", delta, data_cov);
-// FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add
-FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(FactorBase::emplace<FactorOdom3D>(fea1, 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 bc50e62780e9812cbcdab2ffbe03a927ad3a650f..0000000000000000000000000000000000000000
--- a/test/gtest_factor_pose_2D.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/**
- * \file gtest_factor_pose_2D.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
-
-#include "core/factor/factor_pose_2D.h"
-#include "utils_gtest.h"
-
-#include "core/capture/capture_motion.h"
-
-#include "core/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", 2);
-CeresManager ceres_mgr(problem);
-
-// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, 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));
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 2D", 0, nullptr, pose, 3, 3, nullptr);
-// FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
-auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 2D", pose, data_cov);
-// FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
-FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(fea0, 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_2d.cpp b/test/gtest_factor_pose_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d38ec528460e61a13c38970d142ac63b19f17b42
--- /dev/null
+++ b/test/gtest_factor_pose_2d.cpp
@@ -0,0 +1,69 @@
+/**
+ * \file gtest_factor_pose_2d.cpp
+ *
+ * Created on: Mar 30, 2017
+ * \author: jsola
+ */
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/utils/utils_gtest.h"
+#include "core/capture/capture_motion.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input data and covariance
+Vector3d pose(Vector3d::Random());
+Vector3d data_var((Vector3d() << 0.2,0.2,0.2).finished());
+Matrix3d data_cov = data_var.asDiagonal();
+
+// perturbated priors
+Vector3d x0 = pose + Vector3d::Random()*0.25;
+
+// Problem and solver
+ProblemPtr problem = Problem::create("PO", 2);
+SolverCeres solver(problem);
+
+// Two frames
+FrameBasePtr frm0 = problem->emplaceFrame(TimeStamp(0), problem->stateZero());
+
+// Capture, feature and factor from frm1 to frm0
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr);
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom2d", pose, data_cov);
+auto ctr0 = FactorBase::emplace<FactorPose2d>(fea0, fea0, nullptr, false);
+
+////////////////////////////////////////////////////////
+
+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 = solver.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), 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 45ee64674e3988bfb6a2c0a0c2c48c83fc0b438e..0000000000000000000000000000000000000000
--- a/test/gtest_factor_pose_3D.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/**
- * \file gtest_factor_fix_3D.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
-
-#include "core/factor/factor_pose_3D.h"
-#include "utils_gtest.h"
-
-#include "core/capture/capture_motion.h"
-
-#include "core/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", 3);
-CeresManager ceres_mgr(problem);
-
-// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
-
-// Capture, feature and factor
-// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr);
-// FEATUREBASEPTR fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
-auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 3D", pose7, data_cov);
-// FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0)));
-FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(fea0, 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_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8bf5b82ed588a2466cbab42c35184c7158aed5c1
--- /dev/null
+++ b/test/gtest_factor_pose_3d.cpp
@@ -0,0 +1,76 @@
+/**
+ * \file gtest_factor_fix_3d.cpp
+ *
+ * Created on: Mar 30, 2017
+ * \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_pose_3d.h"
+#include "core/capture/capture_motion.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+Vector7d pose6toPose7(Vector6d _pose6)
+{
+ return (Vector7d() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished();
+}
+
+// Input pose6 and covariance
+Vector6d pose(Vector6d::Random());
+Vector7d pose7 = pose6toPose7(pose);
+Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
+Matrix6d data_cov = data_var.asDiagonal();
+
+// perturbated priors
+Vector7d x0 = pose6toPose7(pose + Vector6d::Random()*0.25);
+
+// Problem and solver
+ProblemPtr problem = Problem::create("PO", 3);
+SolverCeres solver(problem);
+
+// Two frames
+FrameBasePtr frm0 = problem->emplaceFrame(0, problem->stateZero() );
+
+// Capture, feature and factor
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr);
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom3d", pose7, data_cov);
+FactorPose3dPtr ctr0 = FactorBase::emplace<FactorPose3d>(fea0, fea0, nullptr, false);
+
+////////////////////////////////////////////////////////
+
+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 = solver.solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose7, 1e-6);
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4c11c8bb0316332910270b07d3aa11f2d8bad64
--- /dev/null
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -0,0 +1,119 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
+
+// Capture from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+
+TEST(FactorRelativePose2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelativePose2d, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2d, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb1f5eb4f3a2d6eacc27cf52aa91a30693945358
--- /dev/null
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -0,0 +1,239 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/math/rotations.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
+
+// Capture from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+
+TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->unfix();
+ sensor_odom2d->getO()->fix();
+ sensor_odom2d->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->unfix();
+ sensor_odom2d->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5771b1246ee1746ed8de3b59d56cec8142ea5f90
--- /dev/null
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -0,0 +1,92 @@
+/**
+ * \file gtest_factor_relative_pose_3d.cpp
+ *
+ * Created on: Mar 30, 2017
+ * \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/capture/capture_motion.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+Vector7d data2delta(Vector6d _data)
+{
+ return (Vector7d() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
+}
+
+// Input odometry data and covariance
+Vector6d data(Vector6d::Random());
+Vector7d delta = data2delta(data);
+Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
+Matrix6d data_cov = data_var.asDiagonal();
+
+// perturbated priors
+Vector7d x0 = data2delta(Vector6d::Random()*0.25);
+Vector7d x1 = data2delta(data + Vector6d::Random()*0.25);
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+SolverCeres solver(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+FactorRelativePose3dPtr ctr1 = FactorBase::emplace<FactorRelativePose3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+
+////////////////////////////////////////////////////////
+
+TEST(FactorRelativePose3d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelativePose3d, expectation)
+{
+ ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
+}
+
+TEST(FactorRelativePose3d, fix_0_solve)
+{
+
+ // Fix frame 0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->setState(x1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6);
+
+}
+
+TEST(FactorRelativePose3d, fix_1_solve)
+{
+ // Fix frame 1, perturb frm0
+ frm0->unfix();
+ frm1->fix();
+ frm0->setState(x0);
+
+ // solve for frm0
+ std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 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_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0834d95071973788df1aa053baba80adf3e95bf4
--- /dev/null
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -0,0 +1,287 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_velocity_local_direction_3d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+int N_TESTS = 100;
+
+class FactorVelocityLocalDirection3dTest : public testing::Test
+{
+ protected:
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ FrameBasePtr frm;
+ StateBlockPtr sb_p;
+ StateBlockPtr sb_o;
+ StateBlockPtr sb_v;
+ CaptureBasePtr cap;
+
+ int n_solved;
+ std::vector<double> convergence, iterations, times, error;
+
+ virtual void SetUp()
+ {
+ // create problem
+ problem = Problem::create("POV", 3);
+
+ // create solver
+ auto params_solver = std::make_shared<ParamsCeres>();
+ params_solver->solver_options.max_num_iterations = 1e3;
+ solver = std::make_shared<SolverCeres>(problem, params_solver);
+
+ // Frame
+ frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+ sb_p = frm->getP();
+ sb_p->fix();
+ sb_o = frm->getO();
+ sb_v = frm->getV();
+
+ // Capture
+ cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase",
+ frm->getTimeStamp(), nullptr);
+ }
+
+ FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local,
+ const double& angle_stdev)
+ {
+ // emplace feature
+ FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
+ v_local,
+ Matrix1d(angle_stdev * angle_stdev));
+
+ // emplace factor
+ return FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
+ fea,
+ 0,
+ nullptr,
+ false);
+ }
+
+ bool testLocalVelocity(const Quaterniond& o,
+ const Vector3d& v_local,
+ bool estimate_o,
+ bool estimate_v)
+ {
+ assert(cap->getFeatureList().empty());
+
+ // set state
+ sb_o->setState(o.coeffs());
+ sb_v->setState(o * v_local);
+
+ // fix or unfix & perturb
+ if (not estimate_o)
+ sb_o->fix();
+ else
+ {
+ sb_o->unfix();
+ sb_o->perturb();
+ }
+ if (not estimate_v)
+ sb_v->fix();
+ else
+ {
+ sb_v->unfix();
+ sb_v->setState(Vector3d::Random());
+ }
+
+ // emplace feature & factor
+ auto fac = emplaceFeatureAndFactor(v_local, 0.01);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ // local coordinates
+ auto v_est_local_normalized = (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized();
+ auto cos_angle_local = v_est_local_normalized.dot(v_local);
+ if (cos_angle_local > 1)
+ cos_angle_local = 1;
+ if (cos_angle_local < -1)
+ cos_angle_local = -1;
+
+ // global coordinates
+ auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local;
+ auto v_est_normalized = sb_v->getState().normalized();
+ auto cos_angle_global = v_est_normalized.dot(v_global);
+ if (cos_angle_global > 1)
+ cos_angle_global = 1;
+ if (cos_angle_global < -1)
+ cos_angle_global = -1;
+
+ // Check angle error
+ if (std::abs(acos(cos_angle_local)) > M_PI/180 or
+ std::abs(acos(cos_angle_global)) > M_PI/180)
+ {
+ WOLF_ERROR("\n\n!!!!!!!!!!!! ERROR TOO BIG iteration: ", iterations.size());
+ WOLF_INFO("cos(angle local) = ", cos_angle_local);
+ WOLF_INFO("angle local = ", acos(cos_angle_local));
+ WOLF_INFO("cos(angle global) = ", cos_angle_global);
+ WOLF_INFO("angle global = ", acos(cos_angle_global));
+
+ problem->print(4,1,1,1);
+ WOLF_INFO(report);
+
+ // Reset
+ fac->getFeature()->remove();
+
+ return false;
+ }
+
+ // Reset
+ fac->getFeature()->remove();
+
+ // Update performaces
+ convergence.push_back(solver->hasConverged() ? 1 : 0);
+ iterations.push_back(solver->iterations());
+ times.push_back(solver->totalTime());
+ error.push_back(acos(cos_angle_local));
+
+ return true;
+ }
+
+ void printAverageResults()
+ {
+ WOLF_INFO("/////////////////// Average results: n_solved = ", iterations.size());
+ double conv_avg = Eigen::Map<Eigen::VectorXd>(convergence.data(), convergence.size()).mean();
+ double iter_avg = Eigen::Map<Eigen::VectorXd>(iterations.data(), iterations.size()).mean();
+ double time_avg = Eigen::Map<Eigen::VectorXd>(times.data(), times.size()).mean();
+ double err_avg = Eigen::Map<Eigen::VectorXd>(error.data(), error.size()).mean();
+
+ double iter_stdev = (Eigen::Map<Eigen::ArrayXd>(iterations.data(), iterations.size()) - iter_avg).matrix().norm();
+ double time_stdev = (Eigen::Map<Eigen::ArrayXd>(times.data(), times.size()) - time_avg).matrix().norm();
+ double err_stdev = (Eigen::Map<Eigen::ArrayXd>(error.data(), error.size()) - err_avg).matrix().norm();
+
+ WOLF_INFO("\tconvergence: ", 100 * conv_avg, "%");
+ WOLF_INFO("\titerations: ", iter_avg, " - stdev: ", iter_stdev);
+ WOLF_INFO("\ttime: ", 1000 * time_avg, " ms", " - stdev: ", time_stdev);
+ WOLF_INFO("\tangle error: ", err_avg, " - stdev: ", err_stdev);
+ }
+};
+
+// Input odometry data and covariance
+Vector3d v_local(1.0, 0.0, 0.0);
+double angle_stdev = 0.1;
+
+TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
+{
+ ASSERT_TRUE(problem->check(0));
+
+ emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1);
+
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ Vector3d::Random().normalized(),
+ false, true));
+ printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveV)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ Vector3d::Random().normalized(),
+ false, true));
+ printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_degenerated)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4,
+ false, true));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+ false, true));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+ false, true));
+ printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveV_degenerated)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+ false, true));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+ false, true));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+ false, true));
+ printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ Vector3d::Random().normalized(),
+ true, false));
+ printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveO)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ Vector3d::Random().normalized(),
+ true, false));
+ printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_degenerated)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4,
+ true, false));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+ true, false));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+ (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+ true, false));
+ printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveO_degenerated)
+{
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+ true, false));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+ true, false));
+ for (auto i = 0; i < N_TESTS; i++)
+ ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+ (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+ true, false));
+ printAverageResults();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96545d1bec4e39f906ef499c3d46c9398fa586c6
--- /dev/null
+++ b/test/gtest_factory_state_block.cpp
@@ -0,0 +1,158 @@
+/*
+ * gtest_factory_state_block.cpp
+ *
+ * Created on: Apr 28, 2020
+ * Author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/common/wolf.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/sensor/factory_sensor.h"
+
+
+
+using namespace wolf;
+
+/*
+// You may use this to make some methods of Foo public
+WOLF_PTR_TYPEDEFS(FooPublic);
+class FooPublic : public Foo
+{
+ // You may use this to make some methods of Foo public
+}
+
+class TestInit : public testing::Test
+{
+ public:
+ // You may use this to initialize stuff
+ // Combine it with TEST_F(FooTest, testName) { }
+ void SetUp() override
+ {
+ // Init all you want here
+ // e.g. FooPublic foo;
+ }
+ void TearDown() override {} // you can delete this if unused
+};
+
+TEST_F(TestInit, testName)
+{
+ // Use class TestInit
+}
+*/
+
+//TEST(FactoryStateBlock, get_getClass)
+//{
+// const auto& f = FactoryStateBlock::get();
+//
+// const std::string& s = f.getClass();
+//
+// ASSERT_EQ(s, "FactoryStateBlock");
+//}
+
+TEST(FactoryStateBlock, creator_default)
+{
+ auto sbp = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
+ auto sbv = FactoryStateBlock::create("V", Eigen::Vector2d(4,5), false);
+ auto sbw = FactoryStateBlock::create("W", Eigen::Vector1d(6), false);
+
+ ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
+
+ ASSERT_FALSE(sbp->hasLocalParametrization());
+ ASSERT_FALSE(sbv->hasLocalParametrization());
+ ASSERT_FALSE(sbw->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_StateBlock)
+{
+ auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false);
+ auto sbv = FactoryStateBlock::create("StateBlock", Eigen::Vector2d(4,5), true);
+ auto sbw = FactoryStateBlock::create("StateBlock", Eigen::Vector1d(6), false);
+
+ ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
+
+ ASSERT_FALSE(sbp->isFixed());
+ ASSERT_TRUE (sbv->isFixed());
+ ASSERT_FALSE(sbw->isFixed());
+
+ ASSERT_FALSE(sbp->hasLocalParametrization());
+ ASSERT_FALSE(sbv->hasLocalParametrization());
+ ASSERT_FALSE(sbw->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Quaternion)
+{
+ auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getLocalSize(), 3);
+ ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Angle)
+{
+ auto sba = FactoryStateBlock::create("StateAngle", Eigen::Vector1d(1), false);
+
+ ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getLocalSize(), 1);
+ ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Homogeneous3d)
+{
+ auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getLocalSize(), 3);
+ ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_H)
+{
+ auto sbh = FactoryStateBlock::create("H", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getLocalSize(), 3);
+ ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_quaternion)
+{
+ auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getLocalSize(), 3);
+ ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_angle)
+{
+ auto sba = FactoryStateBlock::create("O", Eigen::Vector1d(1), false);
+
+ ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getLocalSize(), 1);
+ ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_wrong_size)
+{
+ ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 3a93ef2cb46396defc8f3b3f541042ab85e4a1a8..bbb978af9c69260bb409ee28e0c9da88214609f5 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -7,19 +7,19 @@
#include "core/feature/feature_base.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
using namespace wolf;
using namespace Eigen;
-TEST(FeatureBase, Constructor)
+TEST(FeatureBase, ConstructorCov)
{
- 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();
+ Vector3d m; m << 1,2,3;
+ Matrix3d M; M.setRandom(); M = M*M.transpose();
+ Matrix3d I = M.inverse();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd U = llt_of_info.matrixU();
+ Eigen::MatrixXd L = llt_of_info.matrixL();
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, M));
@@ -29,35 +29,18 @@ TEST(FeatureBase, Constructor)
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
}
-TEST(FeatureBase, SetMeasurement)
+TEST(FeatureBase, ConstructorInfo)
{
- 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();
+ Vector3d m; m << 1,2,3;
+ Matrix3d M; M.setRandom(); M = M*M.transpose();
+ Matrix3d I = M.inverse();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd U = llt_of_info.matrixU();
+ Eigen::MatrixXd L = llt_of_info.matrixL();
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity()));
-
- f->setMeasurement(m);
+ FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, I, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
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);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 4cbc927bfe4e67fb4464c10aad4b1596c45e0987..b37ce9708ffc0799ce95685a8a56d504b01c61a5 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,13 +5,12 @@
* Author: jsola
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+#include "core/factor/factor_relative_pose_2d.h"
#include "core/frame/frame_base.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_motion.h"
#include <iostream>
@@ -31,15 +30,13 @@ TEST(FrameBase, GettersAndSetters)
F->getTimeStamp(t);
ASSERT_EQ(t, 1);
ASSERT_FALSE(F->isFixed());
- ASSERT_EQ(F->isKey(), false);
- ASSERT_EQ(F->isKeyOrAux(), false);
}
TEST(FrameBase, StateBlocks)
{
FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- ASSERT_EQ(F->getStateBlockVec().size(), (unsigned int) 3);
+ ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1);
ASSERT_EQ(F->getV(), nullptr);
@@ -51,9 +48,7 @@ TEST(FrameBase, LinksBasic)
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());
+ SensorOdom2dPtr S = make_shared<SensorOdom2d>(Vector3d::Zero(), ParamsSensorOdom2d());
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
ASSERT_TRUE(F->getConstrainedByList().empty());
@@ -65,41 +60,25 @@ TEST(FrameBase, LinksToTree)
// Problem with 2 frames and one motion factor between them
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- IntrinsicsOdom2D intrinsics_odo;
+ ParamsSensorOdom2d 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);
- auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3s::Zero(), intrinsics_odo);
- // FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // T->addFrame(F1);
+ auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // T->addFrame(F2);
- auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
- // F1->addCapture(C);
- auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
- /// @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);
- auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
- // FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p);
- // f->addFactor(c);
- auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p);
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
+ WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
+ WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ p->link(S);
+ //auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
+ WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
+ auto c = FactorBase::emplace<FactorRelativePose2d>(f, f, F2, p, false, TOP_MOTION);
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
- // c-by link F2 -> c not yet established
- // ASSERT_TRUE(F2->getConstrainedByList().empty());
ASSERT_FALSE(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));
@@ -128,21 +107,76 @@ TEST(FrameBase, LinksToTree)
ASSERT_FALSE(F1->isFixed());
F1->getO()->fix();
ASSERT_TRUE(F1->isFixed());
+}
+
+TEST(FrameBase, Frames)
+{
+ // Problem with 10 frames
+ ProblemPtr P = Problem::create("PO", 2);
+ TrajectoryBasePtr T = P->getTrajectory();
+ ParamsSensorOdom2d intrinsics_odo;
+ intrinsics_odo.k_disp_to_disp = 1;
+ intrinsics_odo.k_rot_to_rot = 1;
+ auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
+ auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // set key
- F1->setKey();
- ASSERT_TRUE(F1->isKey());
- ASSERT_TRUE(F1->isKeyOrAux());
+ // tree is now consistent
+ ASSERT_TRUE(P->check(0));
- // Unlink
- F1->unlinkCapture(C);
- ASSERT_TRUE(F1->getCaptureList().empty());
+ // First and last
+ ASSERT_EQ(F0, T->getFirstFrame());
+ ASSERT_EQ(F9, T->getLastFrame());
+
+ // Find by timestamp
+ ASSERT_EQ(F0, T->closestFrameToTimeStamp(0));
+ ASSERT_EQ(F1, T->closestFrameToTimeStamp(1));
+ ASSERT_EQ(F2, T->closestFrameToTimeStamp(2));
+ ASSERT_EQ(F3, T->closestFrameToTimeStamp(3));
+ ASSERT_EQ(F4, T->closestFrameToTimeStamp(4));
+ ASSERT_EQ(F5, T->closestFrameToTimeStamp(5));
+ ASSERT_EQ(F6, T->closestFrameToTimeStamp(6));
+ ASSERT_EQ(F7, T->closestFrameToTimeStamp(7));
+ ASSERT_EQ(F8, T->closestFrameToTimeStamp(8));
+ ASSERT_EQ(F9, T->closestFrameToTimeStamp(9));
+
+ // Next frame
+ ASSERT_EQ(F1, F0->getNextFrame());
+ ASSERT_EQ(F2, F1->getNextFrame());
+ ASSERT_EQ(F3, F2->getNextFrame());
+ ASSERT_EQ(F4, F3->getNextFrame());
+ ASSERT_EQ(F5, F4->getNextFrame());
+ ASSERT_EQ(F6, F5->getNextFrame());
+ ASSERT_EQ(F7, F6->getNextFrame());
+ ASSERT_EQ(F8, F7->getNextFrame());
+ ASSERT_EQ(F9, F8->getNextFrame());
+ ASSERT_EQ(nullptr, F9->getNextFrame());
+
+ // Prev frame
+ ASSERT_EQ(nullptr, F0->getPreviousFrame());
+ ASSERT_EQ(F0, F1->getPreviousFrame());
+ ASSERT_EQ(F1, F2->getPreviousFrame());
+ ASSERT_EQ(F2, F3->getPreviousFrame());
+ ASSERT_EQ(F3, F4->getPreviousFrame());
+ ASSERT_EQ(F4, F5->getPreviousFrame());
+ ASSERT_EQ(F5, F6->getPreviousFrame());
+ ASSERT_EQ(F6, F7->getPreviousFrame());
+ ASSERT_EQ(F7, F8->getPreviousFrame());
+ ASSERT_EQ(F8, F9->getPreviousFrame());
}
#include "core/state_block/state_quaternion.h"
TEST(FrameBase, GetSetState)
{
- // Create PQV_3D state blocks
+ // Create PQV_3d state blocks
StateBlockPtr sbp = make_shared<StateBlock>(3);
StateBlockPtr sbv = make_shared<StateBlock>(3);
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
@@ -151,8 +185,8 @@ TEST(FrameBase, GetSetState)
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);
+ VectorXd x(10), x1(10), x2(10);
+ VectorXd p(3), v(3), q(4);
p << 1,2,3;
v << 9,8,7;
q << .5, -.5, .5, -.5;
@@ -161,19 +195,39 @@ TEST(FrameBase, GetSetState)
// 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));
+ ASSERT_MATRIX_APPROX(p, F.getP()->getState(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(q, F.getO()->getState(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(v, F.getV()->getState(), Constants::EPS_SMALL);
// get the state, form 2 by return value
- x2 = F.getState();
- ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ x2 = F.getStateVector();
+ ASSERT_MATRIX_APPROX(x2, x, Constants::EPS_SMALL);
+}
+
+TEST(FrameBase, Constructor_composite)
+{
+ FrameBase F = FrameBase(0.0,
+ "POV",
+ VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}));
+
+ ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
+ ASSERT_TRUE (F.getO()->hasLocalParametrization());
+ ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
}
+TEST(FrameBase, Constructor_vectors)
+{
+ FrameBase F = FrameBase(0.0,
+ "POV",
+ {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)});
+
+ ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
+ ASSERT_TRUE (F.getO()->hasLocalParametrization());
+ ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
+}
+
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_graph_search.cpp b/test/gtest_graph_search.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9693846e7994bf071a1aed368c99626c2311d43a
--- /dev/null
+++ b/test/gtest_graph_search.cpp
@@ -0,0 +1,225 @@
+/*
+ * gtest_graph_search.cpp
+ *
+ * Created on: Jul, 2021
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "core/utils/graph_search.h"
+
+#include <iostream>
+#include <thread>
+
+using namespace wolf;
+using namespace Eigen;
+
+class GraphSearchTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ }
+
+ FrameBasePtr emplaceFrame(const TimeStamp& ts)
+ {
+ return problem->emplaceFrame(ts, Vector3d::Zero());
+ }
+
+ FactorBasePtr createFactor(FrameBasePtr frm1, FrameBasePtr frm2)
+ {
+ auto C12 = CaptureBase::emplace<CaptureVoid>(frm2, frm2->getTimeStamp(), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", Vector3d::Zero(), Matrix3d::Identity());
+ return FactorBase::emplace<FactorRelativePose2d>(f12, f12, frm1, nullptr, false, TOP_MOTION);
+ }
+};
+
+TEST_F(GraphSearchTest, setup)
+{
+ ASSERT_TRUE(problem->check());
+}
+
+TEST_F(GraphSearchTest, nonsense11)
+{
+ auto F1 = emplaceFrame(1);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F1, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, single12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_EQ(fac_list.front(), fac12);
+}
+
+TEST_F(GraphSearchTest, single21)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_EQ(fac_list.front(), fac12);
+}
+
+TEST_F(GraphSearchTest, double12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+ auto fac12b = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_TRUE(fac_list.front() == fac12 or fac_list.front() == fac12b);
+}
+
+TEST_F(GraphSearchTest, no_solution12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto fac23 = createFactor(F2,F3);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, no_solution21)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto fac23 = createFactor(F2,F3);
+
+ auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, large)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac45 = createFactor(F4,F5);
+ auto fac56 = createFactor(F5,F6);
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 8);
+
+ ASSERT_EQ(fac_list.size(), 8);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F1, F9, 7);
+
+ ASSERT_EQ(fac_list_2.size(), 0);
+}
+
+TEST_F(GraphSearchTest, large_no_solution)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac45 = createFactor(F4,F5);
+
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
+
+ ASSERT_EQ(fac_list.size(), 0);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
+
+ ASSERT_EQ(fac_list_2.size(), 0);
+}
+
+TEST_F(GraphSearchTest, large_dense)
+{
+ /* ------- ---------------
+ * | | | |
+ * 1---2---3---4---5---6---7---8---9
+ * | |
+ * -----------
+ */
+
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac13 = createFactor(F1,F3);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac36 = createFactor(F3,F6);
+ auto fac45 = createFactor(F4,F5);
+ auto fac56 = createFactor(F5,F6);
+ auto fac59 = createFactor(F5,F9);
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
+
+ ASSERT_EQ(fac_list.size(), 4);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
+
+ ASSERT_EQ(fac_list_2.size(), 4);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
+
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..71e271a92641baffc54a01fd70c79922bbbe0d33
--- /dev/null
+++ b/test/gtest_has_state_blocks.cpp
@@ -0,0 +1,209 @@
+/**
+ * \file gtest_has_state_blocks.cpp
+ *
+ * Created on: Mar 24, 2020
+ * \author: jsola
+ */
+
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/frame/frame_base.h"
+#include "core/sensor/sensor_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_quaternion.h"
+
+using namespace wolf;
+
+class HasStateBlocksTest : public testing::Test
+{
+
+ public:
+
+ ProblemPtr problem;
+ SensorBasePtr S0, S1;
+ FrameBasePtr F0, F1;
+ CaptureBasePtr C0, C1;
+ LandmarkBasePtr L0, L1;
+ StateBlockPtr sbp0, sbv0, sbp1, sbv1;
+ StateQuaternionPtr sbo0, sbo1;
+
+
+ void SetUp() override
+ {
+ problem = Problem::create("POV", 3);
+
+ sbp0 = std::make_shared<StateBlock>(3); // size 3
+ sbo0 = std::make_shared<StateQuaternion>();
+ sbv0 = std::make_shared<StateBlock>(3); // size 3
+ sbp1 = std::make_shared<StateBlock>(3); // size 3
+ sbo1 = std::make_shared<StateQuaternion>();
+ sbv1 = std::make_shared<StateBlock>(3); // size 3
+
+ F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
+
+ }
+ void TearDown() override{}
+
+};
+
+
+TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
+{
+ Notification n;
+
+ // First add SB, then make KF
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ // F0->link(problem->getTrajectory());
+
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ F0->addStateBlock('V', sbv0, nullptr);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ F0->link(problem);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
+ ASSERT_EQ(n, ADD);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+}
+
+TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
+{
+ Notification n;
+
+ // first make KF, then add SB
+
+ // F1->link(problem->getTrajectory());
+ F1->link(problem);
+
+ F1->addStateBlock('P', sbp1, problem);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
+ ASSERT_EQ(n, ADD);
+
+ // add another SB
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
+
+ F1->addStateBlock('V', sbv1, problem);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
+ ASSERT_EQ(n, ADD);
+
+}
+
+TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
+{
+
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+
+
+ Notification n;
+
+ // Add SB, make KF
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->addStateBlock('V', sbv0, nullptr);
+ F0->link(problem);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+ // solve. This will clear all notifications
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ // Notify again the same SB
+
+ problem->notifyStateBlock(sbv0, ADD);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+ // solve again
+
+ report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ // Add again the same SB. This should crash
+
+ ASSERT_DEATH( F0->addStateBlock('V', sbv0, nullptr) , "" );
+
+}
+
+TEST_F(HasStateBlocksTest, hasStateBlock)
+{
+ ASSERT_TRUE(F0->hasStateBlock(sbp0));
+ ASSERT_FALSE(F0->hasStateBlock(sbv1));
+}
+
+TEST_F(HasStateBlocksTest, stateBlockKey)
+{
+ char key;
+ ASSERT_TRUE(F0->stateBlockKey(sbp0, key));
+ ASSERT_TRUE(key == 'P');
+
+ ASSERT_FALSE(F0->stateBlockKey(sbp1, key));
+ ASSERT_TRUE(key == '0');
+}
+
+TEST_F(HasStateBlocksTest, getState_structure)
+{
+ F0->addStateBlock('V', sbv0, nullptr); // now KF0 is POV
+
+ WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure());
+
+ auto state0 = F0->getState();
+ WOLF_DEBUG("getState() = ", state0);
+ ASSERT_EQ(state0.size(), 3);
+ ASSERT_TRUE(state0.count('P'));
+ ASSERT_TRUE(state0.count('O'));
+ ASSERT_TRUE(state0.count('V'));
+
+ state0 = F0->getState("PO");
+ WOLF_DEBUG("getState(\"PO\") = ", state0);
+ ASSERT_EQ(state0.size(), 2);
+ ASSERT_TRUE(state0.count('P'));
+ ASSERT_TRUE(state0.count('O'));
+ ASSERT_FALSE(state0.count('V'));
+
+ state0 = F0->getState("PV");
+ WOLF_DEBUG("getState(\"PV\") = ", state0);
+ ASSERT_EQ(state0.size(), 2);
+ ASSERT_TRUE(state0.count('P'));
+ ASSERT_FALSE(state0.count('O'));
+ ASSERT_TRUE(state0.count('V'));
+
+ state0 = F0->getState("OW"); // W does not exist
+ WOLF_DEBUG("getState(\"OW\") = ", state0);
+ ASSERT_EQ(state0.size(), 1);
+ ASSERT_FALSE(state0.count('P'));
+ ASSERT_TRUE(state0.count('O'));
+ ASSERT_FALSE(state0.count('V'));
+ ASSERT_FALSE(state0.count('W'));
+
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+//::testing::GTEST_FLAG(filter) = "TestGroup.DummyTestExample";
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_is_motion.cpp b/test/gtest_is_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0c8b8fa620a1f8b5261d9a5ca13a6299c896d6d9
--- /dev/null
+++ b/test/gtest_is_motion.cpp
@@ -0,0 +1,185 @@
+//Wolf
+#include "core/utils/utils_gtest.h"
+
+#include "core/processor/is_motion.h"
+#include "dummy/processor_is_motion_dummy.h"
+#include "core/sensor/sensor_odom_2d.h"
+
+#include "core/problem/problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+class IsMotionTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sen;
+ ProcessorBasePtr prc1, prc2, prc3;
+ IsMotionPtr im1, im2, im3;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ double dt = 0.01;
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("POV", 2);
+
+ // Install odom sensor
+ sen = problem->installSensor("SensorOdom2d",
+ "odometer",
+ Vector3d(0,0,0),
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+ // Install 3 odom processors
+ ParamsProcessorIsMotionDummyPtr prc1_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc1_params->time_tolerance = dt/2;
+ prc1_params->state_structure = "PO";
+ prc1_params->state_getter = false;
+ prc1 = problem->installProcessor("ProcessorIsMotionDummy",
+ "not getter processor",
+ sen,
+ prc1_params);
+ im1 = std::dynamic_pointer_cast<IsMotion>(prc1);
+
+ ParamsProcessorIsMotionDummyPtr prc2_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc2_params->time_tolerance = dt/2;
+ prc2_params->state_structure = "PO";
+ prc2_params->state_getter = true;
+ prc2_params->state_priority = 1;
+ prc2 = problem->installProcessor("ProcessorIsMotionDummy",
+ "getter processor",
+ sen,
+ prc2_params);
+ im2 = std::dynamic_pointer_cast<IsMotion>(prc2);
+
+ ParamsProcessorIsMotionDummyPtr prc3_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc3_params->time_tolerance = dt/2;
+ prc3_params->state_structure = "POV";
+ prc3_params->state_getter = true;
+ prc3_params->state_priority = 1;
+ prc3 = problem->installProcessor("ProcessorIsMotionDummy",
+ "getter processor low priority",
+ sen,
+ prc3_params);
+ im3 = std::dynamic_pointer_cast<IsMotion>(prc3);
+ }
+};
+
+/*
+ * Things to be tested:
+ * - state_getter
+ * - state_priority
+ * - Duplicated priority (Problem handles this)
+ * - setOdometry (stateStructures)
+ * - getOdomtry
+ * - Problem::getState/getOdometry (state_getter, state_priority)
+ */
+
+TEST_F(IsMotionTest, install)
+{
+ // All isMotion() = true
+ ASSERT_TRUE (prc1->isMotion());
+ ASSERT_TRUE (prc2->isMotion());
+ ASSERT_TRUE (prc3->isMotion());
+ ASSERT_TRUE (im1 != nullptr);
+ ASSERT_TRUE (im2 != nullptr);
+ ASSERT_TRUE (im3 != nullptr);
+
+ // well configured
+ ASSERT_FALSE(im1->isStateGetter());
+ ASSERT_TRUE(im2->isStateGetter());
+ ASSERT_TRUE(im3->isStateGetter());
+ ASSERT_EQ(im2->getStatePriority(), 1);
+ ASSERT_EQ(im3->getStatePriority(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
+ ASSERT_EQ(im1->getStateStructure(), "PO");
+ ASSERT_EQ(im2->getStateStructure(), "PO");
+ ASSERT_EQ(im3->getStateStructure(), "POV");
+
+ // Only 2 and 3 in problem::processor_is_motion_map_
+ ASSERT_EQ(problem->getProcessorIsMotionMap().size(), 2);
+ ASSERT_EQ(problem->getProcessorIsMotionMap().begin()->second, im2);
+ ASSERT_EQ(std::next(problem->getProcessorIsMotionMap().begin())->second, im3);
+}
+
+TEST_F(IsMotionTest, odometry)
+{
+ VectorComposite odom_p("P"); // missing P key
+ odom_p['P'] = Vector2d::Zero();
+ VectorComposite odom_pov("POV"); // key V not needed by prc1 and prc2
+ odom_pov['P'] = Vector2d::Zero();
+ odom_pov['O'] = Vector1d::Zero();
+ odom_pov['V'] = Vector2d::Zero();
+
+ // Error: required PO keys to be added
+ ASSERT_DEATH({im1->setOdometry(odom_p);},"");
+ im1->setOdometry(odom_pov);
+ im2->setOdometry(odom_pov);
+ im3->setOdometry(odom_pov);
+
+ // im1 ->set odom = 0, 0, 0
+ VectorComposite odom1("PO");
+ odom1['P'] = Vector2d::Zero();
+ odom1['O'] = Vector1d::Zero();
+ im1->setOdometry(odom1);
+ auto odom1_get = im1->getOdometry();
+ EXPECT_TRUE(odom1_get.count('P') == 1);
+ EXPECT_TRUE(odom1_get.count('O') == 1);
+ EXPECT_MATRIX_APPROX(odom1_get.at('P'), odom1.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
+
+ // im1 ->set odom = 1, 1, 1
+ VectorComposite odom2("PO");
+ odom2['P'] = Vector2d::Ones();
+ odom2['O'] = Vector1d::Ones();
+ im2->setOdometry(odom2);
+ auto odom2_get = im2->getOdometry();
+ EXPECT_TRUE(odom2_get.count('P') == 1);
+ EXPECT_TRUE(odom2_get.count('O') == 1);
+ EXPECT_MATRIX_APPROX(odom2_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
+
+ // im1 ->set odom = 2, 2, 2, 2, 2
+ VectorComposite odom3("POV");
+ odom3['P'] = 2 * Vector2d::Ones();
+ odom3['O'] = 2 * Vector1d::Ones();
+ odom3['V'] = 2 * Vector2d::Ones();
+ im3->setOdometry(odom3);
+ auto odom3_get = im3->getOdometry();
+ EXPECT_TRUE(odom3_get.count('P') == 1);
+ EXPECT_TRUE(odom3_get.count('O') == 1);
+ EXPECT_TRUE(odom3_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(odom3_get.at('P'), odom3.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom3_get.at('O'), odom3.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom3_get.at('V'), odom3.at('V'), 1e-9);
+
+ // problem::getOdometry(): by priority P and O should come from im2 and V from im3
+ auto odom_get = problem->getOdometry();
+ EXPECT_TRUE(odom_get.count('P') == 1);
+ EXPECT_TRUE(odom_get.count('O') == 1);
+ EXPECT_TRUE(odom_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(odom_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom_get.at('O'), odom2.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom_get.at('V'), odom3.at('V'), 1e-9);
+
+ // problem::getState(): by priority P and O should come from im2 and V from im3
+ auto state_get = problem->getState();
+ EXPECT_TRUE(state_get.count('P') == 1);
+ EXPECT_TRUE(state_get.count('O') == 1);
+ EXPECT_TRUE(state_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(state_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(state_get.at('O'), odom2.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(state_get.at('V'), odom3.at('V'), 1e-9);
+}
+
+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
index 520a694f6622d4601bfa2689422dbed35a2ef413..8b6ed4fc1e2ca38cc5618eaf0ee587ff408f5d4d 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -5,8 +5,8 @@
* author: jsola
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+
#include "core/state_block/local_parametrization_quaternion.h"
#include "core/state_block/local_parametrization_homogeneous.h"
@@ -18,10 +18,10 @@
#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); \
+ VectorXd dv(3); \
+ Map<const VectorXd> _dv (dv.data(), 3); \
+ VectorXd xo(4); \
+ Map<VectorXd> _xo (xo.data(), 4); \
for (int i = 0; i< 3; i++) \
{\
dv.setZero();\
@@ -39,31 +39,31 @@ TEST(TestLocalParametrization, QuaternionLocal)
{
// Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ VectorXd x_storage(22);
+ MatrixXd 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);
+ Map<VectorXd> q(&x_storage(0),4);
q.normalize();
- Map<VectorXs> da(&x_storage(4),3);
+ Map<VectorXd> 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);
+ Map<VectorXd> qo_m(&x_storage(7),4);
+ Map<MatrixRowXd> J(&M_storage(0,0),4,3);
+ MatrixXd 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);
+ Map<const VectorXd> q_m(q.data(),4);
+ Map<const VectorXd> 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);
+ Quaterniond qref = Map<Quaterniond>(q.data()) * wolf::v2q(da);
ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
Qpar_loc.computeJacobian(q_m,J);
@@ -72,8 +72,8 @@ TEST(TestLocalParametrization, QuaternionLocal)
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);
+ Map<const VectorXd> qoc_m(qo_m.data(), 4);
+ Map<VectorXd> da2_m(x_storage.data() + 10, 3);
Qpar_loc.minus(q_m, qoc_m, da2_m);
@@ -84,31 +84,31 @@ TEST(TestLocalParametrization, QuaternionLocal)
TEST(TestLocalParametrization, QuaternionGlobal)
{
// Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ VectorXd x_storage(22);
+ MatrixXd 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);
+ Map<VectorXd> q(&x_storage(0),4);
q.normalize();
- Map<VectorXs> da(&x_storage(4),3);
+ Map<VectorXd> 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);
+ Map<VectorXd> qo_m(&x_storage(7),4);
+ Map<MatrixRowXd> J(&M_storage(0,0),4,3);
+ MatrixXd 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);
+ Map<const VectorXd> q_m(q.data(),4);
+ Map<const VectorXd> 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());
+ Quaterniond qref = wolf::v2q(da) * Map<Quaterniond>(q.data());
ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
Qpar_glob.computeJacobian(q_m,J);
@@ -117,8 +117,8 @@ TEST(TestLocalParametrization, QuaternionGlobal)
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);
+ Map<const VectorXd> qoc_m(qo_m.data(), 4);
+ Map<VectorXd> da2_m(x_storage.data() + 10, 3);
Qpar_glob.minus(q_m, qoc_m, da2_m);
@@ -129,21 +129,21 @@ TEST(TestLocalParametrization, QuaternionGlobal)
TEST(TestLocalParametrization, Homogeneous)
{
// Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ VectorXd x_storage(22);
+ MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size.
// HOMOGENEOUS ----------------------------------------
- Map<VectorXs> h(&x_storage(11),4);
+ Map<VectorXd> h(&x_storage(11),4);
h.setRandom();
- Map<VectorXs> d(&x_storage(15),3);
+ Map<VectorXd> 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);
+ Map<VectorXd> ho_m(&x_storage(18),4);
+ Map<MatrixRowXd> J(&M_storage(0,0),4,3);
+ MatrixXd J_num(4,3);
LocalParametrizationHomogeneous Hpar;
- Map<const VectorXs> h_m(h.data(),4);
- Map<const VectorXs> d_m(d.data(),3);
+ Map<const VectorXd> h_m(h.data(),4);
+ Map<const VectorXd> d_m(d.data(),3);
Hpar.plus(h_m,d_m,ho_m);
@@ -155,8 +155,8 @@ TEST(TestLocalParametrization, Homogeneous)
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);
+ Map<const VectorXd> hoc_m(ho_m.data(), 4);
+ Map<VectorXd> d2_m(x_storage.data() + 10, 3);
Hpar.minus(h_m, hoc_m, d2_m);
diff --git a/test/gtest_logging.cpp b/test/gtest_logging.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58480003d32db34aec7b643bfa39c4b8259a2d3b
--- /dev/null
+++ b/test/gtest_logging.cpp
@@ -0,0 +1,42 @@
+/**
+ * \file test_wolf_logging.cpp
+ *
+ * Created on: Oct 28, 2016
+ * \author: Jeremie Deray
+ */
+
+#include "core/common/wolf.h"
+#include "core/utils/utils_gtest.h"
+
+
+TEST(logging, info)
+{
+ WOLF_INFO("test info ", 5, " ", 0.123);
+}
+
+TEST(logging, warn)
+{
+ WOLF_WARN("test warn ", 5, " ", 0.123);
+}
+
+TEST(logging, error)
+{
+ WOLF_ERROR("test error ", 5, " ", 0.123);
+}
+
+TEST(logging, trace)
+{
+ WOLF_TRACE("test trace ", 5, " ", 0.123);
+}
+
+TEST(logging, debug)
+{
+ WOLF_DEBUG("test debug ", 5, " ", 0.123);
+}
+
+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
index 5a0b707a6487eda22e37f2601408cda64929015d..ce406cd1a8b21c4f63251b2f28336b0acca9e1ad 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,12 +1,13 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
+#include "core/math/covariance.h"
using namespace Eigen;
using namespace wolf;
TEST(MakePosDefTest, OkTest)
{
- MatrixXs M = MatrixXs::Identity(3,3);
+ MatrixXd M = MatrixXd::Identity(3,3);
EXPECT_TRUE(isCovariance(M));
EXPECT_FALSE(makePosDef(M));
@@ -14,7 +15,7 @@ TEST(MakePosDefTest, OkTest)
TEST(MakePosDefTest, FixTest)
{
- MatrixXs M = MatrixXs::Zero(3,3);
+ MatrixXd M = MatrixXd::Zero(3,3);
EXPECT_FALSE(isCovariance(M));
EXPECT_TRUE(makePosDef(M));
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd3f75031499911304c36a1e8a4f5fa9f7c10e04
--- /dev/null
+++ b/test/gtest_map_yaml.cpp
@@ -0,0 +1,291 @@
+/**
+ * \file test_map_yaml.cpp
+ *
+ * Created on: Jul 27, 2016
+ * \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include "core/map/map_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+#include "core/yaml/yaml_conversion.h"
+
+#include <iostream>
+using namespace wolf;
+
+TEST(MapYaml, save_2d)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ Eigen::Vector2d p1, p2, p3;
+ Eigen::Vector1d o2, o3;
+ p1 << 1, 2;
+ p2 << 3, 4;
+ p3 << 5, 6;
+ o2 << 2;
+ o3 << 3;
+
+ StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1);
+ StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2);
+ StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2);
+ StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true);
+ StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true);
+
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, o3_sb);
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string map_path = wolf_root + "/test/yaml";
+
+ // Check Problem functions
+ std::string filename = map_path + "/map_2d_problem.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ problem->saveMap(filename, "2d map saved from Problem::saveMap()");
+
+ // Check Map functions
+ filename = map_path + "/map_2d_map.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ problem->getMap()->save(filename, "2d map saved from Map::save()");
+}
+
+TEST(MapYaml, load_2d)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string map_path = wolf_root + "/test/yaml";
+
+ // Check Problem functions
+ std::string filename = map_path + "/map_2d_problem.yaml";
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->loadMap(filename);
+
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ }
+ }
+
+ // empty the map
+ {
+ auto lmk_list = problem->getMap()->getLandmarkList();
+ for (auto lmk : lmk_list)
+ lmk->remove();
+ }
+ ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+
+ // Check Map functions
+ filename = map_path + "/map_2d_map.yaml";
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->getMap()->load(filename);
+
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ }
+ }
+}
+TEST(MapYaml, save_3d)
+{
+ ProblemPtr problem = Problem::create("PO", 3);
+
+ Eigen::Vector3d p1, p2, p3;
+ Eigen::Vector4d q2, q3;
+ p1 << 1, 2, 3;
+ p2 << 4, 5, 6;
+ p3 << 7, 8, 9;
+ q2 << 0, 1, 0, 0;
+ q3 << 0, 0, 1, 0;
+
+ auto p1_sb = std::make_shared<StateBlock>(p1);
+ auto p2_sb = std::make_shared<StateBlock>(p2);
+ auto q2_sb = std::make_shared<StateQuaternion>(q2);
+ auto p3_sb = std::make_shared<StateBlock>(p3, true);
+ auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
+
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, q2_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, q3_sb);
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string map_path = wolf_root + "/test/yaml";
+
+ // Check Problem functions
+ std::string filename = map_path + "/map_3d_problem.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ problem->saveMap(filename, "3d map saved from Problem::saveMap()");
+
+ // Check Map functions
+ filename = map_path + "/map_3d_map.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ problem->getMap()->save(filename, "3d map saved from Map::save()");
+}
+
+TEST(MapYaml, load_3d)
+{
+ ProblemPtr problem = Problem::create("PO", 3);
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string map_path = wolf_root + "/test/yaml";
+
+ // Check Problem functions
+ std::string filename = map_path + "/map_3d_problem.yaml";
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->loadMap(filename);
+
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
+ }
+
+ // empty the map
+ {
+ auto lmk_list = problem->getMap()->getLandmarkList();
+ for (auto lmk : lmk_list)
+ lmk->remove();
+ }
+ ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+
+ // Check Map functions
+ filename = map_path + "/map_3d_map.yaml";
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->getMap()->load(filename);
+
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
+ }
+}
+
+TEST(MapYaml, remove_map_files)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string map_path = wolf_root + "/test/yaml";
+
+ std::string filename = map_path + "/map_2d_problem.yaml";
+ ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+ filename = map_path + "/map_2d_map.yaml";
+ ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+ filename = map_path + "/map_3d_problem.yaml";
+ ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+ filename = map_path + "/map_3d_map.yaml";
+ ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+}
+
+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
index 3ef0bfaa050b6125427b2b16256c56719f54fbaf..f47e9bd12826c323be8f61c882ed638ba35a5745 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -5,8 +5,8 @@
* Author: jsola
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+
#include "core/processor/motion_buffer.h"
@@ -18,7 +18,7 @@ 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 newMotion(TimeStamp t, double d, double D, double C, double J_d, double J_D)
{
Motion m(t, 1, 1, 1, 0);
m.delta_(0) = d;
@@ -40,10 +40,10 @@ Motion m4 = newMotion(t4, 4, 10, 1, .1, 1);
TEST(MotionBuffer, QueryTimeStamps)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
+ MB.push_back(m0);
+ MB.push_back(m1);
TimeStamp t;
t.set(-1); // t is older than m0.ts_ -> return m0
@@ -64,12 +64,12 @@ TEST(MotionBuffer, QueryTimeStamps)
TEST(MotionBuffer, getMotion)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB;
- MB.get().push_back(m0);
+ MB.push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
- MB.get().push_back(m1);
+ MB.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_);
@@ -78,13 +78,13 @@ TEST(MotionBuffer, getMotion)
TEST(MotionBuffer, getDelta)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB;
- MB.get().push_back(m0);
+ MB.push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
- MB.get().push_back(m1);
+ MB.push_back(m1);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_);
@@ -92,21 +92,21 @@ TEST(MotionBuffer, getDelta)
TEST(MotionBuffer, Split)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB;
- 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
+ MB.push_back(m0);
+ MB.push_back(m1);
+ MB.push_back(m2);
+ MB.push_back(m3);
+ MB.push_back(m4); // put 5 motions
- MotionBuffer MB_old(1,1,1,0);
+ MotionBuffer MB_old;
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.size(), (unsigned int) 2);
+ ASSERT_EQ(MB .size(), (unsigned int) 3);
ASSERT_EQ(MB_old.getMotion(t1).ts_, t1);
ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1
@@ -116,57 +116,57 @@ TEST(MotionBuffer, Split)
// TEST(MotionBuffer, integrateCovariance)
// {
-// MotionBuffer MB(1,1,1,0);
+// MotionBuffer MB;
//
-// 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
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
-// Eigen::MatrixXs cov = MB.integrateCovariance();
+// Eigen::MatrixXd cov = MB.integrateCovariance();
// ASSERT_NEAR(cov(0), 0.04, 1e-8);
//
// }
//
// TEST(MotionBuffer, integrateCovariance_ts)
// {
-// MotionBuffer MB(1,1,1,0);
+// MotionBuffer MB;
//
-// 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
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
-// Eigen::MatrixXs cov = MB.integrateCovariance(t2);
+// Eigen::MatrixXd cov = MB.integrateCovariance(t2);
// ASSERT_NEAR(cov(0), 0.02, 1e-8);
// }
//
// TEST(MotionBuffer, integrateCovariance_ti_tf)
// {
-// MotionBuffer MB(1,1,1,0);
+// MotionBuffer MB;
//
-// 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
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
-// Eigen::MatrixXs cov = MB.integrateCovariance(t1, t3);
+// Eigen::MatrixXd 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
+// cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integrate
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
// }
TEST(MotionBuffer, print)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- MB.get().push_back(m2);
+ MB.push_back(m0);
+ MB.push_back(m1);
+ MB.push_back(m2);
MB.print();
MB.print(0,0,0,0);
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
deleted file mode 100644
index faa22aff39d4af1878f051e6687a7a327e789f67..0000000000000000000000000000000000000000
--- a/test/gtest_odom_2D.cpp
+++ /dev/null
@@ -1,489 +0,0 @@
-/**
- * \file test_odom_2D.cpp
- *
- * Created on: Mar 15, 2016
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-// Classes under test
-#include "core/processor/processor_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
-
-// Wolf includes
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/state_block/state_block.h"
-#include "core/common/wolf.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// STL includes
-#include <map>
-#include <list>
-#include <algorithm>
-#include <iterator>
-
-// General includes
-#include <iostream>
-#include <iomanip> // std::setprecision
-#include "core/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", 2);
- 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, Vector3s::Zero(), t);
- auto C1 = CaptureBase::emplace<CaptureBase>(F1, "ODOM 2D", t);
- auto f1 = FeatureBase::emplace<FeatureBase>(C1, "ODOM 2D", delta, delta_cov);
- auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr);
-
- // KF2 and motion from KF1
- t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
- auto C2 = CaptureBase::emplace<CaptureBase>(F2, "ODOM 2D", t);
- auto f2 = FeatureBase::emplace<FeatureBase>(C2, "ODOM 2D", delta, delta_cov);
- auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr);
-
- 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", 2);
- 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", 2);
- 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.000001;
- 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, 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, 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_2d.cpp b/test/gtest_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3aad59381e4fe9b37df987d18314fae17903cbde
--- /dev/null
+++ b/test/gtest_odom_2d.cpp
@@ -0,0 +1,511 @@
+/**
+ * \file test_odom_2d.cpp
+ *
+ * Created on: Mar 15, 2016
+ * \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+
+// Classes under test
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
+
+// Wolf includes
+#include "core/state_block/state_block.h"
+#include "core/common/wolf.h"
+#include "core/capture/capture_pose.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+
+// STL includes
+#include <map>
+#include <list>
+#include <algorithm>
+#include <iterator>
+
+// General includes
+#include <iostream>
+#include <iomanip> // std::setprecision
+
+using namespace wolf;
+using namespace Eigen;
+
+VectorXd plus(const VectorXd& _pose, const Vector2d& _data)
+{
+ VectorXd _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;
+}
+
+MatrixXd plus_jac_u(const VectorXd& _pose, const Vector2d& _data)
+{
+ MatrixXd 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;
+}
+
+MatrixXd plus_jac_x(const VectorXd& _pose, const Vector2d& _data)
+{
+ Matrix3d 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())
+ {
+ 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->getStateVector().transpose() << endl;
+ Eigen::MatrixXd 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;
+ double dt = .01;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ Vector3d delta (2,0,0);
+ Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
+
+ ProblemPtr Pr = Problem::create("PO", 2);
+ SolverCeres solver(Pr);
+
+ // KF0 and absolute prior
+ FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
+
+ // KF1 and motion from KF0
+ t += dt;
+ FrameBasePtr F1 = Pr->emplaceFrame(t, Vector3d::Zero());
+ auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
+ auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
+ auto c1 = FactorBase::emplace<FactorRelativePose2d>(f1, f1, F0, nullptr, false, TOP_MOTION);
+
+ // KF2 and motion from KF1
+ t += dt;
+ FrameBasePtr F2 = Pr->emplaceFrame(t, Vector3d::Zero());
+ auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
+ auto c2 = FactorBase::emplace<FactorRelativePose2d>(f2, f2, F1, nullptr, false, TOP_MOTION);
+
+ ASSERT_TRUE(Pr->check(0));
+
+// cout << "===== full ====" << endl;
+ F0->setState(Vector3d(1,2,3));
+ F1->setState(Vector3d(2,3,1));
+ F2->setState(Vector3d(3,1,2));
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+// std::cout << report << std::endl;
+
+ solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+// show(Pr);
+
+ Matrix3d 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
+ MatrixXd 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->getStateVector(), Vector3d(0,0,0), 1e-6);
+ auto s0_vector = s0.vector("PO");
+ ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((s0_vector.array() * s0_vector.array()).matrix().asDiagonal()), 1e-6);
+ ASSERT_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6);
+ ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
+ ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6);
+ ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
+}
+
+TEST(Odom2d, VoteForKfAndSolve)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ std::cout << std::setprecision(4);
+ // time
+ TimeStamp t0(0.0), t = t0;
+ double dt = .01;
+ // Origin frame:
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ // motion data
+ VectorXd data(Vector2d(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
+ Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
+ int N = 7; // number of process() steps
+
+ // Create Wolf tree nodes
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
+ params->voting_active = true;
+ params->dist_traveled = 100;
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
+ params->max_time_span = 100;
+ params->cov_det = 100;
+ params->time_tolerance = dt/2;
+ params->unmeasured_perturbation_std = 0.00;
+ Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
+ ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "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
+ SolverCeres solver(problem);
+
+ // Origin Key Frame
+ auto KF = problem->setPriorFactor(x0, s0, t, dt/2);
+ processor_odom2d->setOrigin(KF);
+
+ solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+
+ // std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
+ // std::cout << "Initial covariance : " << std::endl << problem->getLastFrameCovariance() << std::endl;
+ // std::cout << "Motion data : " << data.transpose() << std::endl;
+
+ // Check covariance values
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
+ Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
+ Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
+ Eigen::MatrixXd Ju(3, 2);
+ Eigen::Matrix3d Jx;
+ std::vector<Eigen::VectorXd> integrated_pose_vector;
+ std::vector<Eigen::MatrixXd> integrated_cov_vector;
+
+ integrated_pose_vector.push_back(integrated_pose);
+ integrated_cov_vector.push_back(integrated_cov);
+
+// 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 " << n << " at time " << t << std::endl;
+ // re-use capture with updated timestamp
+ capture->setTimeStamp(t);
+
+ // Processor
+ capture->process();
+ ASSERT_TRUE(problem->check(0));
+ Matrix3d odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_;
+
+ // 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_INFO("n: ", n, " t:", t);
+ WOLF_INFO("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
+ WOLF_INFO("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->getState().vector("PO"), integrated_pose, 1e-6);
+
+ integrated_pose_vector.push_back(integrated_pose);
+ integrated_cov_vector.push_back(integrated_cov);
+
+ t += dt;
+ }
+
+ // Solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+
+
+ problem->print(4,1,1,1);
+
+ // Check the 3 KFs' states and covariances
+ MatrixXd computed_cov;
+ int n = 0;
+ for (auto F : *problem->getTrajectory())
+ {
+ ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6);
+ ASSERT_TRUE (F->getCovariance(computed_cov));
+ ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
+ n += 3;
+ }
+}
+
+TEST(Odom2d, KF_callback)
+{
+ using std::cout;
+ using std::endl;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ std::cout << std::setprecision(4);
+ // time
+ TimeStamp t0(0.0), t = t0;
+ double dt = .01;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite x0_cov(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
+ Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
+ int N = 8; // 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", 2);
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
+ params->dist_traveled = 100;
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
+ params->max_time_span = 100;
+ params->cov_det = 100;
+ params->time_tolerance = dt/2;
+ params->unmeasured_perturbation_std = 0.000001;
+ Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
+ ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
+ ProcessorOdom2dPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2d>(prc_base);
+ processor_odom2d->setTimeTolerance(dt/2);
+
+ // Ceres wrapper
+ SolverCeres solver(problem);
+
+ // Origin Key Frame
+ FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
+ processor_odom2d->setOrigin(keyframe_0);
+
+ // Check covariance values
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
+ Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
+ Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
+ Eigen::MatrixXd Ju(3, 2);
+ Eigen::Matrix3d Jx;
+ std::vector<Eigen::VectorXd> integrated_pose_vector;
+ std::vector<Eigen::MatrixXd> 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
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+
+ std::cout << "t: " << t << std::endl;
+ for (int n=1; n<=N; n++)
+ {
+ t += dt;
+
+ // re-use capture with updated timestamp
+ capture->setTimeStamp(t);
+
+ // Processor
+ capture->process();
+ 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;
+
+ Vector3d x_split = processor_odom2d->getState(t_split).vector("PO");
+ FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
+
+ // there must be 2KF and one F
+ ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
+ // The last KF must have TS = 0.08
+ ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
+
+ ASSERT_TRUE(problem->check(0));
+ processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
+ ASSERT_TRUE(problem->check(0));
+ t += dt;
+ capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ capture->process();
+ 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 = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+// std::cout << report << std::endl;
+ solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+
+ ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
+ MatrixXd computed_cov;
+ ASSERT_TRUE(problem->getLastFrameCovariance(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).vector("PO");
+ FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split);
+
+ ASSERT_TRUE(problem->check(0));
+ processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
+ ASSERT_TRUE(problem->check(0));
+ t += dt;
+ capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ capture->process();
+ 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(Vector3d(1,2,3));
+ keyframe_1->setState(Vector3d(2,3,1));
+ keyframe_2->setState(Vector3d(3,1,2));
+
+ report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+
+ problem->print(4,1,1,1);
+
+ // check the split KF
+ MatrixXd KF1_cov;
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
+ ASSERT_POSE2d_APPROX(keyframe_1->getStateVector(), 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
+ MatrixXd KF2_cov;
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
+ ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
+
+ // Check full trajectory
+ t = t0;
+ for (int n=1; n<=N; n++)
+ {
+ t += dt;
+ WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).vector("PO").transpose());
+ WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
+ ASSERT_POSE2d_APPROX(problem->getState(t).vector("PO"), integrated_pose_vector[n], 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+// testing::GTEST_FLAG(filter) = "Odom2d.VoteForKfAndSolve";
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
deleted file mode 100644
index ce52ec3932b0162ca400cff88680ea27b7b4b196..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 "core/common/wolf.h"
-#include "core/utils/logging.h"
-
-#include "core/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
index c2b8958d0ca9170f4ea317404b0c0cf2ccb9457f..d1fce41e50c72c91fb520d748a8154d5d516a1e6 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -5,12 +5,12 @@
* Author: jsola
*/
//Wolf
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
+#include "dummy/processor_tracker_feature_dummy.h"
#include "core/capture/capture_void.h"
#include "core/problem/problem.h"
@@ -26,13 +26,13 @@ class BufferPackKeyFrameTest : public testing::Test
{
public:
- PackKeyFrameBuffer pack_kf_buffer;
+ BufferPackKeyFrame pack_kf_buffer;
FrameBasePtr f10, f20, f21, f28;
- Scalar tt10, tt20, tt21, tt28;
+ double tt10, tt20, tt21, tt28;
TimeStamp timestamp;
- Scalar timetolerance;
+ double timetolerance;
- void SetUp(void)
+ void SetUp(void) override
{
f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
@@ -137,7 +137,9 @@ TEST_F(BufferPackKeyFrameTest, selectPack)
{
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();
}
@@ -154,7 +156,7 @@ TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
// input time stamps
std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
- Scalar tt = 0.01;
+ double tt = 0.01;
// Solution matrix
// q_ts | pack
@@ -184,7 +186,7 @@ TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
// 20.5 null
// 21.5 21
- Eigen::MatrixXs truth(3,6), res(3,6);
+ Eigen::MatrixXd 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();
@@ -209,7 +211,7 @@ TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
TEST_F(BufferPackKeyFrameTest, removeUpTo)
{
// Small time tolerance for all test asserts
- Scalar tt = 0.1;
+ double tt = 0.1;
pack_kf_buffer.clear();
pack_kf_buffer.add(f10, tt10);
pack_kf_buffer.add(f20, tt20);
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index e5ddb8653a7b8e8246c1a2e8707c6eb186e66ae5..89670c6229a298741f61a4dceba52799297f22d4 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -5,24 +5,23 @@
* Author: jvallve
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/sensor/sensor_odom_3D.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include <iostream>
using namespace wolf;
ProblemPtr problem_ptr = Problem::create("PO", 3);
-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());
+SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
+Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).finished());
+Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
+Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
-SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>()));
+SensorOdom3dPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3d>(problem_ptr->installSensor("SensorOdom3d", "odometer", initial_extrinsics, std::make_shared<ParamsSensorOdom3d>()));
TEST(ParameterPrior, initial_extrinsics)
{
@@ -35,36 +34,36 @@ TEST(ParameterPrior, initial_extrinsics)
TEST(ParameterPrior, prior_p)
{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3s::Identity());
+ odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3d::Identity());
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6);
}
TEST(ParameterPrior, prior_o)
{
- odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3s::Identity());
+ odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3d::Identity());
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
}
TEST(ParameterPrior, prior_p_overwrite)
{
- odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3s::Identity());
+ odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3d::Identity());
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6);
}
TEST(ParameterPrior, prior_p_segment)
{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2);
+ odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2);
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
}
@@ -72,7 +71,7 @@ TEST(ParameterPrior, prior_p_segment)
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),"");
+ ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter('O', prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),"");
}
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index a24b07a7e01b8a37b17806fd89d25d10a5e8f22b..bf427f782f1fbb6e790a498535a8d1c03f56af55 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,33 +1,72 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
using namespace std;
using namespace wolf;
std::string wolf_root = _WOLF_ROOT_DIR;
-parserYAML parse(string _file, string _path_root)
+TEST(ParamsServer, Default)
{
- parserYAML parser = parserYAML(_file, _path_root);
- parser.parse();
- return parser;
+ ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
}
-TEST(ParamsServer, Default)
+TEST(ParamsServer, getParamsOk)
+{
+ ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ EXPECT_EQ(server.getParam<int>("int_1"), -3);
+ EXPECT_EQ(server.getParam<int>("int_2"), 0);
+ EXPECT_EQ(server.getParam<int>("int_3"), 6);
+
+ EXPECT_EQ(server.getParam<unsigned int>("uint_1"), 2);
+ EXPECT_EQ(server.getParam<unsigned int>("uint_2"), 0);
+ EXPECT_EQ(server.getParam<unsigned int>("uint_3"), 6);
+
+ EXPECT_EQ(server.getParam<double>("double_1"), 3.6);
+ EXPECT_EQ(server.getParam<double>("double_2"), -3);
+ EXPECT_EQ(server.getParam<double>("double_3"), 3.6);
+
+ EXPECT_EQ(server.getParam<std::string>("string_1"), std::string("wolf"));
+ EXPECT_EQ(server.getParam<std::string>("string_2"), std::string("Wolf"));
+
+ EXPECT_EQ(server.getParam<bool>("bool_1"), true);
+ EXPECT_EQ(server.getParam<bool>("bool_2"), true);
+ EXPECT_EQ(server.getParam<bool>("bool_3"), true);
+ EXPECT_EQ(server.getParam<bool>("bool_4"), true);
+ EXPECT_EQ(server.getParam<bool>("bool_5"), false);
+ EXPECT_EQ(server.getParam<bool>("bool_6"), false);
+ EXPECT_EQ(server.getParam<bool>("bool_7"), false);
+ EXPECT_EQ(server.getParam<bool>("bool_8"), false);
+}
+
+TEST(ParamsServer, getParamsWrong)
{
- auto parser = parse("/test/yaml/params1.yaml", wolf_root);
- auto params = parser.getParams();
- paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization());
- EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6);
- EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active", "true"), false);
- EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance", "23"), 23);
- EXPECT_THROW({ server.getParam<unsigned int>("test error"); }, std::runtime_error);
- EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance"), 23);
- EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active"), false);
+ ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ EXPECT_ANY_THROW({ server.getParam<double>("should_not_exist"); });
+
+ EXPECT_ANY_THROW({ server.getParam<int>("int_wrong_1"); });
+ EXPECT_ANY_THROW({ server.getParam<int>("int_wrong_2"); });
+ EXPECT_ANY_THROW({ server.getParam<int>("int_wrong_3"); });
+
+ EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_1"); });
+ EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_2"); });
+ EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_3"); });
+ EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_4"); });
+
+ EXPECT_ANY_THROW({ server.getParam<double>("double_wrong_1"); });
+ EXPECT_ANY_THROW({ server.getParam<double>("double_wrong_2"); });
+
+ EXPECT_ANY_THROW({ server.getParam<bool>("bool_wrong_2"); });
}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 752c06fd29831bb29dcd550ed35cdc24bd76d3de..df50e8f75bb0bdad8c015e83e87376a31f75573e 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,41 +1,60 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
using namespace std;
using namespace wolf;
std::string wolf_root = _WOLF_ROOT_DIR;
+std::string sensor_prefix = "sensor/";
+std::string processor_prefix = "processor/";
-parserYAML parse(string _file, string _path_root)
+TEST(ParserYaml, RegularParse)
{
- parserYAML parser = parserYAML(_file, _path_root);
- parser.parse();
- return parser;
+ auto parser = ParserYaml("test/yaml/params1.yaml", wolf_root);
+ auto params = parser.getParams();
+ // for(auto it : params)
+ // cout << it.first << " %% " << it.second << endl;
+ EXPECT_EQ(params[sensor_prefix + "odom/intrinsic/k_rot_to_rot"], "0.1");
+ EXPECT_EQ(params[processor_prefix + "processor1/sensor_name"], "odom");
}
-
-TEST(ParserYAML, RegularParse)
+TEST(ParserYaml, ParseMap)
{
- auto parser = parse("/test/yaml/params1.yaml", wolf_root);
+ auto parser = ParserYaml("test/yaml/params2.yaml", wolf_root);
auto params = parser.getParams();
// for(auto it : params)
// cout << it.first << " %% " << it.second << endl;
- EXPECT_EQ(params["odom/intrinsic/k_rot_to_rot"], "0.1");
- EXPECT_EQ(params["processor1/sensor name"], "odom");
+ EXPECT_EQ(params[processor_prefix + "processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
+ // EXPECT_EQ(params["processor1/$mymap/k1"], "v1");
+}
+TEST(ParserYaml, FollowFile)
+{
+ auto parser = ParserYaml("test/yaml/params1.yaml", wolf_root);
+ auto params = parser.getParams();
+ EXPECT_EQ(params[processor_prefix + "my_proc_test/max_buff_length"], "100");
+ EXPECT_EQ(params[processor_prefix + "my_proc_test/voting_active"], "false");
+}
+TEST(ParserYaml, FollowOdom3d)
+{
+ auto parser = ParserYaml("test/yaml/params1.yaml", wolf_root);
+ auto params = parser.getParams();
+ EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_buff_length"], "10");
+ EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_time_span"], "0.2");
}
-TEST(ParserYAML, ParseMap)
+TEST(ParserYaml, JumpFile)
{
- auto parser = parse("/test/yaml/params2.yaml", wolf_root);
+ auto parser = ParserYaml("test/yaml/params3.yaml", wolf_root);
auto params = parser.getParams();
- EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
+ EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/max_buff_length"], "100");
+ EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/voting_active"], "false");
}
-TEST(ParserYAML, JumpFile)
+TEST(ParserYaml, ProblemConfig)
{
- auto parser = parse("/test/yaml/params3.yaml", wolf_root);
+ auto parser = ParserYaml("test/yaml/params2.yaml", wolf_root);
auto params = parser.getParams();
- EXPECT_EQ(params["my_proc_test/max_buff_length"], "100");
- EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false");
+ EXPECT_EQ(params["problem/frame_structure"], "POV");
+ EXPECT_EQ(params["problem/dimension"], "2");
}
int main(int argc, char **argv)
{
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 942e49086acba86cbd74101bd21ec1ec14195e66..6d0e7014d1a5d7b5f719cfae79953647ec704b09 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -5,16 +5,24 @@
* Author: jsola
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/processor/processor_odom_3d.h"
+#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/processor/processor_diff_drive.h"
+#include "core/capture/capture_diff_drive.h"
+#include "core/factor/factor_diff_drive.h"
+#include "core/state_block/state_quaternion.h"
+
#include <iostream>
@@ -22,34 +30,8 @@ using namespace wolf;
using namespace Eigen;
-WOLF_PTR_TYPEDEFS(DummySolverManager);
-
-class DummySolverManager : public SolverManager
-{
-public:
- DummySolverManager(const ProblemPtr& _problem)
- : SolverManager(_problem)
- {
- //
- }
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool hasConverged(){return true;};
- virtual SizeStd iterations(){return 0;};
- virtual Scalar initialCost(){return 0;};
- virtual Scalar finalCost(){return 0;};
- virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr){};
- virtual void removeFactor(const FactorBasePtr& fac_ptr){};
- virtual void addStateBlock(const StateBlockPtr& state_ptr){};
- virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
-};
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
TEST(Problem, create)
{
@@ -61,7 +43,7 @@ TEST(Problem, create)
ASSERT_EQ(P, P->getMap()->getProblem());
// check frame structure through the state size
- ASSERT_EQ(P->getFrameStructureSize(), 10);
+ ASSERT_EQ(P->getFrameStructure(), "POV");
}
TEST(Problem, Sensors)
@@ -69,8 +51,6 @@ TEST(Problem, Sensors)
ProblemPtr P = Problem::create("POV", 3);
// add a dummy sensor
- // SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
- // P->addSensor(S);
auto S = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
// check pointers
@@ -84,127 +64,155 @@ TEST(Problem, Processor)
ProblemPtr P = Problem::create("PO", 3);
// check motion processor is null
- ASSERT_FALSE(P->getProcessorMotion());
+ ASSERT_FALSE(P->getProcessorIsMotion());
// 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);
- auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D());
+ auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
// add motion processor
- // ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>());
- // Sm->addProcessor(Pm);
- auto Pm = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>()));
+ auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
- // 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);
+ // check motion processor IS NOT by emplace
+ ASSERT_EQ(P->getProcessorIsMotion(), Pm);
}
TEST(Problem, Installers)
{
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7s xs;
+ Eigen::Vector7d xs;
- SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
+ SensorBasePtr S = P->installSensor ("SensorOdom3d", "odometer", xs, wolf_root + "/test/yaml/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));
- auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(S, ProcessorTrackerFeatureDummy(params));
- // S->addProcessor(pt);
+ auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
// check motion processor IS NOT set
- ASSERT_FALSE(P->getProcessorMotion());
+ ASSERT_FALSE(P->getProcessorIsMotion());
// install processor motion
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// check motion processor is set
- ASSERT_TRUE(P->getProcessorMotion());
+ ASSERT_TRUE(P->getProcessorIsMotion() != nullptr);
// check motion processor is correct
- ASSERT_EQ(P->getProcessorMotion(), pm);
+ ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm);
}
-TEST(Problem, SetOrigin_PO_2D)
+TEST(Problem, SetOrigin_PO_2d)
{
ProblemPtr P = Problem::create("PO", 2);
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
+ // Eigen::VectorXd x0(3); x0 << 1,2,3;
+ VectorComposite x0(Vector3d(1,2,3), "PO", {2,1});
+ // Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- P->setPrior(x0, P0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0, 1.0);
+ WOLF_INFO("printing.-..");
+ P->print(4,1,1,1);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (SizeStd) 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());
+ FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ FactorBasePtrList fac_list;
+ F->getFactorList(fac_list);
+
+ // check that we have one frame (prior)
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
+ // check that we have one capture (prior)
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
+ // check that we have two features (prior: one per state block)
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
+ // check that we have two factors (prior: one per state block)
+ ASSERT_EQ(fac_list.size(), (SizeStd) 2);
+
+ // check that the factors are absolute (no pointers to other F, f, or L)
+ for (auto fac : fac_list)
+ {
+ ASSERT_FALSE(fac->getFrameOther());
+ ASSERT_FALSE(fac->getLandmarkOther());
+ ASSERT_FALSE(fac->getCaptureOther());
+ ASSERT_FALSE(fac->getFeatureOther());
+ }
+ auto x0_vector = x0.vector("PO");
+ auto s0_vector = s0.vector("PO");
+ MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
-TEST(Problem, SetOrigin_PO_3D)
+TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
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
+ Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
+ VectorComposite x0(vec7, "PO", {3,4});
+ // Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
+ VectorComposite s0(vec6, "PO", {3,3});
- P->setPrior(x0, P0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0, 1.0);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((x0.vector("PO") - P->getState().vector("PO")).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(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (SizeStd) 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());
-
+ // FeatureBasePtr fo = C->getFeatureList().front();
+ // FeatureBasePtr fp = C->getFeatureList().back();
+ FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ FactorBasePtrList fac_list;
+ F->getFactorList(fac_list);
+
+ // check that we have one frame (prior)
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
+ // check that we have one capture (prior)
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
+ // check that we have two features (prior: one per state block)
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
+ // check that we have two factors (prior: one per state block)
+ ASSERT_EQ(fac_list.size(), (SizeStd) 2);
+
+ // check that the factors are absolute (no pointers to other F, f, or L)
+ for (auto fac : fac_list)
+ {
+ ASSERT_FALSE(fac->getFrameOther());
+ ASSERT_FALSE(fac->getLandmarkOther());
+ ASSERT_FALSE(fac->getCaptureOther());
+ ASSERT_FALSE(fac->getFeatureOther());
+ }
+
+ auto x0_vector = x0.vector("PO");
+ auto s0_vector = s0.vector("PO");
+ MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
// 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));
+ ASSERT_MATRIX_APPROX(x0_vector.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -213,25 +221,24 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceFrame("PO", 2, KEY, VectorXs(3), TimeStamp(0.0));
- FrameBasePtr f1 = P->emplaceFrame("PO", 3, KEY, VectorXs(7), TimeStamp(1.0));
- FrameBasePtr f2 = P->emplaceFrame("POV", 3, KEY, VectorXs(10), TimeStamp(2.0));
+ FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2, VectorXd(3) );
+ FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, VectorXd(7) );
+ FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, VectorXd(10) );
- // 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(), (SizeStd) 3);
+ ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
+
+ ASSERT_EQ(f0->getStateVector().size(), 3 );
+ ASSERT_EQ(f1->getStateVector().size(), 7 );
+ ASSERT_EQ(f2->getStateVector().size(), 10);
// check that all frames are linked to Problem
ASSERT_EQ(f0->getProblem(), P);
ASSERT_EQ(f1->getProblem(), P);
ASSERT_EQ(f2->getProblem(), P);
+
+ ASSERT_EQ(P->getFrameStructure(), "POV");
}
TEST(Problem, StateBlocks)
@@ -239,28 +246,23 @@ TEST(Problem, StateBlocks)
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7s xs3d;
- Eigen::Vector3s xs2d;
+ Eigen::Vector7d xs3d; xs3d << 1,2,3,0,0,0,1; // required normalized quaternion (solver manager checks this)
+ Eigen::Vector3d xs2d;
// 2 state blocks, fixed
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom3d", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
- // 2 state blocks, fixed
- SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
+// // 2 state blocks, fixed
+// SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, "");
+// ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = 0.1;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 10;
-
- auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
+ auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 2 state blocks, estimated
- auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2));
+ auto KF = P->emplaceFrame(0, "PO", 3, xs3d );
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
// Notifications
Notification notif;
@@ -271,27 +273,29 @@ TEST(Problem, StateBlocks)
// P->print(4,1,1,1);
// change some SB properties
- St->unfixExtrinsics();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
+ Sm->unfixExtrinsics();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
- // P->print(4,1,1,1);
+ P->print(4,1,1,1);
// consume notifications
- DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2));
- SM->update(); // calls P->consumeStateBlockNotificationMap();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map
+ SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
+ SM->update(); // calls P->consumeNotifications();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consumeNotifications empties the notification map
// remove frame
auto SB_P = KF->getP();
auto SB_O = KF->getO();
+ WOLF_INFO("removing frame...");
KF->remove();
+ WOLF_INFO("removed");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2));
ASSERT_TRUE(P->getStateBlockNotification(SB_P, notif));
ASSERT_EQ(notif, REMOVE);
ASSERT_TRUE(P->getStateBlockNotification(SB_O, notif));
ASSERT_EQ(notif, REMOVE);
-
+ WOLF_INFO("test end");
}
TEST(Problem, Covariances)
@@ -299,43 +303,268 @@ TEST(Problem, Covariances)
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7s xs3d;
- Eigen::Vector3s xs2d;
+ Eigen::Vector7d xs3d;
+ Eigen::Vector3d xs2d;
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
- SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom3d", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ // SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr St = P->installSensor ("SensorOdom3d", "other odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
+ ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>();
params->time_tolerance = 0.1;
params->max_new_features = 5;
params->min_features_for_keyframe = 10;
- // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
- auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
- // St->addProcessor(pt);
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", Sm, params);
+ auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
+ FrameBasePtr F = P->emplaceFrame(0, "PO", 3, xs3d );
// set covariance (they are not computed without a solver)
- P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
- P->addCovarianceBlock(F->getO(), Eigen::Matrix3s::Identity());
- P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3s::Zero());
+ P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
+ P->addCovarianceBlock(F->getO(), Eigen::Matrix3d::Identity());
+ P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3d::Zero());
// get covariance
- Eigen::MatrixXs Cov;
+ Eigen::MatrixXd Cov;
ASSERT_TRUE(P->getFrameCovariance(F, Cov));
ASSERT_EQ(Cov.cols() , 6);
ASSERT_EQ(Cov.rows() , 6);
- ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6s::Identity(), 1e-12);
+ ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6d::Identity(), 1e-12);
+
+}
+TEST(Problem, perturb)
+{
+ auto problem = Problem::create("PO", 2);
+
+ // make a sensor first
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0;
+ intr->radius_right = 1.0;
+ intr->wheel_separation = 1.0;
+ intr->ticks_per_wheel_revolution = 100;
+ Vector3d extr(0,0,0);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+ sensor->setStateBlockDynamic('I', true);
+
+ Vector3d pose; pose << 0,0,0;
+
+ int i = 0;
+ for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+ {
+ auto F = problem->emplaceFrame(t, pose );
+ if (i==0) F->fix();
+
+ for (int j = 0; j< 2 ; j++)
+ {
+ auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto cap = CaptureBase::emplace<CaptureBase>(F, "CaptureBase", t, sensor, nullptr, nullptr, sb);
+ }
+ i++;
+ }
+
+ for (int l = 0; l < 2; l++)
+ {
+ auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
+ if (l==0) L->fix();
+ }
+
+ /// PRINT, PERTURB, AND PRINT AGAIN
+ problem->print(2,0,1,1);
+
+ problem->perturb(0.2);
+
+ problem->print(2,0,1,1);
+
+
+ //// CHECK ALL STATE BLOCKS ///
+
+#define prec 1e-2
+
+ for (auto S : problem->getHardware()->getSensorList())
+ {
+ ASSERT_MATRIX_APPROX(S->getP()->getState(), Vector2d(0,0), prec );
+ ASSERT_MATRIX_APPROX(S->getO()->getState(), Vector1d(0), prec );
+ ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
+ }
+
+ for (auto pair_T_F : problem->getTrajectory()->getFrameMap())
+ {
+ auto F = pair_T_F.second;
+ if (F->isFixed()) // fixed
+ {
+ ASSERT_MATRIX_APPROX (F->getP()->getState(), Vector2d(0,0), prec );
+ ASSERT_MATRIX_APPROX (F->getO()->getState(), Vector1d(0), prec );
+ }
+ else
+ {
+ ASSERT_FALSE(F->getP()->getState().isApprox(Vector2d(0,0), prec) );
+ ASSERT_FALSE(F->getO()->getState().isApprox(Vector1d(0), prec) );
+ }
+ for (auto C : F->getCaptureList())
+ {
+ // all are estimated
+ ASSERT_FALSE(C->getSensorIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
+ }
+ }
+ for (auto L : problem->getMap()->getLandmarkList())
+ {
+ if ( L->isFixed() ) // fixed
+ {
+ ASSERT_MATRIX_APPROX (L->getP()->getState(), Vector2d(1,2), prec );
+ ASSERT_MATRIX_APPROX (L->getO()->getState(), Vector1d(3), prec );
+ }
+ else
+ {
+ ASSERT_FALSE(L->getP()->getState().isApprox(Vector2d(1,2), prec) );
+ ASSERT_FALSE(L->getO()->getState().isApprox(Vector1d(3), prec) );
+ }
+ }
}
+TEST(Problem, check)
+{
+ auto problem = Problem::create("PO", 2);
+
+ // make a sensor first
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0;
+ intr->radius_right = 1.0;
+ intr->wheel_separation = 1.0;
+ intr->ticks_per_wheel_revolution = 100;
+ Vector3d extr(0,0,0);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+ sensor->setStateBlockDynamic('I', true);
+
+ Vector3d pose; pose << 0,0,0;
+
+ int i = 0;
+ for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+ {
+ auto F = problem->emplaceFrame(t, pose);
+ if (i==0) F->fix();
+
+ for (int j = 0; j< 2 ; j++)
+ {
+ auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto cap = CaptureBase::emplace<CaptureDiffDrive>(F, t, sensor, Vector2d(1,2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
+
+ for (int k = 0; k<2; k++)
+ {
+ Vector3d d(1,2,3), c(1,1,1);
+ Matrix3d D = Matrix3d::Identity(), J=Matrix3d::Zero();
+ auto fea = FeatureBase::emplace<FeatureMotion>(cap, "FeatureDiffDrive", d, D, c, J);
+ auto fac = FactorBase::emplace<FactorDiffDrive>(fea, fea, cap, nullptr, false);
+ }
+ }
+ i++;
+ }
+
+ for (int l = 0; l < 2; l++)
+ {
+ auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
+ if (l==0) L->fix();
+ }
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ // remove stuff from problem, and re-check
+
+ auto F = problem->getLastFrame();
+
+ auto cby = F->getConstrainedByList().front();
+
+ cby->remove(true);
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ F->remove();
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+}
+
+TEST(Problem, getState)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_odom_3d.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ auto S = P->getHardware()->getSensorList().front();
+
+ auto C = std::make_shared<CaptureOdom3d>(0.0, S, 0.1*Vector6d::Ones(), 0.01*Matrix6d::Identity());
+
+ for (TimeStamp t = 0.0; t <= 3.9; t += 0.1)
+ {
+ C->setTimeStamp(t);
+ C->process();
+ }
+ P->print(4,1,1,1);
+
+ // get at t = origin
+ WOLF_DEBUG("P (0) = ", P->getState(0, "P")); // partial structure
+ WOLF_DEBUG("PO(0) = ", P->getState(0, "PO")); // all but explicit structure
+ WOLF_DEBUG("x (0) = ", P->getState(TimeStamp(0))); // own structure
+ ASSERT_EQ(P->getState(0, "P").size(), 1);
+ ASSERT_EQ(P->getState(0, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(0)).size(), 2);
+
+ // get at t = before KF
+ WOLF_DEBUG("P (1) = ", P->getState(1, "P"));
+ WOLF_DEBUG("PO(1) = ", P->getState(1, "PO"));
+ WOLF_DEBUG("x (1) = ", P->getState(1));
+ ASSERT_EQ(P->getState(1, "P").size(), 1);
+ ASSERT_EQ(P->getState(1, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(1)).size(), 2);
+
+ // get at t = KF
+ WOLF_DEBUG("P (2) = ", P->getState(2, "P"));
+ WOLF_DEBUG("PO(2) = ", P->getState(2, "PO"));
+ WOLF_DEBUG("x (2) = ", P->getState(2));
+ ASSERT_EQ(P->getState(2, "P").size(), 1);
+ ASSERT_EQ(P->getState(2, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(2)).size(), 2);
+
+ // get at t = after last KF
+ WOLF_DEBUG("P (3) = ", P->getState(3, "P"));
+ WOLF_DEBUG("PO(3) = ", P->getState(3, "PO"));
+ WOLF_DEBUG("x (3) = ", P->getState(3));
+ ASSERT_EQ(P->getState(3, "P").size(), 1);
+ ASSERT_EQ(P->getState(3, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(3)).size(), 2);
+
+ // get at t = last processed capture
+ WOLF_DEBUG("P (3.9) = ", P->getState(3.9, "P"));
+ WOLF_DEBUG("PO(3.9) = ", P->getState(3.9, "PO"));
+ WOLF_DEBUG("x (3.9) = ", P->getState(3.9));
+ ASSERT_EQ(P->getState(3.9, "P").size(), 1);
+ ASSERT_EQ(P->getState(3.9, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(3.9)).size(), 2);
+
+ // get at t = current state
+ WOLF_DEBUG("P () = ", P->getState("P"));
+ WOLF_DEBUG("PO() = ", P->getState("PO"));
+ WOLF_DEBUG("x () = ", P->getState());
+ ASSERT_EQ(P->getState("P").size(), 1);
+ ASSERT_EQ(P->getState("PO").size(), 2);
+ ASSERT_EQ(P->getState().size(), 2);
+
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec7700ae21723253d5c5931d84ff4c151f8644cb
--- /dev/null
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -0,0 +1,326 @@
+/**
+ * \file gtest_factor_pose_with_extrinsics.cpp
+ *
+ * Created on: Feb 19, 2020
+ * \author: mfourmy
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/processor/processor_pose.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+
+class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
+{
+ /**
+ Factor graph implemented common for all the tests
+ 2 KF is not enough since the SensorPose extrinsics translation would be unobservable along the first KF1-KF2 transformation axis of rotation
+ (KF1)----|Odom3d|----(KF2)----|Odom3d|----(KF3)
+ | | |
+ | | |
+ |Pose3dWE| |Pose3dWE| |Pose3dWE|
+ \ /
+ \ /
+ \____________(Extrinsics)_______________/
+ */
+
+ // In this class, the poses of the 3 KF are chosen randomly and then measurement data for the factors are
+ // derived from random extrinsics
+ // The problem and Pose Sensor/Processor are implemented
+
+ public:
+
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ SensorBasePtr sensor_pose_;
+ FrameBasePtr KF1_;
+ FrameBasePtr KF2_;
+ FrameBasePtr KF3_;
+
+ Vector7d pose1_;
+ Vector7d pose2_;
+ Vector7d pose3_;
+ Vector7d pose_12_;
+ Vector7d pose_23_;
+ Vector3d b_p_bm_;
+ Quaterniond b_q_m_;
+
+ Vector7d pose1_meas_;
+ Vector7d pose2_meas_;
+ Vector7d pose3_meas_;
+
+ void SetUp() override
+ {
+ // 2 random 3d positions
+ Vector3d w_p_wb1 = Vector3d::Random();
+ Quaterniond w_q_b1 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb2 = Vector3d::Random();
+ Quaterniond w_q_b2 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb3 = Vector3d::Random();
+ Quaterniond w_q_b3 = Quaterniond::UnitRandom();
+
+ // pose vectors
+ pose1_ << w_p_wb1, w_q_b1.coeffs();
+ pose2_ << w_p_wb2, w_q_b2.coeffs();
+ pose3_ << w_p_wb3, w_q_b3.coeffs();
+
+ /////////////////////////////////////////
+ // extrinsics ground truth and mocap data
+ // b_p_bm_ << 0.1,0.2,0.3;
+ b_p_bm_ << Vector3d::Random();
+ b_q_m_ = Quaterniond::UnitRandom();
+
+ Vector3d w_p_wb1_meas = w_p_wb1 + w_q_b1 * b_p_bm_;
+ Vector3d w_p_wb2_meas = w_p_wb2 + w_q_b2 * b_p_bm_;
+ Vector3d w_p_wb3_meas = w_p_wb3 + w_q_b3 * b_p_bm_;
+ Quaterniond w_q_b1_meas = w_q_b1 * b_q_m_;
+ Quaterniond w_q_b2_meas = w_q_b2 * b_q_m_;
+ Quaterniond w_q_b3_meas = w_q_b3 * b_q_m_;
+ pose1_meas_ << w_p_wb1_meas, w_q_b1_meas.coeffs();
+ pose2_meas_ << w_p_wb2_meas, w_q_b2_meas.coeffs();
+ pose3_meas_ << w_p_wb3_meas, w_q_b3_meas.coeffs();
+
+ /////////////////////
+ // relative odom data
+ Vector3d b1_p_b1b2 = w_q_b1.conjugate()*(w_p_wb2 - w_p_wb1);
+ Quaterniond b1_q_b2 = w_q_b1.conjugate()*w_q_b2;
+ pose_12_ << b1_p_b1b2, b1_q_b2.coeffs();
+
+ Vector3d b2_p_b2b3 = w_q_b2.conjugate()*(w_p_wb3 - w_p_wb2);
+ Quaterniond b2_q_b3 = w_q_b2.conjugate()*w_q_b3;
+ pose_23_ << b2_p_b2b3, b2_q_b3.coeffs();
+
+ // Problem and solver_
+ problem_ = Problem::create("PO", 3);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ intr_sp->std_p = 1;
+ intr_sp->std_o = 1;
+ Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->time_tolerance = 0.5;
+ auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
+ // somehow by default the sensor extrinsics is fixed
+ sensor_pose_->unfixExtrinsics();
+ }
+
+ void TearDown() override{};
+};
+
+class FactorPose3dWithExtrinsicsMANUAL_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+
+ ///////////////////
+ // Create factor graph
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // Captures mocap
+ auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1_, 1, sensor_pose_, pose1_meas_, cov6d);
+ auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap1 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap1, fea_mocap1, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap2 = CaptureBase::emplace<CapturePose>(KF2_, 2, sensor_pose_, pose2_meas_, cov6d);
+ auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap3 = CaptureBase::emplace<CapturePose>(KF3_, 3, sensor_pose_, pose3_meas_, cov6d);
+ auto fea_mocap3 = FeatureBase::emplace<FeatureBase>(cap_mocap3, "pose", pose3_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap3 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap3, fea_mocap3, nullptr, false, TOP_MOTION);
+
+
+ }
+
+ void TearDown() override{};
+};
+
+
+class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+ problem_->keyFrameCallback(KF1_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF2_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+
+ ///////////////////
+ // Create factor graph
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // process mocap data
+ auto cap_mocap1 = std::make_shared<CapturePose>(1, sensor_pose_, pose1_meas_, cov6d);
+ auto cap_mocap2 = std::make_shared<CapturePose>(2, sensor_pose_, pose2_meas_, cov6d);
+ auto cap_mocap3 = std::make_shared<CapturePose>(3, sensor_pose_, pose3_meas_, cov6d);
+ cap_mocap1->process();
+ cap_mocap2->process();
+ cap_mocap3->process();
+
+ }
+
+ void TearDown() override{};
+};
+
+
+class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ ///////////////////
+ // Create factor graph
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // process mocap data
+ auto cap_mocap1 = std::make_shared<CapturePose>(1, sensor_pose_, pose1_meas_, cov6d);
+ auto cap_mocap2 = std::make_shared<CapturePose>(2, sensor_pose_, pose2_meas_, cov6d);
+ auto cap_mocap3 = std::make_shared<CapturePose>(3, sensor_pose_, pose3_meas_, cov6d);
+ cap_mocap1->process();
+ cap_mocap2->process();
+ cap_mocap3->process();
+
+ // keyframe callback called after all mocap captures have been processed
+ problem_->keyFrameCallback(KF1_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF2_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+ }
+
+ void TearDown() override{};
+};
+
+
+
+
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, error)
+{
+ // Since initialized at the right state, error function should return 0
+ std::vector<FrameBasePtr> KF_vec = {KF1_, KF2_, KF3_};
+ for (auto KF: KF_vec){
+ for (auto fac: KF->getFactorList()){
+ auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
+ if (f){
+ ASSERT_MATRIX_APPROX(f->error(), Vector6d::Zero(), 1e-6);
+ }
+ }
+ }
+}
+
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
+{
+ problem_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+}
+
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve)
+{
+ problem_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+
+}
+
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve)
+{
+ problem_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 9128e1ee66c9a1442a87a2998ec43ba29878f2bf..4497954899991b26100d23af38130e8218ed4a14 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -6,12 +6,12 @@
*/
//Wolf
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
+#include "dummy/processor_tracker_feature_dummy.h"
#include "core/capture/capture_void.h"
#include "core/problem/problem.h"
@@ -23,6 +23,47 @@
using namespace wolf;
using namespace Eigen;
+TEST(ProcessorBase, IsMotion)
+{
+ using namespace wolf;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using Eigen::Vector2d;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ double dt = 0.01;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ // Install tracker (sensor and processor)
+ auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorDummy",
+ nullptr,
+ nullptr,
+ nullptr,
+ 2);
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
+
+ // Install odometer (sensor and processor)
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d",
+ "odometer",
+ Vector3d(0,0,0),
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
+ proc_odo_params->time_tolerance = dt/2;
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d",
+ "odom processor",
+ sens_odo,
+ proc_odo_params);
+
+ ASSERT_FALSE(proc_trk->isMotion());
+ ASSERT_TRUE (proc_odo->isMotion());
+}
+
+
TEST(ProcessorBase, KeyFrameCallback)
{
@@ -30,36 +71,30 @@ TEST(ProcessorBase, KeyFrameCallback)
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
- using Eigen::Vector2s;
+ using Eigen::Vector2d;
- Scalar dt = 0.01;
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ double dt = 0.01;
// Wolf problem
ProblemPtr problem = Problem::create("PO", 2);
// 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);
-
- auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(), "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);
- auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, params);
-
- // problem->addSensor(sens_trk);
- // sens_trk->addProcessor(proc_trk);
+ auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorTrackerDummy",
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
+ proc_trk_params->time_tolerance = dt/2;
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk, proc_trk_params);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
- ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odometer", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
@@ -67,33 +102,45 @@ TEST(ProcessorBase, KeyFrameCallback)
// initialize
TimeStamp t(0.0);
- Vector3s x(0,0,0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
- problem->setPrior(x, P, t, dt/2); // KF1
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ problem->setPriorFactor(x, P, t, dt/2); // KF1
- CaptureOdom2DPtr capt_odo = make_shared<CaptureOdom2D>(t, sens_odo, Vector2s(0.5,0));
+ CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
// Track
CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
- proc_trk->process(capt_trk);
+ proc_trk->captureCallback(capt_trk);
for (size_t ii=0; ii<10; ii++ )
{
// Move
t = t+dt;
WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+ std::cout << "1\n";
capt_odo->setTimeStamp(t);
- proc_odo->process(capt_odo);
+ std::cout << "2\n";
+// auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo);
+// auto last_ptr = proc_odo_motion->last_ptr_;
+// auto last_ptr_frame = last_ptr->getFrame();
+ proc_odo->captureCallback(capt_odo);
+ std::cout << "3\n";
// Track
capt_trk = make_shared<CaptureVoid>(t, sens_trk);
- proc_trk->process(capt_trk);
+ std::cout << "4\n";
+ problem->print(4,1,1,1, std::cout);
+ proc_trk->captureCallback(capt_trk);
+ std::cout << "5\n";
-// problem->print(4,1,1,0);
+ problem->print(4,1,1,0);
+ std::cout << "6\n";
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 );
+ ASSERT_TRUE( problem->getLastFrame()->getStructure().compare("PO")==0 );
}
}
@@ -102,4 +149,3 @@ int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc30487fe96765f36caea276defddc7abee9a6ac
--- /dev/null
+++ b/test/gtest_processor_diff_drive.cpp
@@ -0,0 +1,407 @@
+/**
+ * \file gtest_processor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
+
+#include "core/processor/processor_diff_drive.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/utils/utils_gtest.h"
+
+#include "core/capture/capture_diff_drive.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorDiffDrivePublic);
+
+class ProcessorDiffDrivePublic : public ProcessorDiffDrive
+{
+ using Base = ProcessorDiffDrive;
+ public:
+ ProcessorDiffDrivePublic(ParamsProcessorDiffDrivePtr _params_motion) :
+ ProcessorDiffDrive(_params_motion)
+ {
+ //
+ }
+ void configure(SensorBasePtr _sensor) override
+ {
+ Base::configure(_sensor);
+ }
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override
+ {
+ Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
+ }
+
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+ }
+
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+ }
+
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const override
+ {
+ Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
+ }
+
+ Eigen::VectorXd deltaZero() const override
+ {
+ return Base::deltaZero();
+ }
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override
+ {
+ return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
+ }
+
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+ {
+ return Base::emplaceFeature(_capture_own);
+ }
+
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin) override
+ {
+ return Base::emplaceFactor(_feature_motion, _capture_origin);
+ }
+
+ ParamsProcessorDiffDrivePtr getParams()
+ {
+ return params_prc_diff_drv_selfcal_;
+ }
+
+ double getRadPerTick()
+ {
+ return radians_per_tick_;
+ }
+
+};
+
+class ProcessorDiffDriveTest : public testing::Test
+{
+ public:
+ ParamsSensorDiffDrivePtr intr;
+ SensorDiffDrivePtr sensor;
+ ParamsProcessorDiffDrivePtr params;
+ ProcessorDiffDrivePublicPtr processor;
+ ProblemPtr problem;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3d extr(0,0,0);
+ sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+
+ // params and processor
+ params = std::make_shared<ParamsProcessorDiffDrive>();
+ params->voting_active = true;
+ params->angle_turned = 1;
+ params->dist_traveled = 2;
+ params->max_buff_length = 3;
+ params->max_time_span = 2.5;
+ params->unmeasured_perturbation_std = 1e-4;
+ processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(problem->installProcessor("ProcessorDiffDrive", "processor diff drive", sensor, params));
+ }
+
+ void TearDown() override{}
+
+};
+
+TEST(ProcessorDiffDrive, constructor)
+{
+ auto params = std::make_shared<ParamsProcessorDiffDrive>();
+
+ ASSERT_NE(params, nullptr);
+
+ auto prc = std::make_shared<ProcessorDiffDrive>(params);
+
+ ASSERT_NE(prc, nullptr);
+
+ ASSERT_EQ(prc->getType(), "ProcessorDiffDrive");
+}
+
+TEST(ProcessorDiffDrive, create)
+{
+ // make a sensor first
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ Vector3d extr(1,2,3);
+ auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+
+ // params
+ auto params = std::make_shared<ParamsProcessorDiffDrive>();
+
+ // processor
+ auto prc_base = ProcessorDiffDrive::create("prc", params);
+
+ auto prc = std::static_pointer_cast<ProcessorDiffDrive>(prc_base);
+
+ ASSERT_NE(prc, nullptr);
+
+ ASSERT_EQ(prc->getType(), "ProcessorDiffDrive");
+}
+
+TEST_F(ProcessorDiffDriveTest, params)
+{
+ // read
+ ASSERT_EQ(processor->getParams()->angle_turned , 1.0);
+ ASSERT_EQ(processor->getParams()->dist_traveled, 2.0);
+
+ // write
+ processor->getParams()->angle_turned = 5;
+ ASSERT_EQ(processor->getParams()->angle_turned, 5.0);
+}
+
+TEST_F(ProcessorDiffDriveTest, configure)
+{
+ processor->configure(sensor);
+ ASSERT_EQ(processor->getRadPerTick(), 2.0*M_PI/100); // 100 ticks per revolution
+}
+
+TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
+{
+ // 1. zero delta
+ Vector2d data(0,0);
+ Matrix2d data_cov; data_cov . setIdentity();
+ Vector3d calib(1,1,1);
+ double dt = 1.0;
+ VectorXd delta(3);
+ MatrixXd delta_cov(3,3), J_delta_calib(3,3);
+
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ ASSERT_MATRIX_APPROX(delta, Vector3d::Zero(), 1e-8);
+
+ // 2. left turn 90 deg --> ends up in (1.5, 1.5, pi/2)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+
+ // 2. right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ data(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+}
+
+TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
+{
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
+ Vector3d calib(1,1,1);
+ double dt = 1.0;
+ VectorXd delta(3), delta1(3), delta2(3);
+ MatrixXd delta_cov(3,3), J_delta_calib(3,3);
+
+ // 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / 3;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 30
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 60
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
+
+ ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+
+ // 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / 4;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 22.5
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 45
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 67.5
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
+
+ ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+}
+
+TEST_F(ProcessorDiffDriveTest, statePlusDelta)
+{
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
+ Vector3d calib(1,1,1);
+ double dt = 1.0;
+ VectorXd delta(3);
+ VectorComposite x1("PO", {2,1}), x2("PO", {2,1});
+ MatrixXd delta_cov(3,3), J_delta_calib(3,3);
+
+ // 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / 3;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ x1 .setZero();
+ processor->statePlusDelta(x1, delta, dt, x2); // 30
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 60
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 90
+
+ ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+
+ // 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / 4;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ x1 .setZero();
+ processor->statePlusDelta(x1, delta, dt, x2); // 22.5
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 45
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 67.5
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 90
+
+ ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+}
+
+TEST_F(ProcessorDiffDriveTest, process)
+{
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
+ TimeStamp t = 0.0;
+ double dt = 1.0;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
+
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
+
+ // 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
+ int N = 9;
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+
+ for (int n = 1; n <= N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
+ }
+
+ problem->print(4,1,1,1);
+}
+
+TEST_F(ProcessorDiffDriveTest, linear)
+{
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
+ TimeStamp t = 0.0;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
+
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
+
+ // Straight one turn of the wheels, in one go
+ data(0) = 100.0 ; // one turn of the wheels
+ data(1) = 100.0 ;
+
+ t += 1.0;
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+
+ C->process();
+
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
+
+ // radius is 1.0m, 100 ticks per revolution, so advanced distance is
+ double distance = 2 * M_PI * 1.0;
+
+ ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,0), 1e-6)
+}
+
+TEST_F(ProcessorDiffDriveTest, angular)
+{
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
+ TimeStamp t = 0.0;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
+
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
+
+ // Straight one turn of the wheels, in one go
+ t += 1.0;
+ data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
+ data(1) = 20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+
+ C->process();
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
+
+ // this is a turn in place, so distance = 0;
+ double distance = 0.0;
+
+ // radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is
+ double angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5);
+
+ ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,angle), 1e-6)
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fix_wing_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e817a9c400c53cdd569c72de90c672ac703fdaf4
--- /dev/null
+++ b/test/gtest_processor_fix_wing_model.cpp
@@ -0,0 +1,136 @@
+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/processor/processor_fix_wing_model.h"
+#include "core/state_block/state_quaternion.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class ProcessorFixWingModelTest : public testing::Test
+{
+ protected:
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor;
+ ProcessorBasePtr processor;
+
+ virtual void SetUp()
+ {
+ // create problem
+ problem = Problem::create("POV", 3);
+
+ // create solver
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // Emplace sensor
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StateBlock>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>((Vector4d() << 0,0,0,1).finished()),
+ nullptr,
+ 2);
+
+ // Emplace processor
+ ParamsProcessorFixWingModelPtr params = std::make_shared<ParamsProcessorFixWingModel>();
+ params->velocity_local = (Vector3d() << 1, 0, 0).finished();
+ params->angle_stdev = 0.1;
+ params->min_vel_norm = 0;
+ processor = ProcessorBase::emplace<ProcessorFixWingModel>(sensor, params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+};
+
+TEST_F(ProcessorFixWingModelTest, setup)
+{
+ EXPECT_TRUE(problem->check());
+}
+
+TEST_F(ProcessorFixWingModelTest, keyFrameCallback)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // check one capture
+ ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
+ auto cap = frm1->getCaptureOf(sensor);
+ ASSERT_TRUE(cap != nullptr);
+
+ // check one feature
+ ASSERT_EQ(cap->getFeatureList().size(), 1);
+
+ // check one factor
+ auto fac = frm1->getFactorOf(processor, "FactorVelocityLocalDirection3d");
+ ASSERT_TRUE(fac != nullptr);
+ ASSERT_TRUE(fac->getFeature() != nullptr);
+ ASSERT_TRUE(fac->getCapture() == cap);
+}
+
+TEST_F(ProcessorFixWingModelTest, keyFrameCallbackRepeated)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // repeated keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // check one capture
+ ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
+ auto cap = frm1->getCaptureOf(sensor);
+ ASSERT_TRUE(cap != nullptr);
+
+ // check one feature
+ ASSERT_EQ(cap->getFeatureList().size(), 1);
+
+ // check one factor
+ auto fac = frm1->getFactorOf(processor, "FactorVelocityLocalDirection3d");
+ ASSERT_TRUE(fac != nullptr);
+ ASSERT_TRUE(fac->getFeature() != nullptr);
+ ASSERT_TRUE(fac->getCapture() == cap);
+}
+
+TEST_F(ProcessorFixWingModelTest, solve_origin)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // perturb
+ frm1->getP()->fix();
+ frm1->getO()->fix();
+ frm1->getV()->unfix();
+ frm1->getV()->setState(Vector3d::Random());
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ WOLF_INFO(report);
+
+ ASSERT_GT(frm1->getV()->getState()(0), 0);
+ ASSERT_NEAR(frm1->getV()->getState()(1), 0, Constants::EPS);
+ ASSERT_NEAR(frm1->getV()->getState()(2), 0, Constants::EPS);
+}
+
+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 95b16425310926f60e6fbe29d4ce16629d90f55e..0000000000000000000000000000000000000000
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ /dev/null
@@ -1,228 +0,0 @@
-
-/**
- * \file gtest_processor_frame_nearest_neighbor_filter.cpp
- *
- * Created on: Aug 2, 2017
- * \author: tessajohanna
- */
-
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
-
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/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", 2);
-
-// 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;
-
-
- // create Keyframes
- F1 = problem->emplaceFrame(wolf::KEY, state1, 1);
- F2 = problem->emplaceFrame(wolf::KEY, state2, 2);
- F3 = problem->emplaceFrame(wolf::KEY, state3, 3);
- F4 = problem->emplaceFrame(wolf::KEY, state4, 4);
-
- auto stateblock_pptr1 = F1->getP();
- auto stateblock_optr1 = F1->getO();
-
- auto stateblock_pptr2 = F2->getP();
- auto stateblock_optr2 = F2->getO();
-
- auto stateblock_pptr3 = F3->getP();
- auto stateblock_optr3 = F3->getO();
-
- auto stateblock_pptr4 = F4->getP();
- auto stateblock_optr4 = F4->getO();
-
- // add dummy capture
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F1, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F2, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F3, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F4, "DUMMY", 1.0, sensor_ptr);
-
- // 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 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 = wolf::CaptureBase::emplace<wolf::CaptureBase>(nullptr, "DUMMY", 1.2, sensor_ptr);
-
- // Make 3rd frame last Key frame
- F3->setTimeStamp(wolf::TimeStamp(2.0));
- problem->getTrajectory()->sortFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_point2d->process(incomming_dummy);
-
- // const std::vector<wolf::FrameBasePtr> &testloops =
- // processor_ptr_point2d->getCandidates();
-
- //TODO: Due to changes in the emplace refactor these tests are not working. To be fixed.
- // 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 Key frame
- F3->setTimeStamp(wolf::TimeStamp(2.0));
- problem->getTrajectory()->sortFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_ellipse2d->process(incomming_dummy);
- const std::vector<wolf::FrameBasePtr> &testloops =
- processor_ptr_ellipse2d->getCandidates();
-
- //TODO: Due to changes in the emplace refactor these tests are not working. To be fixed.
- // 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 Key frame
- F4->setTimeStamp(wolf::TimeStamp(3.0));
- problem->getTrajectory()->sortFrame(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 = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, 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 = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, 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_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aec8092de22eaee6a4577b43d8b7a10f8528582b
--- /dev/null
+++ b/test/gtest_processor_loop_closure.cpp
@@ -0,0 +1,327 @@
+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/capture/capture_base.h"
+#include "core/factor/factor_relative_pose_2d.h"
+
+#include "dummy/processor_loop_closure_dummy.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class ProcessorLoopClosureTest : public testing::Test
+{
+ protected:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ ProcessorLoopClosureDummyPtr processor;
+
+ virtual void SetUp()
+ {
+ // Emplace sensor
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()),
+ nullptr,
+ 2);
+
+ // Emplace processor
+ ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
+ params->time_tolerance = 0.5;
+ processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor,
+ params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureBasePtr emplaceCapture(FrameBasePtr frame)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureBase>(frame,
+ "CaptureBase",
+ frame->getTimeStamp(),
+ sensor);
+ }
+
+ CaptureBasePtr createCapture(TimeStamp ts)
+ {
+ // new capture
+ return std::make_shared<CaptureBase>("CaptureBase",
+ ts,
+ sensor);
+ }
+};
+
+TEST_F(ProcessorLoopClosureTest, installProcessor)
+{
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, frame_stored)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, capture_stored)
+{
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(2, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ auto frm3 = emplaceFrame(3, Vector3d::Zero());
+ auto frm4 = emplaceFrame(4, Vector3d::Zero());
+ auto frm5 = emplaceFrame(5, Vector3d::Zero());
+ // new captures
+ auto cap4 = createCapture(4);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+ problem->keyFrameCallback(frm3, nullptr, 0.5);
+ problem->keyFrameCallback(frm4, nullptr, 0.5);
+ problem->keyFrameCallback(frm5, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(frm1->getCaptureList().size(), 0);
+ EXPECT_EQ(frm2->getCaptureList().size(), 0);
+ EXPECT_EQ(frm3->getCaptureList().size(), 0);
+ EXPECT_EQ(frm4->getCaptureList().size(), 1);
+ EXPECT_EQ(frm5->getCaptureList().size(), 0);
+
+ EXPECT_TRUE(cap4->getFrame() == frm4);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1); // all oldest frames are removed from buffer
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
+{
+ // new frame
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ // new captures
+ auto cap1 = createCapture(1);
+ auto cap2 = createCapture(2);
+ auto cap3 = createCapture(3);
+ auto cap4 = createCapture(4);
+ auto cap5 = createCapture(5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ processor->captureCallback(cap2);
+ processor->captureCallback(cap3);
+ processor->captureCallback(cap4);
+ processor->captureCallback(cap5);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_TRUE(cap2->getFrame() == frm2);
+ EXPECT_TRUE(cap3->getFrame() == nullptr);
+ EXPECT_TRUE(cap4->getFrame() == nullptr);
+ EXPECT_TRUE(cap5->getFrame() == nullptr);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().size(), 0);
+ EXPECT_EQ(cap5->getFeatureList().size(), 0);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 4);
+}
+
+TEST_F(ProcessorLoopClosureTest, emplaceFactors)
+{
+ // emplace frame and capture
+ auto cap1 = emplaceCapture(emplaceFrame(1, Vector3d::Zero()));
+ processor->captureCallback(cap1);
+
+ auto cap2 = emplaceCapture(emplaceFrame(2, Vector3d::Ones()));
+ processor->captureCallback(cap2);
+
+ auto cap3 = emplaceCapture(emplaceFrame(3, 2*Vector3d::Ones()));
+ processor->captureCallback(cap3);
+
+ auto cap4 = emplaceCapture(emplaceFrame(4, Vector3d::Zero()));
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 1);
+ EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap4->getFrame()->getConstrainedByList().size(), 0);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 1);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(cap1->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap3->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().front()->getFactorList().size(), 1);
+
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().front(), cap4->getFeatureList().front()->getFactorList().front());
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 60d175a6d3d6ee66f58b4549f65f73f068b83703..92bf678d4ba06af85c6c0da5724f9b7f4908f186 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -5,71 +5,132 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
-#include "core/utils/logging.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
+
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
-class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
+class ProcessorOdom2dPublic : public ProcessorOdom2d
+{
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()
+ ProcessorOdom2dPublic(ParamsProcessorOdom2dPtr params) : ProcessorOdom2d(params)
{
+ //
+ }
+ ~ProcessorOdom2dPublic() override{}
+
+ void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
+ TimeStamp ts_split,
+ const FrameBasePtr& keyframe_target,
+ const wolf::CaptureMotionPtr& capture_target)
+ {
+ ProcessorOdom2d::splitBuffer(capture_source,
+ ts_split,
+ keyframe_target,
+ capture_target);
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorOdom2dPublic);
+
+class ProcessorMotion_test : public testing::Test{
+ public:
+ double dt;
+ ProblemPtr problem;
+ SensorOdom2dPtr sensor;
+ ProcessorOdom2dPublicPtr processor;
+ CaptureMotionPtr capture;
+ Vector2d data;
+ Matrix2d data_cov;
+
+ void SetUp() override
+ {
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
dt = 1.0;
problem = Problem::create("PO", 2);
- ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
- params->time_tolerance = 0.25;
+ sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"));
+ ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
+ params->time_tolerance = 0.5;
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);
+ processor = ProcessorBase::emplace<ProcessorOdom2dPublic>(sensor, params);
+ capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, nullptr);
}
- Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
- {
- return ProcessorMotion::interpolate(_ref, _second, _ts);
- }
+ void TearDown() override{}
- Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
- {
- return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
- }
+};
+TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
- virtual void TearDown(){}
+ 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->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, getState_structure)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ 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->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_TRUE (processor->getState("P").count('P'));
+ ASSERT_FALSE(processor->getState("P").count('O'));
+ ASSERT_FALSE(processor->getState("O").count('P'));
+ ASSERT_TRUE (processor->getState("O").count('O'));
+
+ WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState('V'));
+ ASSERT_EQ (processor->getState("V").size(), 0);
+}
-TEST_F(ProcessorMotion_test, IntegrateStraight)
+
+TEST_F(ProcessorMotion_test, getState_time_structure)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
data << 1, 0; // advance straight
data_cov.setIdentity();
TimeStamp t(0.0);
@@ -80,15 +141,237 @@ TEST_F(ProcessorMotion_test, IntegrateStraight)
capture->setTimeStamp(t);
capture->setData(data);
capture->setDataCovariance(data_cov);
- processor->process(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ capture->process();
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ problem->print(2,1,1,1);
+
+ ASSERT_TRUE (processor->getState(7, "P").count('P'));
+ ASSERT_FALSE(processor->getState(7, "P").count('O'));
+ ASSERT_FALSE(processor->getState(7, "O").count('P'));
+ ASSERT_TRUE (processor->getState(7, "O").count('O'));
+
+ WOLF_DEBUG("processor->getState(7, \"V\") = ", processor->getState(7, "V"));
+ ASSERT_EQ (processor->getState(7, "V").size(), 0);
+}
+
+
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, mergeCaptures)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<9,0,0).finished(), 1e-8);
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "CaptureOdom2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ // copy initial buffer
+ MotionBuffer initial_buffer = C_source->getBuffer();
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+
+ // merge captures
+ processor->mergeCaptures(C_target, C_source);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+
+ // Check that merged buffer is the initial one (before splitting)
+ EXPECT_EQ(C_source->getBuffer().size(), initial_buffer.size());
+
+ auto init_it = initial_buffer.begin();
+ auto res_it = C_source->getBuffer().begin();
+ while (init_it != initial_buffer.end())
+ {
+ EXPECT_EQ(init_it->data_size_, init_it->data_size_);
+ EXPECT_EQ(init_it->delta_size_, init_it->delta_size_);
+ EXPECT_EQ(init_it->cov_size_, init_it->cov_size_);
+ EXPECT_EQ(init_it->calib_size_, init_it->calib_size_);
+ EXPECT_EQ(init_it->ts_, init_it->ts_);
+
+ EXPECT_MATRIX_APPROX(init_it->data_ ,init_it->data_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->data_cov_,init_it->data_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_, init_it->delta_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_cov_, init_it->delta_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_integr_, init_it->delta_integr_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_integr_cov_, init_it->delta_integr_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_delta_, init_it->jacobian_delta_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_delta_integr_, init_it->jacobian_delta_integr_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_calib_, init_it->jacobian_calib_, 1e-12);
+
+ init_it++;
+ res_it++;
+ }
}
-TEST_F(ProcessorMotion_test, IntegrateCircle)
+TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
@@ -99,20 +382,89 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
capture->setTimeStamp(t);
capture->setData(data);
capture->setDataCovariance(data_cov);
- processor->process(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8);
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
}
-TEST_F(ProcessorMotion_test, Interpolate)
+TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
- data << 1, 2*M_PI/10; // advance in turn
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+}
+
+TEST_F(ProcessorMotion_test, splitBufferFixPrior)
+{
+ // Prior
TimeStamp t(0.0);
- std::vector<Motion> motions;
- motions.push_back(motionZero(t));
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
for (int i = 0; i<10; i++) // one full turn exactly
{
@@ -120,78 +472,62 @@ TEST_F(ProcessorMotion_test, Interpolate)
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());
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- TimeStamp tt = 2.2;
- Motion ref = motions[2];
- Motion second = motions[3];
- Motion interp = interpolate(ref, second, tt);
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+}
+
+TEST_F(ProcessorMotion_test, initOdometry)
+{
+ auto odometry = processor->getOdometry();
+
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+}
+
+TEST_F(ProcessorMotion_test, integrateOdometry)
+{
+ auto odometry = processor->getOdometry();
+
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
- 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);
+ data << 1,0;
+ capture->setData(data);
- tt = 2.6;
- interp = interpolate(ref, second, tt);
+ capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+ capture->process();
+ odometry = processor->getOdometry();
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(1,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
- 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);
+ capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+ capture->process();
+ odometry = processor->getOdometry();
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(2,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
}
-//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)
{
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..876a4450d65e0da3231f1f9b91b9e56e3b97ac69
--- /dev/null
+++ b/test/gtest_processor_odom_3d.cpp
@@ -0,0 +1,144 @@
+/**
+ * \file gtest_odom_3d.cpp
+ *
+ * Created on: Nov 11, 2016
+ * \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/common/wolf.h"
+
+
+#include "core/processor/processor_odom_3d.h"
+
+#include <iostream>
+
+#define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \
+{ VectorXd Do(7); \
+ prc_ptr->deltaPlusDelta(D, d, dt, Do); \
+ VectorXd dd(7); \
+ VectorXd DD(7); \
+ VectorXd 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();
+};
+ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ParamsProcessorOdom3d>())
+{
+ //
+}
+
+TEST(ProcessorOdom3d, computeCurrentDelta)
+{
+ // One instance of the processor to test
+ ProcessorOdom3dTest prc;
+
+ // input data
+ Vector6d data; data.setRandom();
+ double dt = 1; // irrelevant, just for the API.
+
+ // Build delta from Eigen tools
+ Vector3d data_dp = data.head<3>();
+ Vector3d data_do = data.tail<3>();
+ Vector3d delta_dp = data_dp;
+ Quaterniond delta_dq = v2q(data_do);
+ Vector7d delta;
+ delta.head<3>() = delta_dp;
+ delta.tail<4>() = delta_dq.coeffs();
+
+ double dvar = 0.1;
+ double rvar = 0.2;
+ Vector6d diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
+ Matrix6d data_cov = diag.asDiagonal();
+ Matrix6d delta_cov = data_cov;
+
+ // return values for data2delta()
+ VectorXd delta_ret(7);
+ MatrixXd delta_cov_ret(6,6);
+ MatrixXd jac_delta_calib(6,0);
+
+ // call the function under test
+ prc.computeCurrentDelta(data, data_cov, VectorXd::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;
+
+ VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
+ VectorXd 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
+ VectorXd D_int_check(7);
+ D_int_check.head<3>() = D.head<3>() + Quaterniond(D.data()+3) * d.head<3>();
+ D_int_check.tail<4>() = (Quaterniond(D.data()+3) * Quaterniond(d.data()+3)).coeffs();
+
+ double dt = 1; // dummy, not used in Odom3d
+
+ VectorXd D_int(7);
+
+ prc.deltaPlusDelta(D, d, dt, D_int);
+
+ ASSERT_MATRIX_APPROX(D_int , D_int_check, 1e-10);
+}
+
+TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
+{
+ std::shared_ptr<ProcessorOdom3dTest> prc_ptr = std::make_shared<ProcessorOdom3dTest>();
+
+ VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
+ VectorXd d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
+ double dt = 1;
+ VectorXd Do(7);
+ MatrixXd D_D(6,6);
+ MatrixXd 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);
+
+ MatrixXd 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);
+
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49593d64afa531c28a8a490de0ba562ed1c13bb2
--- /dev/null
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -0,0 +1,364 @@
+
+// wolf includes
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/factory_sensor.h"
+#include "dummy/processor_tracker_feature_dummy.h"
+#include "core/capture/capture_void.h"
+
+using namespace wolf;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummyDummy);
+
+class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
+{
+ public:
+
+ ProcessorTrackerFeatureDummyDummy(ParamsProcessorTrackerFeatureDummyPtr& _params) :
+ ProcessorTrackerFeatureDummy(_params){}
+
+ void setLast(CaptureBasePtr _last_ptr){ last_ptr_ = _last_ptr; }
+ void setInc(CaptureBasePtr _incoming_ptr){ incoming_ptr_ = _incoming_ptr; }
+
+ unsigned int callProcessKnown(){ return this->processKnown(); }
+
+ unsigned int callProcessNew(const int& _max_new_features){ return this->processNew(_max_new_features); }
+
+ unsigned int callDetectNewFeatures(const int& _max_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out){ return this->detectNewFeatures(_max_features, _capture, _features_out); }
+
+ unsigned int callTrackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences){ return this->trackFeatures(_features_in, _capture, _features_out, _feature_correspondences); }
+
+ FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return this->emplaceFactor(_feature_ptr, _feature_other_ptr); }
+
+ void callEstablishFactors(){ this->establishFactors();}
+
+ TrackMatrix getTrackMatrix(){ return track_matrix_; }
+
+ FeatureMatchMap getMatchesLastFromIncoming() { return matches_last_from_incoming_; }
+
+ void callReset()
+ {
+ this->resetDerived();
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
+ };
+};
+
+bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
+{
+ return ftr->getCapture() == cap &&
+ std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+
+}
+
+bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
+{
+ return fac->getFeature() == ftr &&
+ std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+
+}
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class ProcessorTrackerFeatureDummyTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sensor;
+ ParamsProcessorTrackerFeatureDummyPtr params;
+ ProcessorTrackerFeatureDummyDummyPtr processor;
+
+ ~ProcessorTrackerFeatureDummyTest() override{}
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("PO", 2);
+
+ // Install camera
+ sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
+
+ // Install processor
+ params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
+ params->max_new_features = 10;
+ params->min_features_for_keyframe = 7;
+ params->time_tolerance = 0.25;
+ params->voting_active = true;
+ params->n_tracks_lost = 1; // 1 (the first) track is lost each time trackFeatures is called
+ processor = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
+ processor->link(sensor);
+ }
+};
+
+TEST_F(ProcessorTrackerFeatureDummyTest, installProcessor)
+{
+ ASSERT_EQ(processor->getProblem(), problem);
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, callDetectNewFeatures)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ FeatureBasePtrList feat_list;
+
+ // demo callDetectNewFeatures
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ ASSERT_EQ(n_feat, feat_list.size());
+ ASSERT_EQ(n_feat, params->max_new_features);
+
+ // check the features are emplaced
+ ASSERT_EQ(n_feat, last_cap->getFeatureList().size());
+ for (auto feat : feat_list)
+ ASSERT_TRUE(isFeatureLinked(feat, last_cap));
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ FeatureBasePtrList feat_list;
+
+ //fill feat_last list
+ processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+
+ // Put a capture on incoming_ptr_
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+ processor->setInc(inc_cap);
+
+ //test trackFeatures
+ FeatureBasePtrList feat_list_out;
+ FeatureMatchMap feat_correspondance;
+ processor->callTrackFeatures(feat_list, inc_cap, feat_list_out, feat_correspondance);
+ ASSERT_EQ(feat_list_out.size(), feat_correspondance.size());
+ ASSERT_EQ(feat_list.size(), feat_list_out.size()+1); // one of each 10 tracks is lost
+
+ // check the features are emplaced
+ ASSERT_EQ(inc_cap->getFeatureList().size(), feat_list_out.size());
+ for (auto feat : feat_list_out)
+ ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ // Put a capture on incoming_ptr_
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+ processor->setInc(inc_cap);
+
+ auto n_new_feat = processor->callProcessNew(10); // detect 10 features
+
+ ASSERT_EQ(n_new_feat, 10); // detected 10 features
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
+ ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 track lost
+ ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 9); // 1 track lost
+
+ // check the features are emplaced
+ for (auto feat : last_cap->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, last_cap));
+ for (auto feat : inc_cap->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ // Put a capture on incoming_ptr_
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+ processor->setInc(inc_cap);
+
+ processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
+ ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
+ ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
+
+ processor->callReset(); // now incoming is last and last is origin
+
+ // Put a capture on last_ptr_
+ CaptureBasePtr new_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setInc(new_cap);
+
+ auto n_tracked_feat = processor->callProcessKnown();
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), 14); // 14 previously tracked features
+ ASSERT_EQ(n_tracked_feat, 13); // 1 track lost
+ ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 13); // 1 track lost
+ ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 13); // 1 track lost
+
+ // check the features are emplaced
+ for (auto feat : processor->getOrigin()->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, processor->getOrigin()));
+ for (auto feat : processor->getLast()->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, processor->getLast()));
+ for (auto feat : processor->getIncoming()->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, processor->getIncoming()));
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
+{
+ FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1)));
+ FeatureBasePtr ftr_other(std::make_shared<FeatureBase>("DUMMY FEATURE",
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1)));
+
+ FactorBasePtr fac = processor->callEmplaceFactor(ftr, ftr_other);
+
+ // check factor correctly emplaced
+ ASSERT_TRUE(isFactorLinked(fac, ftr));
+ ASSERT_EQ(fac->getFrameOther(),nullptr);
+ ASSERT_EQ(fac->getCaptureOther(),nullptr);
+ ASSERT_EQ(fac->getFeatureOther(),ftr_other);
+ ASSERT_EQ(fac->getLandmarkOther(),nullptr);
+ ASSERT_TRUE(std::find(ftr_other->getConstrainedByList().begin(),ftr_other->getConstrainedByList().end(), fac) != ftr_other->getConstrainedByList().end());
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ // Put a capture on incoming_ptr_
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+ processor->setInc(inc_cap);
+
+ processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
+ ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
+ ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
+
+ processor->callReset(); // now incoming is last and last is origin
+
+ // test establishFactors()
+ processor->callEstablishFactors();
+ TrackMatrix track_matrix = processor->getTrackMatrix();
+ unsigned int n_factors_ori = 0;
+ unsigned int n_factors_last = 0;
+ for (auto feat : processor->getLast()->getFeatureList())
+ {
+ if (!feat->getFactorList().empty())
+ {
+ n_factors_last++;
+ ASSERT_EQ(feat->getFactorList().size(), 1);
+ ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
+ ASSERT_EQ(feat->getFactorList().front()->getFeatureOther(), track_matrix.feature(feat->trackId(), processor->getOrigin()));
+ }
+ }
+
+ for (auto feat : processor->getOrigin()->getFeatureList())
+ {
+ if (!feat->getConstrainedByList().empty())
+ {
+ n_factors_ori++;
+ ASSERT_EQ(feat->getConstrainedByList().size(), 1);
+ ASSERT_EQ(feat->getConstrainedByList().front()->getFeatureOther(), feat);
+ ASSERT_EQ(feat->getConstrainedByList().front()->getFeature(), track_matrix.feature(feat->trackId(), processor->getLast()));
+ }
+ }
+ ASSERT_EQ(n_factors_ori, 14);
+ ASSERT_EQ(n_factors_last, 14);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, process)
+{
+
+ //1ST TIME -> KF (origin)
+ WOLF_INFO("First time...");
+ CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
+ cap1->process();
+
+ ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap1->getFrame()->id());
+ ASSERT_TRUE(problem->check(0));
+
+ //2ND TIME
+ WOLF_INFO("Second time...");
+ CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
+ cap2->process();
+
+ ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-1);
+ ASSERT_TRUE(problem->check(0));
+
+ //3RD TIME
+ WOLF_INFO("Third time...");
+ CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
+ cap3->process();
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2);
+ ASSERT_TRUE(problem->check(0));
+
+ //4TH TIME
+ WOLF_INFO("Forth time...");
+ CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
+ cap4->process();
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);
+ ASSERT_TRUE(problem->check(0));
+
+ //5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
+ WOLF_INFO("Fifth time...");
+ CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
+ cap5->process();
+
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap4->getFrame()->id());
+ ASSERT_TRUE(problem->check(0));
+
+ // check factors
+ WOLF_INFO("checking factors...");
+ TrackMatrix track_matrix = processor->getTrackMatrix();
+ ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
+ ASSERT_TRUE(problem->check(0));
+ unsigned int n_factors0 = 0;
+ unsigned int n_factors4 = 0;
+ for (auto feat : cap4->getFeatureList())
+ {
+ if (!feat->getFactorList().empty())
+ {
+ n_factors0++;
+ ASSERT_EQ(feat->getFactorList().size(), 1);
+ ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
+ ASSERT_EQ(feat->getFactorList().front()->getFeatureOther(), track_matrix.feature(feat->trackId(), cap1));
+ }
+ }
+
+ for (auto feat : cap1->getFeatureList())
+ {
+ if (!feat->getConstrainedByList().empty())
+ {
+ n_factors4++;
+ ASSERT_EQ(feat->getConstrainedByList().size(), 1);
+ ASSERT_EQ(feat->getConstrainedByList().front()->getFeatureOther(), feat);
+ ASSERT_EQ(feat->getConstrainedByList().front()->getFeature(), track_matrix.feature(feat->trackId(), cap4));
+ }
+ }
+ ASSERT_EQ(n_factors0, 7);
+ ASSERT_EQ(n_factors4, 7);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0fe1b1d8a748df603db674cf7604720c69e4d71
--- /dev/null
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -0,0 +1,386 @@
+
+// wolf includes
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/factory_sensor.h"
+#include "dummy/processor_tracker_landmark_dummy.h"
+#include "core/capture/capture_void.h"
+
+using namespace wolf;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummyDummy);
+
+class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
+{
+ public:
+
+ ProcessorTrackerLandmarkDummyDummy(ParamsProcessorTrackerLandmarkDummyPtr& _params) :
+ ProcessorTrackerLandmarkDummy(_params){}
+
+ void setLast(CaptureBasePtr _last_ptr){ last_ptr_ = _last_ptr; }
+ void setInc(CaptureBasePtr _incoming_ptr){ incoming_ptr_ = _incoming_ptr; }
+
+ unsigned int callProcessKnown(){ return this->processKnown(); }
+
+ unsigned int callProcessNew(const int& _max_new_features){ return this->processNew(_max_new_features); }
+
+ unsigned int callDetectNewFeatures(const int& _max_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out){ return this->detectNewFeatures(_max_features,
+ _capture,
+ _features_out); }
+
+ unsigned int callFindLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences){ return this->findLandmarks(_landmarks_in,
+ _capture,
+ _features_out,
+ _feature_landmark_correspondences); }
+
+ LandmarkBasePtr callEmplaceLandmark(FeatureBasePtr _feature_ptr){ return this->emplaceLandmark(_feature_ptr); }
+ void callEmplaceNewLandmarks(){ this->emplaceNewLandmarks(); }
+ FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr){ return this->emplaceFactor(_feature_ptr,
+ _landmark_ptr); }
+
+ void callEstablishFactors(){ this->establishFactors();}
+
+ void setNewFeaturesLast(FeatureBasePtrList& _new_features_list){ new_features_last_ = _new_features_list;}
+
+ LandmarkMatchMap getMatchesLandmarkFromIncoming() { return matches_landmark_from_incoming_;}
+ LandmarkMatchMap getMatchesLandmarkFromLast() { return matches_landmark_from_last_;}
+ LandmarkBasePtrList getNewLandmarks() { return new_landmarks_;}
+
+ void callReset()
+ {
+ this->resetDerived();
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
+ };
+};
+
+bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
+{
+ return ftr->getCapture() == cap &&
+ std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+
+}
+
+bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
+{
+ return fac->getFeature() == ftr &&
+ std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+
+}
+
+bool isLandmarkLinked(LandmarkBasePtr lmk, MapBasePtr map)
+{
+ return lmk->getMap() == map &&
+ std::find(map->getLandmarkList().begin(), map->getLandmarkList().end(), lmk) != map->getLandmarkList().end();
+
+}
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class ProcessorTrackerLandmarkDummyTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sensor;
+ ParamsProcessorTrackerLandmarkDummyPtr params;
+ ProcessorTrackerLandmarkDummyDummyPtr processor;
+
+ ~ProcessorTrackerLandmarkDummyTest() override{}
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("PO", 2);
+
+ // Install camera
+ sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
+
+ // Install processor
+ params = std::make_shared<ParamsProcessorTrackerLandmarkDummy>();
+ params->max_new_features = 10;
+ params->min_features_for_keyframe = 7;
+ params->time_tolerance = 0.25;
+ params->voting_active = true;
+ params->n_landmarks_lost = 1; // 1 (the last) landmark is not found each time findLandmark is called
+ processor = std::make_shared<ProcessorTrackerLandmarkDummyDummy>(params);
+ processor->link(sensor);
+ }
+};
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, installProcessor)
+{
+ ASSERT_EQ(processor->getProblem(), problem);
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, detectNewFeatures)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ FeatureBasePtrList feat_list;
+
+ // demo callDetectNewFeatures
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
+ ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+
+ // check the features are emplaced
+ ASSERT_EQ(n_feat, last_cap->getFeatureList().size());
+ for (auto feat : feat_list)
+ ASSERT_TRUE(isFeatureLinked(feat, last_cap));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceLandmark)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ FeatureBasePtrList feat_list;
+
+ // demo callDetectNewFeatures
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+
+ for (auto ftr : feat_list)
+ {
+ auto lmk = processor->callEmplaceLandmark(ftr);
+ // check that it is correctly emplaced
+ ASSERT_TRUE(isLandmarkLinked(lmk,problem->getMap()));
+ }
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(),n_feat); // emplaced 10 landmarks
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceNewLandmarks)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ FeatureBasePtrList feat_list;
+
+ // test detectNewFeatures
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+
+ // test createNewLandmarks
+ processor->setNewFeaturesLast(feat_list);
+ processor->callEmplaceNewLandmarks();
+ ASSERT_EQ(processor->getNewLandmarks().size(),n_feat); // emplaced 10 landmarks
+
+ // check that it is correctly emplaced
+ for (auto lmk : processor->getNewLandmarks())
+ ASSERT_TRUE(isLandmarkLinked(lmk,problem->getMap()));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ FeatureBasePtrList feat_list;
+
+ // test detectNewFeatures
+ processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+
+ // test createNewLandmarks
+ processor->setNewFeaturesLast(feat_list);
+ processor->callEmplaceNewLandmarks();
+ LandmarkBasePtrList new_landmarks = processor->getNewLandmarks();
+
+ //test findLandmarks
+ LandmarkMatchMap feature_landmark_correspondences;
+ FeatureBasePtrList feat_found;
+
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setInc(inc_cap);
+
+ processor->callFindLandmarks(new_landmarks, inc_cap, feat_found, feature_landmark_correspondences);
+ ASSERT_EQ(feature_landmark_correspondences.size(), feat_found.size());
+ ASSERT_EQ(feat_list.size(), feat_found.size()+1); // one of each 10 tracks is lost
+
+ for (auto feat : feat_found)
+ ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ // Put a capture on incoming_ptr_
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+ processor->setInc(inc_cap);
+
+ auto n_new_feat = processor->callProcessNew(10); // detect 10 features in last, create landmarks, find landmarks in incoming
+
+ ASSERT_EQ(n_new_feat, 10); // detected 10 features
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 10); // created 10 landmarks
+ ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found
+ ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(), 10); // all last features have the landmark correspondence
+ ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ // create 10 landmarks and link them to map
+ FeatureBasePtrList feat_list;
+ processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ processor->setNewFeaturesLast(feat_list);
+ processor->callEmplaceNewLandmarks();
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(),10); // created 10 landmarks
+
+ // Put a capture on incoming_ptr_
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+ processor->setInc(inc_cap);
+
+ // Test processKnown
+ processor->callProcessKnown();
+
+ ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found
+ ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
+{
+ FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1)));
+ LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("LandmarkBase",
+ std::make_shared<StateBlock>(1),
+ std::make_shared<StateBlock>(1)));
+
+ FactorBasePtr fac = processor->callEmplaceFactor(ftr, lmk);
+ ASSERT_EQ(fac->getFeature(),ftr);
+ ASSERT_EQ(fac->getFrameOther(),nullptr);
+ ASSERT_EQ(fac->getCaptureOther(),nullptr);
+ ASSERT_EQ(fac->getFeatureOther(),nullptr);
+ ASSERT_EQ(fac->getLandmarkOther(),lmk);
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
+{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
+ // Put a capture on incoming_ptr_
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+ processor->setInc(inc_cap);
+
+ processor->callProcessNew(15); // detect 15 features, 1 of each 10 tracks is lost
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
+ ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 of each 10 tracks is lost
+ ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(), 15); // all landmarks
+ ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 14); // 1 of each 10 tracks is lost
+
+ // test establishFactors()
+ processor->callEstablishFactors();
+ LandmarkMatchMap landmark_from_last = processor->getMatchesLandmarkFromLast();
+ unsigned int n_factors_last = 0;
+ unsigned int n_factors_landmark = 0;
+ for (auto feat : processor->getLast()->getFeatureList())
+ {
+ n_factors_last++;
+ ASSERT_EQ(feat->getFactorList().size(), 1);
+ ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
+ ASSERT_EQ(feat->getFactorList().front()->getLandmarkOther(), landmark_from_last[feat]->landmark_ptr_);
+ }
+
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ n_factors_landmark++;
+ ASSERT_EQ(lmk->getConstrainedByList().size(), 1);
+ ASSERT_EQ(lmk->getConstrainedByList().front()->getLandmarkOther(), lmk);
+ ASSERT_EQ(landmark_from_last[lmk->getConstrainedByList().front()->getFeature()]->landmark_ptr_, lmk);
+ }
+ ASSERT_EQ(n_factors_last, 15);
+ ASSERT_EQ(n_factors_landmark, 15);
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, process)
+{
+
+ //1ST TIME -> KF (origin)
+ WOLF_DEBUG("First time...");
+ CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
+ cap1->process();
+
+ ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap1->getFrame());
+
+ //2ND TIME
+ WOLF_DEBUG("Second time...");
+ CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
+ cap2->process();
+
+ ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-1);
+
+ //3RD TIME
+ WOLF_DEBUG("Third time...");
+ CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
+ cap3->process();
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2);
+
+ //4TH TIME
+ WOLF_DEBUG("Forth time...");
+ CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
+ cap4->process();
+
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);
+
+ //5TH TIME -> KF in cap4 (found landmarks < 7 (params->min_features_for_keyframe))
+ WOLF_DEBUG("Fifth time...");
+ CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
+ cap5->process();
+
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap4->getFrame());
+ ASSERT_EQ(processor->getOrigin(), cap4);
+ ASSERT_EQ(processor->getLast(), cap5);
+
+ // check factors
+ WOLF_DEBUG("checking factors...");
+ LandmarkMatchMap landmark_from_last = processor->getMatchesLandmarkFromLast();
+ unsigned int n_factors_cap1 = 0;
+ unsigned int n_factors_cap4 = 0;
+
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ for (auto fac : lmk->getConstrainedByList())
+ {
+ ASSERT_EQ(fac->getFeature()->getFactorList().size(), 1); // only one factor per feature
+ if (fac->getFeature()->getCapture() == cap1)
+ n_factors_cap1++;
+ else if (fac->getFeature()->getCapture() == cap4)
+ n_factors_cap4++;
+ else
+ ASSERT_TRUE(false);// this shouldn't happen!
+ }
+ }
+ ASSERT_EQ(n_factors_cap1, 10);
+ ASSERT_EQ(n_factors_cap4, 17);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 513cddd55abc972f4f06df308a2eb35becffd366..a8a0efd094462347f379bf4507d8dfa38e591038 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -18,7 +18,7 @@
#include <iomanip>
#include <ctime>
#include <cmath>
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
//#define write_results
@@ -29,12 +29,12 @@ using namespace Eigen;
TEST(exp_q, unit_norm)
{
- Vector3s v0 = Vector3s::Random();
- Scalar scale = 1.0;
+ Vector3d v0 = Vector3d::Random();
+ double scale = 1.0;
for (int i = 0; i < 20; i++)
{
- Vector3s v = v0 * scale;
- Quaternions q = exp_q(v);
+ Vector3d v = v0 * scale;
+ Quaterniond 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;
}
@@ -42,7 +42,7 @@ TEST(exp_q, unit_norm)
TEST(rotations, pi2pi)
{
- ASSERT_NEAR(M_PI_2, pi2pi((Scalar)M_PI_2), 1e-10);
+ ASSERT_NEAR(M_PI_2, pi2pi((double)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);
@@ -54,12 +54,12 @@ TEST(rotations, pi2pi)
TEST(skew, Skew_vee)
{
using namespace wolf;
- Vector3s vec3 = Vector3s::Random();
- Matrix3s skew_mat;
+ Vector3d vec3 = Vector3d::Random();
+ Matrix3d skew_mat;
skew_mat = skew(vec3);
// vee
- Vector3s vec3_bis;
+ Vector3d vec3_bis;
vec3_bis = vee(skew_mat);
ASSERT_TRUE(vec3_bis == vec3);
@@ -69,23 +69,23 @@ TEST(exp_q, v2q_q2v)
{
using namespace wolf;
//defines scalars
- wolf::Scalar deg_to_rad = M_PI/180.0;
+ double deg_to_rad = M_PI/180.0;
- Vector4s vec0, vec1;
+ Vector4d vec0, vec1;
//v2q
- Vector3s rot_vector0, rot_vector1;
- rot_vector0 = Vector3s::Random();
+ Vector3d rot_vector0, rot_vector1;
+ rot_vector0 = Vector3d::Random();
rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
rot_vector0 = rot_vector0*deg_to_rad;
- Quaternions quat0, quat1;
+ Quaterniond 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;
+ Vector3d quat_to_v0, quat_to_v1;
+ VectorXd quat_to_v0x, quat_to_v1x;
quat_to_v0 = q2v(quat0);
quat_to_v1 = q2v(quat1);
@@ -105,19 +105,19 @@ TEST(exp_R, v2R_R2v)
//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;
+ double deg_to_rad = M_PI/180.0;
+ Vector3d rot_vector0, rot_vector1;
- rot_vector0 = Vector3s::Random();
+ rot_vector0 = Vector3d::Random();
rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
rot_vector0 = rot_vector0*deg_to_rad;
- Matrix3s rot0, rot1;
+ Matrix3d rot0, rot1;
rot0 = v2R(rot_vector0);
rot1 = v2R(rot_vector1);
//R2v
- Vector3s rot0_vec, rot1_vec;
+ Vector3d rot0_vec, rot1_vec;
rot0_vec = R2v(rot0);
rot1_vec = R2v(rot1);
@@ -130,13 +130,13 @@ 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;
+ double scale = 1;
+ Matrix3d v_to_R, initial_matrix;
+ Vector3d R_to_v;
- //Vector3s rv;
+ //Vector3d rv;
for(int i = 0; i<8; i++){
- initial_matrix = v2R(Vector3s::Random().eval() * scale);
+ initial_matrix = v2R(Vector3d::Random().eval() * scale);
R_to_v = R2v(initial_matrix);
v_to_R = v2R(R_to_v);
@@ -150,19 +150,19 @@ 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;
+ double scale = 1;
+ Matrix3d rotation_mat;
+ Vector3d rv;
for(int i = 0; i<8; i++){
- rotation_mat = v2R(Vector3s::Random().eval() * scale);
+ rotation_mat = v2R(Vector3d::Random().eval() * scale);
//rv = R2v(rotation_mat); //decomposing R2v below
- AngleAxis<wolf::Scalar> aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
+ AngleAxis<double> aa0 = AngleAxis<double>(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());
+ ASSERT_FALSE(rv == Vector3d::Zero());
scale = scale*0.1;
}
}
@@ -170,14 +170,14 @@ TEST(log_R, R2v_v2R_AAlimits)
TEST(exp_q, v2q2R2v)
{
using namespace wolf;
- wolf::Scalar scale = 1;
+ double 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_);
+ Vector3d vector_ = Vector3d::Random()*scale;
+ Quaterniond quat_ = v2q(vector_);
+ Matrix3d mat_ = quat_.matrix();
+ Vector3d vector_bis = R2v(mat_);
ASSERT_MATRIX_APPROX(vector_, vector_bis, wolf::Constants::EPS);
scale = scale*0.1;
@@ -191,22 +191,22 @@ TEST(rotations, AngleAxis_limits)
// 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;
+ double scale = 1;
+ Matrix3d res, res_i, rotation_mati, rotation_mat;
+ Vector3d 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_mat = v2R(Vector3d::Random().eval() * scale);
rotation_mati = rotation_mat;
//rv = R2v(rotation_mat); //decomposing R2v below
- AngleAxis<wolf::Scalar> aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
+ AngleAxis<double> aa0 = AngleAxis<double>(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);
+ AngleAxis<double> aa1 = AngleAxis<double>(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();
@@ -237,17 +237,17 @@ TEST(compose, Quat_compos_const_rateOfTurn)
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;
+ double deg_to_rad = M_PI/180.0;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
+ Eigen::Vector3d 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;
+ double tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
with * pi/180
@@ -267,7 +267,7 @@ TEST(compose, Quat_compos_const_rateOfTurn)
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;
+ const double dt = 0.1;
for(unsigned int data_iter = 0; data_iter < 100; data_iter ++)
{
@@ -279,7 +279,7 @@ TEST(compose, Quat_compos_const_rateOfTurn)
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
- Eigen::Vector3s final_orientation(q2v(qRot));
+ Eigen::Vector3d 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;
}
@@ -308,18 +308,18 @@ TEST(compose, Quat_compos_var_rateOfTurn)
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;
+ double deg_to_rad = M_PI/180.0;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
- wolf::Scalar time = 0;
+ Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
+ double 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;
+ double tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
with * pi/180
@@ -334,7 +334,7 @@ TEST(compose, Quat_compos_var_rateOfTurn)
wz = pi * z_rot_vel * cos(z_rot_vel * t * pi/180) * pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const double dt = 0.001;
for(unsigned int data_iter = 0; data_iter <= 10000; data_iter ++)
{
@@ -352,7 +352,7 @@ TEST(compose, Quat_compos_var_rateOfTurn)
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
- Eigen::Vector3s final_orientation(q2v(qRot));
+ Eigen::Vector3d 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;
@@ -385,18 +385,18 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
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;
+ double deg_to_rad = M_PI/180.0;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
- wolf::Scalar time = 0;
+ Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
+ double 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;
+ double tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
with * pi/180
@@ -411,7 +411,7 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
wz = pi * z_rot_vel * cos(z_rot_vel * t * pi/180) * pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const double dt = 0.001;
for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
{
@@ -429,7 +429,7 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
- Eigen::Vector3s final_orientation(q2v(qRot));
+ Eigen::Vector3d 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;
@@ -440,14 +440,14 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
TEST(Plus, Random)
{
- Quaternions q;
+ Quaterniond q;
q .coeffs().setRandom().normalize();
- Vector3s v;
+ Vector3d v;
v .setRandom();
- Quaternions q2 = q * exp_q(v);
- Quaternions q3 = exp_q(v) * q;
+ Quaterniond q2 = q * exp_q(v);
+ Quaterniond q3 = exp_q(v) * q;
ASSERT_QUATERNION_APPROX(plus(q,v) , q2, 1e-12);
ASSERT_QUATERNION_APPROX(plus_right(q,v), q2, 1e-12);
@@ -457,14 +457,14 @@ TEST(Plus, Random)
TEST(Plus, Identity_plus_small)
{
- Quaternions q;
+ Quaterniond q;
q .setIdentity();
- Vector3s v;
+ Vector3d v;
v .setRandom();
v *= 1e-6;
- Quaternions q2;
+ Quaterniond q2;
q2.w() = 1;
q2.vec() = 0.5*v;
@@ -473,12 +473,12 @@ TEST(Plus, Identity_plus_small)
TEST(Minus_and_diff, Random)
{
- Quaternions q1, q2, qo;
+ Quaterniond 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());
+ Vector3d vr = log_q(q1.conjugate() * q2);
+ Vector3d vl = log_q(q2 * q1.conjugate());
ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12);
ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12);
@@ -499,24 +499,24 @@ TEST(Minus_and_diff, Random)
TEST(Jacobians, Jr)
{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Vector3d theta; theta.setRandom();
+ Vector3d 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);
+ Matrix3d 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;
+ Vector3d theta; theta.setRandom();
+ Vector3d 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);
+ Matrix3d 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);
@@ -529,28 +529,28 @@ TEST(Jacobians, Jl)
TEST(Jacobians, Jr_inv)
{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
- Quaternions q = v2q(theta);
- Matrix3s R = v2R(theta);
+ Vector3d theta; theta.setRandom();
+ Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Quaterniond q = v2q(theta);
+ Matrix3d 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);
+ Matrix3d 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);
+ Vector3d theta; theta.setRandom();
+ Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Quaterniond q = v2q(theta);
+ Matrix3d 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);
+ Matrix3d 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);
}
@@ -558,19 +558,19 @@ TEST(Jacobians, Jl_inv)
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;
+ Vector3d th1(.1,.2,.3), th2(.3,.1,.2);
+ Quaterniond q1(exp_q(th1));
+ Quaterniond q2(exp_q(th2));
+ Quaterniond qc;
+ Matrix3d 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;
+ double dx = 1e-6;
+ Vector3d pert;
+ Quaterniond q1_pert, q2_pert, qc_pert;
for (int i = 0; i<3; i++)
{
pert.setZero();
@@ -594,19 +594,19 @@ TEST(Jacobians, compose)
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;
+ Vector3d th1(.1,.2,.3), th2(.3,.1,.2);
+ Quaterniond q1(exp_q(th1));
+ Quaterniond q2(exp_q(th2));
+ Quaterniond qc;
+ Matrix3d 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;
+ double dx = 1e-6;
+ Vector3d pert;
+ Quaterniond q1_pert, q2_pert, qc_pert;
for (int i = 0; i<3; i++)
{
pert.setZero();
@@ -629,17 +629,16 @@ TEST(Jacobians, between)
TEST(exp_q, small)
{
- Vector3s u; u.setRandom().normalize();
- Vector3s v;
- Quaternions q;
- Scalar scale = 1.0;
+ Vector3d u; u.setRandom().normalize();
+ Vector3d v;
+ Quaterniond q;
+ double 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());
+ WOLF_TRACE("scale = ", scale, "; ratio = ", (q.vec().array() / v.array()).transpose());
scale /= 10;
}
@@ -648,20 +647,20 @@ TEST(exp_q, small)
TEST(log_q, double_cover)
{
- Quaternions qp; qp.coeffs().setRandom().normalize();
- Quaternions qn; qn.coeffs() = - qp.coeffs();
+ Quaterniond qp; qp.coeffs().setRandom().normalize();
+ Quaterniond 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;
+ Vector3d u; u.setRandom().normalize();
+ double scale = 1.0;
for (int i = 0; i<20; i++)
{
- Vector3s v = u*scale;
- Quaternions q = exp_q(v);
- Vector3s l = log_q(q);
+ Vector3d v = u*scale;
+ Quaterniond q = exp_q(v);
+ Vector3d l = log_q(q);
ASSERT_MATRIX_APPROX(v, l, 1e-10);
@@ -669,16 +668,14 @@ TEST(log_q, small)
}
}
-//<<<<<<< HEAD
-//=======
TEST(Conversions, q2R_R2q)
{
- Vector3s v; v.setRandom();
- Quaternions q = v2q(v);
- Matrix3s R = v2R(v);
+ Vector3d v; v.setRandom();
+ Quaterniond q = v2q(v);
+ Matrix3d R = v2R(v);
- Quaternions q_R = R2q(R);
- Quaternions qq_R(R);
+ Quaterniond q_R = R2q(R);
+ Quaterniond qq_R(R);
ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS);
ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS);
@@ -690,10 +687,34 @@ TEST(Conversions, q2R_R2q)
ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS);
}
+TEST(Conversions, R2q_norm_of_q)
+{
+ Isometry3d T = Translation3d(1,2,3) * AngleAxisd(M_PI_4, Vector3d(1,2,3).normalized());
+ Eigen::Vector4d cqt = R2q(T.linear()).coeffs();
+ Eigen::Matrix3d R = T.linear();
+ Eigen::Vector4d cqt2 = R2q(R).coeffs();
+ std::cout << "cqt: " << cqt.transpose() << std::endl;
+ std::cout << "cqt2: " << cqt2.transpose() << std::endl;
+ std::cout << "cqt.norm(): " << cqt.norm() << std::endl;
+ std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl;
+
+ ASSERT_NEAR(cqt.norm(), 1, 1e-8);
+ ASSERT_NEAR(cqt2.norm(), 1, 1e-8);
+
+ // Theck T.linear() vs T.rotation()
+ T.linear().setRandom();
+
+ Matrix3d TL = T.linear();
+ Matrix3d TR = T.rotation();
+
+ WOLF_INFO("TL = ", TL);
+ WOLF_INFO("TR = ", TR);
+}
+
TEST(Conversions, e2q_q2e)
{
- Vector3s e, eo;
- Quaternions q;
+ Vector3d e, eo;
+ Quaterniond q;
e << 0.1, .2, .3;
@@ -707,81 +728,46 @@ TEST(Conversions, e2q_q2e)
}
-//>>>>>>> master
TEST(Conversions, e2q_q2R_R2e)
{
- Vector3s e, eo;
- Quaternions q;
- Matrix3s R;
-
-//<<<<<<< HEAD
-// e.setRandom();
-//=======
-//>>>>>>> master
+ Vector3d e, eo;
+ Quaterniond q;
+ Matrix3d R;
+
e << 0.1, .2, .3;
q = e2q(e);
R = q2R(q);
eo = R2e(R);
-//<<<<<<< HEAD
-// WOLF_TRACE("euler ", e.transpose());
-// WOLF_TRACE("quat ", q.coeffs().transpose());
-// WOLF_TRACE("R \n", R);
-//
-// WOLF_TRACE("euler o ", eo.transpose());
-//
-//
-// ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-//
-//=======
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
}
TEST(Conversions, e2R_R2e)
{
- Vector3s e, eo;
- Matrix3s R;
+ Vector3d e, eo;
+ Matrix3d R;
e << 0.1, 0.2, 0.3;
R = e2R(e);
eo = R2e(R);
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-//>>>>>>> master
}
TEST(Conversions, e2R_R2q_q2e)
{
- Vector3s e, eo;
- Quaternions q;
- Matrix3s R;
-
-//<<<<<<< HEAD
-// e.setRandom();
-// e << 0.1, 0.2, 0.3;
-// R = e2R(e(0), e(1), e(2));
-//=======
+ Vector3d e, eo;
+ Quaterniond q;
+ Matrix3d R;
+
e << 0.1, 0.2, 0.3;
R = e2R(e);
-//>>>>>>> master
q = R2q(R);
eo = q2e(q.coeffs());
-//<<<<<<< HEAD
-// WOLF_TRACE("euler ", e.transpose());
-// WOLF_TRACE("R \n", R);
-// WOLF_TRACE("quat ", q.coeffs().transpose());
-//
-// WOLF_TRACE("euler o ", eo.transpose());
-//
-//
-// ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-//
-//=======
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-//>>>>>>> master
}
int main(int argc, char **argv)
@@ -795,7 +781,7 @@ int main(int argc, char **argv)
- 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)
+ - VectorXd q2v(const Quaterniond& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
- v2R
- R2v
- jac_SO3_right
@@ -806,5 +792,6 @@ int main(int argc, char **argv)
*/
testing::InitGoogleTest(&argc, argv);
+ ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index d8420b2b3fa2d9635149c28b34ce3853bd63ca1a..3472a400fcbd571f6c7ab1232bbe09e1319d0e13 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -7,16 +7,16 @@
#include "core/sensor/sensor_base.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
using namespace wolf;
TEST(SensorBase, setNoiseStd)
{
- SensorBasePtr S (std::make_shared<SensorBase>("BASE", nullptr, nullptr, nullptr, 2)); // noise size 2
+ SensorBasePtr S (std::make_shared<SensorBase>("SensorBase", 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;
+ Eigen::Vector2d noise_std = Eigen::Vector2d::Ones() * 0.1;
+ Eigen::Matrix2d noise_cov = Eigen::Matrix2d::Identity() * 0.01;
S->setNoiseStd(noise_std);
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2936a8ddec8c06a6fc28df0f8c7dc5ed1c90f97
--- /dev/null
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -0,0 +1,79 @@
+/**
+ * \file gtest_sensor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
+
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/utils/utils_gtest.h"
+
+#include <cstdio>
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(DiffDrive, constructor)
+{
+
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ Vector3d extr(1,2,3);
+
+ auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), Vector2d(1,2), 1e-12);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), Vector1d(3), 1e-12);
+}
+
+TEST(DiffDrive, getParams)
+{
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 2;
+ intr->radius_right = 3;
+ intr->ticks_per_wheel_revolution = 4;
+ intr->wheel_separation = 5;
+
+ Vector3d extr(1,2,3);
+
+ auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+
+ ASSERT_NE(sen->getParams(), nullptr);
+
+ ASSERT_EQ(sen->getRadiansPerTick(), 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
+ ASSERT_EQ(sen->getParams()->radius_left, 2);
+ ASSERT_EQ(sen->getParams()->radius_right, 3);
+ ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);
+ ASSERT_EQ(sen->getParams()->wheel_separation, 5);
+}
+
+TEST(DiffDrive, create)
+{
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 2;
+ intr->radius_right = 3;
+ intr->ticks_per_wheel_revolution = 4;
+ intr->wheel_separation = 5;
+
+ Vector3d extr(1,2,3);
+
+ auto sen_base = SensorDiffDrive::create("name", extr, intr);
+
+ auto sen = std::static_pointer_cast<SensorDiffDrive>(sen_base);
+
+ ASSERT_NE(sen->getParams(), nullptr);
+
+ ASSERT_EQ(sen->getRadiansPerTick(), 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
+ ASSERT_EQ(sen->getParams()->radius_left, 2);
+ ASSERT_EQ(sen->getParams()->radius_right, 3);
+ ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);
+ ASSERT_EQ(sen->getParams()->wheel_separation, 5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..64d6b73a1a77ddf4a1ee7a85f72164d4174c8ff6
--- /dev/null
+++ b/test/gtest_sensor_pose.cpp
@@ -0,0 +1,69 @@
+/**
+ * \file gtest_sensor_pose.cpp
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+#include "core/sensor/sensor_pose.h"
+#include "core/utils/utils_gtest.h"
+
+#include <cstdio>
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(Pose, constructor)
+{
+
+ auto intr = std::make_shared<ParamsSensorPose>();
+ Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+ auto sen = std::make_shared<SensorPose>(extr, intr);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), extr.head<3>(), 1e-12);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), extr.tail<4>(), 1e-12);
+}
+
+TEST(Pose, getParams)
+{
+ auto intr = std::make_shared<ParamsSensorPose>();
+ intr->std_p = 2;
+ intr->std_o = 3;
+
+ Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+ auto sen = std::make_shared<SensorPose>(extr, intr);
+
+ ASSERT_EQ(sen->getStdP(), intr->std_p);
+ ASSERT_EQ(sen->getStdO(), intr->std_o);
+ // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
+ // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+}
+
+TEST(Pose, create)
+{
+ auto intr = std::make_shared<ParamsSensorPose>();
+ intr->std_p = 2;
+ intr->std_o = 3;
+
+ Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+ auto sen_base = SensorPose::create("name", extr, intr);
+
+ auto sen = std::static_pointer_cast<SensorPose>(sen_base);
+
+ ASSERT_EQ(sen->getStdP(), intr->std_p);
+ ASSERT_EQ(sen->getStdO(), intr->std_o);
+ // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
+ // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+}
+
+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
index 6b14fff0c5b9db4d16f8c0e9b22393157387fd9d..9506f844b32ddcf5d986d4a8067d77ad23030f71 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/node_base.h"
class CChildBase;
@@ -13,7 +13,7 @@ class CParentBase : public wolf::NodeBase
NodeBase("")
{};
- virtual ~CParentBase(){};
+ ~CParentBase() override{};
virtual void addChild(std::shared_ptr<CChildBase> _child_ptr) final
{
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e5efa2dff18a27131a13cb173e064edad2d0a3c2
--- /dev/null
+++ b/test/gtest_solver_ceres.cpp
@@ -0,0 +1,1522 @@
+/*
+ * gtest_solver_ptr.cpp
+ *
+ * Created on: Jun, 2018
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_angle.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+
+#include "core/solver/solver_manager.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/local_parametrization_wrapper.h"
+
+#include "ceres/ceres.h"
+
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_manager.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
+{
+ return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+ VectorXd::Zero(sb_ptr->getSize()),
+ MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+ sb_ptr,
+ nullptr,
+ false);
+}
+
+TEST(SolverCeres, Create)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverCeres, FloatingStateBlock_Add)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddFix)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddFixed)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock localparam
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock localparam
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddRemove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // update solver
+ solver_ptr->update();
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // update solver
+ solver_ptr->update();
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddUpdated)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Fix
+ sb_ptr->fix();
+
+ // Set State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, ADD);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverCeres, StateBlockAndFactor_Add)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_Fix)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddFixed)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD is posponed
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+
+ // update solver
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Fix
+ sb_ptr->fix();
+
+ // Change State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // update solver
+ solver_ptr->update();
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverCeres, OnlyFactor_Add)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // update solver
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, OnlyFactor_AddRemove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // update solver
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3cd41dde30bc72ecc16fc27d70b8cde5fe327a7b
--- /dev/null
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -0,0 +1,81 @@
+/*
+ * gtest_solver_ptr.cpp
+ *
+ * Created on: Jun, 2018
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_angle.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+
+#include "core/solver/solver_manager.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/local_parametrization_wrapper.h"
+
+#include "ceres/ceres.h"
+
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_manager_multithread.cpp
+ * (modifications should be applied to both tests)
+ */
+
+TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ TimeStamp ts(0);
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceFrame(ts, P->stateZero());
+ auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+ ts += 1.0;
+
+ if (P->getTrajectory()->getFrameMap().size() > 10)
+ (*P->getTrajectory()->begin())->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 496dab16c21dc0de603c0b2169d47a89a85514ed..2eb3f46e927270a7e61abb03734d01898722f471 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -5,625 +5,1508 @@
* Author: jvallve
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
-#include "core/factor/factor_pose_2D.h"
-#include "core/solver/solver_manager.h"
-#include "core/state_block/local_parametrization_base.h"
-#include "core/state_block/local_parametrization_angle.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+#include "dummy/solver_manager_dummy.h"
#include <iostream>
+#include <thread>
using namespace wolf;
using namespace Eigen;
-WOLF_PTR_TYPEDEFS(SolverManagerWrapper);
-class SolverManagerWrapper : public SolverManager
+/*
+ * Following tests are the same as in gtest_solver_ceres.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
{
- 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)
- {
- 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)
- {
- 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 std::vector<StateBlockPtr>& st_list){};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
-
- // 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();
- };
-};
+ return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+ VectorXd::Zero(sb_ptr->getSize()),
+ MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+ sb_ptr,
+ nullptr,
+ false);
+}
TEST(SolverManager, Create)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
- // check double pointers to branches
- ASSERT_EQ(P, solver_manager_ptr->getProblem());
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverManager, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleAddStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // add stateblock again
- P->addStateBlock(sb_ptr);
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, UpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Remove local param
sb_ptr->removeLocalParametrization();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancel out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check state block
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // add state_block
- P->addStateBlock(sb_ptr);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // add stateblock
- P->addStateBlock(sb_ptr);
+ // update solver
+ solver_ptr->update();
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdatedStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Fix
sb_ptr->fix();
// Set State
- Vector2s state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Check flags have been set true
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
// == When an ADD is notified: a ADD notification should be stored ==
- // add state_block
- P->addStateBlock(sb_ptr);
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ // check notifications
Notification notif;
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
// Set State --> FLAG
- state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Fix --> FLAG
sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
- // == ADD + REMOVE: notification map should be empty ==
- P->addStateBlock(sb_ptr);
- P->removeStateBlock(sb_ptr);
- ASSERT_TRUE(P->getStateBlockNotificationMapSize() == 0);
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// == UPDATES should clear the list of notifications ==
- // add state_block
- P->addStateBlock(sb_ptr);
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverManager, StateBlockAndFactor_Add)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_Fix)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddFixed)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddFactor)
+TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // notification
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
- // check factor
- ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // factor ADD is posponed
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add factor
- P->removeFactor(c);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // notification
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, REMOVE);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // repeated REMOVE should be ignored
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ // Fix
+ sb_ptr->fix();
- // notification
+ // Change State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // add factor
- P->removeFactor(c);
+ // == Insert OTHER notifications ==
- // notification
- ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
- ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverManager, DoubleRemoveFactor)
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverManager, OnlyFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // remove factor
- P->removeFactor(c);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // update solver
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
- // remove factor
- P->removeFactor(c);
+TEST(SolverManager, OnlyFactor_AddRemove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // nothing to update
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
int main(int argc, char **argv)
@@ -631,4 +1514,3 @@ int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4e772c1bb74747d80a6ceda04d62a679f323a4e3
--- /dev/null
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -0,0 +1,75 @@
+/*
+ * gtest_solver_manager.cpp
+ *
+ * Created on: Jun, 2018
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+#include "dummy/solver_manager_dummy.h"
+
+#include <iostream>
+#include <thread>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_ceres_multithread.cpp
+ * (modifications should be applied to both tests)
+ */
+
+TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ TimeStamp ts(0);
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceFrame(ts, P->stateZero());
+ auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+ ts += 1.0;
+
+ if (P->getTrajectory()->getFrameMap().size() > 10)
+ (*P->getTrajectory()->begin())->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dbcc4b6745e1c4ed99c1b1843c04fcbd7c1b546d
--- /dev/null
+++ b/test/gtest_state_block.cpp
@@ -0,0 +1,69 @@
+/*
+ * gtest_state_block.cpp
+ *
+ * Created on: Mar 31, 2020
+ * Author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+
+#include <iostream>
+
+
+using namespace Eigen;
+using namespace std;
+using namespace wolf;
+
+TEST(StateBlock, block_perturb)
+{
+ Vector3d x(10,20,30);
+ StateBlock sb(x);
+
+ sb.perturb(0.5);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_NE(x.norm(), sb.getState().norm());
+ ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, angle_perturb)
+{
+ Vector1d x(1.0);
+ StateAngle sb(x(0));
+
+ sb.perturb(0.1);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_NE(x.norm(), sb.getState().norm());
+ ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, quaternion_perturb)
+{
+ Vector4d x(0.5,0.5,0.5,0.5);
+ StateQuaternion sb(x);
+
+ sb.perturb(0.01);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
+ ASSERT_MATRIX_APPROX(x , sb.getState() , 0.01 * 4); // 4-sigma test...
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
+
+
+
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4bcea67bd4e51af793e026f1496f2591fcfe98a
--- /dev/null
+++ b/test/gtest_state_composite.cpp
@@ -0,0 +1,872 @@
+/*
+ * gtest_state_composite.cpp
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+#include "core/state_block/state_composite.h"
+#include "core/state_block/state_quaternion.h"
+
+#include "core/utils/utils_gtest.h"
+
+using namespace wolf;
+using namespace std;
+
+
+class StateBlockCompositeInit : public testing::Test
+{
+ public:
+
+ StateBlockPtr sbp, sbv, sbx;
+ StateQuaternionPtr sbq;
+
+ StateBlockComposite states;
+
+ void SetUp() override
+ {
+ sbp = states.emplace('P', Vector3d(1,2,3));
+ sbv = states.emplace('V', Vector3d(4,5,6));
+ sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5));
+
+ sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
+ }
+};
+
+TEST_F(StateBlockCompositeInit, stateSize)
+{
+ ASSERT_EQ( states.stateSize() , 10 );
+ ASSERT_EQ( states.stateSize("PQ"), 7 );
+ ASSERT_EQ( states.stateSize("PV"), 6 );
+}
+
+TEST_F(StateBlockCompositeInit, stateVector)
+{
+ ASSERT_EQ( states.stateVector("PQ") , (VectorXd( 7) << 1,2,3,.5,.5,.5,.5) .finished() );
+ ASSERT_EQ( states.stateVector("PVQ"), (VectorXd(10) << 1,2,3,4,5,6,.5,.5,.5,.5).finished() );
+ ASSERT_EQ( states.stateVector("PQV"), (VectorXd(10) << 1,2,3,.5,.5,.5,.5,4,5,6).finished() );
+}
+
+TEST_F(StateBlockCompositeInit, emplace)
+{
+ // Emplaces called in SetUp()
+
+ // check 3 elements
+ ASSERT_EQ(states.getStateBlockMap().size(), 3);
+}
+
+TEST_F(StateBlockCompositeInit, has_key)
+{
+ ASSERT_TRUE(states.has('P'));
+ ASSERT_FALSE(states.has('X'));
+}
+
+TEST_F(StateBlockCompositeInit, has_sb)
+{
+ ASSERT_TRUE(states.has(sbp));
+ ASSERT_FALSE(states.has(sbx));
+}
+
+TEST_F(StateBlockCompositeInit, at)
+{
+ ASSERT_EQ(states.at('P'), sbp);
+
+ ASSERT_EQ(states.at('X'), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_sb)
+{
+ states.set('P', sbx);
+
+ ASSERT_EQ(states.at('P'), sbx);
+
+ states.set('P', sbp);
+
+ ASSERT_DEATH(states.set('X', sbx), "");
+
+ ASSERT_EQ(states.at('X'), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_vec)
+{
+ Vector3d p(11,22,33);
+ Vector3d x(55,66,77);
+
+ states.set('P', p);
+
+ ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
+
+ ASSERT_DEATH(states.set('X', x), "");
+
+ ASSERT_EQ(states.at('X'), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_vectors)
+{
+ Vector3d p(11,22,33);
+ Vector4d q(11,22,33,44); q.normalize();
+ Vector3d x(55,66,77);
+
+ states.setVectors("PQ", {p, q});
+
+ ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
+ ASSERT_MATRIX_APPROX(states.at('Q')->getState(), q, 1e-20);
+
+ ASSERT_DEATH(states.setVectors("PX", {p,x}), "");
+
+ ASSERT_EQ(states.at('X'), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, key_ref)
+{
+ char key;
+ ASSERT_TRUE(states.key(sbp, key));
+ ASSERT_EQ(key, 'P');
+
+ ASSERT_FALSE(states.key(sbx, key));
+ ASSERT_EQ(key, '0');
+}
+
+TEST_F(StateBlockCompositeInit, key_return)
+{
+ // existing key
+ ASSERT_EQ(states.key(sbp), 'P');
+
+ // non existing key returns zero char
+ ASSERT_EQ(states.key(sbx), '0');
+}
+
+TEST_F(StateBlockCompositeInit, find)
+{
+ auto it = states.find(sbp);
+ ASSERT_NE(it, states.getStateBlockMap().end());
+
+ it = states.find(sbx);
+ ASSERT_EQ(it, states.getStateBlockMap().end());
+}
+
+TEST_F(StateBlockCompositeInit, add)
+{
+ states.add('X', sbx);
+
+ ASSERT_EQ(states.at('X'), sbx);
+}
+
+TEST_F(StateBlockCompositeInit, remove)
+{
+ // remove existing block
+ states.remove('V');
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+
+ // remove non existing block -- no effect
+ states.remove('X');
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+}
+
+TEST_F(StateBlockCompositeInit, perturb)
+{
+ ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.perturb(0.1);
+
+ // values have moved wrt original
+ ASSERT_FALSE(states.at('P')->getState().isApprox(Vector3d(1,2,3), 1e-3));
+ ASSERT_FALSE(states.at('V')->getState().isApprox(Vector3d(4,5,6), 1e-3));
+ ASSERT_FALSE(states.at('Q')->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
+}
+
+TEST_F(StateBlockCompositeInit, setIdentity)
+{
+ ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.setIdentity();
+
+ // values have moved wrt original
+ ASSERT_TRUE(states.at('P')->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at('V')->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at('Q')->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, identity)
+{
+ VectorComposite v = states.identity();
+
+ ASSERT_TRUE(v.at('P').isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at('V').isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at('Q').isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, fix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at('P')->isFixed());
+ ASSERT_TRUE(states.at('V')->isFixed());
+ ASSERT_TRUE(states.at('Q')->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, unfix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at('P')->isFixed());
+ ASSERT_TRUE(states.at('V')->isFixed());
+ ASSERT_TRUE(states.at('Q')->isFixed());
+
+ states.unfix();
+
+ ASSERT_FALSE(states.at('P')->isFixed());
+ ASSERT_FALSE(states.at('V')->isFixed());
+ ASSERT_FALSE(states.at('Q')->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, isFixed)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.isFixed());
+
+ states.at('P')->unfix();
+
+ ASSERT_FALSE(states.isFixed());
+}
+
+
+TEST(VectorComposite, constructor_empty)
+{
+ VectorComposite v;
+ ASSERT_TRUE(v.empty());
+}
+
+TEST(VectorComposite, constructor_copy)
+{
+ VectorComposite u;
+ u.emplace('a', Vector2d(1,2));
+ u.emplace('b', Vector3d(3,4,5));
+
+ VectorComposite v(u);
+
+ ASSERT_FALSE(v.empty());
+
+ ASSERT_MATRIX_APPROX(u['a'], v['a'], 1e-20);
+ ASSERT_MATRIX_APPROX(u['b'], v['b'], 1e-20);
+}
+
+TEST(VectorComposite, constructor_from_list)
+{
+ VectorComposite v(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
+}
+
+TEST(VectorComposite, set)
+{
+ VectorComposite v;
+
+ v.set(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
+}
+
+TEST(VectorComposite, operatorStream)
+{
+ using namespace Eigen;
+
+ VectorComposite x;
+
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
+
+ WOLF_DEBUG("X = ", x);
+}
+
+TEST(VectorComposite, operatorPlus)
+{
+ VectorComposite x, y;
+
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
+ y.emplace('P', Vector2d(1,1));
+ y.emplace('O', Vector3d(2,2,2));
+
+ WOLF_DEBUG("x + y = ", x + y);
+
+ ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20);
+}
+
+TEST(VectorComposite, operatorMinus)
+{
+ VectorComposite x, y;
+
+ x.emplace('P', Vector2d(3,3));
+ x.emplace('O', Vector3d(6,6,6));
+ y.emplace('P', Vector2d(1,1));
+ y.emplace('O', Vector3d(2,2,2));
+
+ WOLF_DEBUG("x = ", x);
+ WOLF_DEBUG("y = ", y);
+ WOLF_DEBUG("x - y = ", x - y);
+
+ ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20);
+}
+
+TEST(VectorComposite, unary_Minus)
+{
+ VectorComposite x, y;
+
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
+
+ WOLF_DEBUG("-x = ", -x);
+
+ ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20);
+ ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20);
+}
+
+TEST(VectorComposite, stateVector)
+{
+ VectorComposite x;
+
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
+ x.emplace('V', Vector4d(3,3,3,3));
+
+ VectorXd y(5); y<<1,1,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("PO"), y, 1e-20);
+
+ y.resize(7);
+ y << 3,3,3,3,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("VO"), y, 1e-20);
+
+ y.resize(9);
+ y << 1,1,3,3,3,3,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("PVO"), y, 1e-20);
+}
+
+TEST(MatrixComposite, Constructor_empty)
+{
+ MatrixComposite M;
+
+ ASSERT_EQ(M.size() , 0);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_structure)
+{
+ MatrixComposite M("PO", "POV");
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at('P').size() , 3);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_structure_sizes)
+{
+ MatrixComposite M("PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at('P').size() , 3);
+
+ ASSERT_EQ(M.at('P','O').rows() , 3);
+ ASSERT_EQ(M.at('P','O').cols() , 4);
+
+ ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
+ ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes)
+{
+ MatrixXd m ( MatrixXd::Random(7,10) );
+
+ MatrixComposite M(m, "PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at('P').size() , 3);
+
+ ASSERT_EQ(M.at('P','O').rows() , 3);
+ ASSERT_EQ(M.at('P','O').cols() , 4);
+
+ ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
+ ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
+
+ ASSERT_MATRIX_APPROX(M.at('P','O'), m.block(0,3,3,4), 1e-20);
+ ASSERT_MATRIX_APPROX(M.at('O','V'), m.block(3,7,4,3), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes_mismatches)
+{
+ MatrixXd m;
+
+// // input m has too few rows
+// m.setRandom(6,10);
+// MatrixComposite M1(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too many rows
+// m.setRandom(8,10);
+// MatrixComposite M2(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too few cols
+// m.setRandom(7,9) ;
+// MatrixComposite M3(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too many cols
+// m.setRandom(7,11) ;
+// MatrixComposite M4(m, "PO", {3,4}, "POV", {3,4,3});
+
+}
+
+TEST(MatrixComposite, Zero)
+{
+ MatrixComposite M = MatrixComposite::zero("PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_MATRIX_APPROX(M.matrix("PO", "POV"), MatrixXd::Zero(7,10), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Identity)
+{
+ MatrixComposite M = MatrixComposite::identity("POV", {3,4,3});
+
+ ASSERT_MATRIX_APPROX(M.matrix("POV", "POV"), MatrixXd::Identity(10,10), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, emplace_operatorStream)
+{
+ MatrixComposite M;
+
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ ASSERT_TRUE(M.emplace('P', 'P', Mpp));
+ ASSERT_TRUE(M.emplace('P', 'O', Mpo));
+ ASSERT_TRUE(M.emplace('O', 'P', Mop));
+ ASSERT_TRUE(M.emplace('O', 'O', Moo));
+
+ cout << "M = " << M << endl;
+}
+
+//TEST(MatrixComposite, operatorBrackets)
+//{
+// MatrixComposite M;
+//
+// unsigned int psize, osize;
+// psize = 2;
+// osize = 3;
+//
+// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+//
+// Mpp.setOnes();
+// Mpo.setOnes(); Mpo *= 2;
+// Mop.setOnes(); Mop *= 3;
+// Moo.setOnes(); Moo *= 4;
+//
+// M.emplace('P', 'P', Mpp);
+// ASSERT_MATRIX_APPROX( M['P']['P'], Mpp, 1e-20);
+//
+// M.emplace('P', 'O', Mpo);
+// ASSERT_MATRIX_APPROX( M['P']['O'], Mpo, 1e-20);
+//
+// // return default empty matrix if block not present
+// MatrixXd N = M['O']['O'];
+// ASSERT_EQ(N.rows(), 0);
+// ASSERT_EQ(N.cols(), 0);
+//}
+
+//TEST(MatrixComposite, operatorParenthesis)
+//{
+// MatrixComposite M;
+//
+// unsigned int psize, osize;
+// psize = 2;
+// osize = 3;
+//
+// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+//
+// Mpp.setOnes();
+// Mpo.setOnes(); Mpo *= 2;
+// Mop.setOnes(); Mop *= 3;
+// Moo.setOnes(); Moo *= 4;
+//
+// M.emplace('P', 'P', Mpp);
+// ASSERT_MATRIX_APPROX( M('P', 'P'), Mpp, 1e-20);
+//
+// M.emplace('P', 'O', Mpo);
+// ASSERT_MATRIX_APPROX( M('P', 'O'), Mpo, 1e-20);
+//
+// // return default empty matrix if block not present
+// MatrixXd N = M('O', 'O');
+// ASSERT_EQ(N.rows(), 0);
+// ASSERT_EQ(N.cols(), 0);
+//}
+
+TEST(MatrixComposite, operatorAt)
+{
+ MatrixComposite M;
+
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace('P', 'P', Mpp);
+ ASSERT_MATRIX_APPROX( M.at('P', 'P'), Mpp, 1e-20);
+
+ M.emplace('P', 'O', Mpo);
+ ASSERT_MATRIX_APPROX( M.at('P', 'O'), Mpo, 1e-20);
+
+ // error if block not present
+ ASSERT_DEATH(MatrixXd N = M.at('O', 'O'), "");
+}
+
+TEST(MatrixComposite, productScalar)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixComposite M;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
+ WOLF_DEBUG("M = " , M);
+
+
+ // right-multiply by scalar
+ MatrixComposite R = M * 1.2;
+ ASSERT_MATRIX_APPROX(R.matrix("PO","PO"), 1.2 * M.matrix("PO","PO"), 1e-20);
+
+
+ // left-multiply by scalar
+ MatrixComposite L = 1.2 * M;
+ ASSERT_MATRIX_APPROX(L.matrix("PO","PO"), 1.2 * M.matrix("PO","PO"), 1e-20);
+
+
+}
+
+TEST(MatrixComposite, productVector)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ VectorComposite x;
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
+
+ cout << "x= " << x << endl;
+
+// WOLF_DEBUG("x = " , x);
+
+ MatrixComposite M;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
+ WOLF_DEBUG("M = " , M);
+
+ VectorComposite y;
+
+ y = M * x;
+
+// WOLF_DEBUG("y = M * x = " , y);
+
+ /* M * x = y
+ * p o
+ * p [1 1 2 2 2] p [1] p [14]
+ * [1 1 2 2 2] [1] [14]
+ * [3 3 4 4 4] * [2] = [30]
+ * o [3 3 4 4 4] o [2] o [30]
+ * [3 3 4 4 4] [2] [30]
+ */
+
+ Vector2d yp(14,14);
+ Vector3d yo(30,30,30);
+
+ ASSERT_MATRIX_APPROX(y.at('P'), yp, 1e-20);
+ ASSERT_MATRIX_APPROX(y.at('O'), yo, 1e-20);
+
+ // throw if x has extra blocks
+ // x.emplace('V', Vector2d(3,3));
+ // ASSERT_DEATH(y = M * x , ""); // M * x --> does not die if x has extra blocks wrt M
+
+ // throw if x has missing blocks
+ // x.erase('O');
+ // cout << "x = " << x << endl;
+ // ASSERT_DEATH(y = M * x , ""); // M * x --> exception if x has missing blocks wrt M, not caught by ASSERT_DEATH
+
+}
+
+TEST(MatrixComposite, product)
+{
+ unsigned int psize, osize, vsize;
+ psize = 2;
+ osize = 1;
+ vsize = 2;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
+ WOLF_DEBUG("M = " , M);
+
+ MatrixXd Noo(osize,osize), Nov(osize, vsize), Npo(psize,osize), Npv(psize,vsize);
+ Noo.setOnes();
+ Nov.setOnes(); Nov *= 2;
+ Npo.setOnes(); Npo *= 3;
+ Npv.setOnes(); Npv *= 4;
+
+ N.emplace('O', 'O', Noo);
+ N.emplace('O', 'V', Nov);
+ N.emplace('P', 'O', Npo);
+ N.emplace('P', 'V', Npv);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MN;
+
+ MN = M * N;
+
+ WOLF_DEBUG("MN = M * N = " , MN);
+
+ /* M * N = MN
+ * p o o v o v
+ * p [1 1 2] p [3 4 4] p [ 8 12 12]
+ * [1 1 2] * [3 4 4] = [ 8 12 12]
+ * o [3 3 4] o [1 2 2] o [22 32 32]
+ */
+
+ MatrixXd MNpo(MatrixXd::Ones(psize,osize) * 8);
+ MatrixXd MNpv(MatrixXd::Ones(psize,vsize) * 12);
+ MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22);
+ MatrixXd MNov(MatrixXd::Ones(osize,vsize) * 32);
+
+ ASSERT_MATRIX_APPROX(MN.at('P','O'), MNpo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at('P','V'), MNpv, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at('O','O'), MNoo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at('O','V'), MNov, 1e-20);
+
+ ASSERT_TRUE(MN.check());
+}
+
+TEST(MatrixComposite, propagate)
+{
+ unsigned int psize, osize, vsize, wsize;
+ psize = 2;
+ osize = 3;
+ vsize = 4;
+ wsize = 1;
+
+ MatrixComposite Q, J;
+
+ MatrixXd Qpp(psize,psize), Qpo(psize, osize), Qop(osize,psize), Qoo(osize,osize);
+ Qpp.setOnes();
+ Qpo.setOnes(); Qpo *= 2;
+ Qop.setOnes(); Qop *= 2;
+ Qoo.setOnes(); Qoo *= 3;
+
+ Q.emplace('P', 'P', Qpp);
+ Q.emplace('P', 'O', Qpo);
+ Q.emplace('O', 'P', Qop);
+ Q.emplace('O', 'O', Qoo);
+ WOLF_DEBUG("Q = " , Q);
+
+ MatrixXd Jvp(vsize,psize), Jvo(vsize, osize), Jwp(wsize,psize), Jwo(wsize,osize);
+ Jvp.setOnes();
+ Jvo.setOnes(); Jvo *= 2;
+ Jwp.setOnes(); Jwp *= 3;
+ Jwo.setOnes(); Jwo *= 4;
+
+ J.emplace('V', 'P', Jvp);
+ J.emplace('V', 'O', Jvo);
+ J.emplace('W', 'P', Jwp);
+ J.emplace('W', 'O', Jwo);
+ WOLF_DEBUG("J = " , J);
+
+ MatrixComposite JQJt;
+
+ JQJt = J.propagate(Q);
+
+ WOLF_DEBUG("JQJt = J * Q * J.tr = " , JQJt);
+
+ WOLF_DEBUG("JQJt = J * Q * J.tr = \n" , JQJt.matrix("VW", "VW"));
+
+ ASSERT_MATRIX_APPROX(JQJt.matrix("VW", "VW"), J.matrix("VW","PO") * Q.matrix("PO","PO") * J.matrix("VW","PO").transpose(), 1e-8);
+
+ ASSERT_TRUE(JQJt.check());
+
+}
+
+TEST(MatrixComposite, sum)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
+ WOLF_DEBUG("M = " , M);
+ N.emplace('P', 'P', Mpp);
+ N.emplace('P', 'O', Mpo);
+ N.emplace('O', 'P', Mop);
+ N.emplace('O', 'O', Moo);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MpN;
+
+ MpN = M + N;
+
+ WOLF_DEBUG("MpN = M + N = " , MpN);
+
+ ASSERT_MATRIX_APPROX(MpN.at('P','P'), 2 * Mpp, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at('P','O'), 2 * Mpo, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at('O','P'), 2 * Mop, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at('O','O'), 2 * Moo, 1e-10);
+
+}
+
+TEST(MatrixComposite, difference)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
+ WOLF_DEBUG("M = " , M);
+ N.emplace('P', 'P', Mpp);
+ N.emplace('P', 'O', Mpo);
+ N.emplace('O', 'P', Mop);
+ N.emplace('O', 'O', Moo);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MmN;
+
+ MmN = M - N;
+
+ WOLF_DEBUG("MmN = M - N = " , MmN);
+
+ ASSERT_MATRIX_APPROX(MmN.at('P','P'), Mpp * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at('P','O'), Mpo * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at('O','P'), Mop * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at('O','O'), Moo * 0, 1e-10);
+
+}
+
+TEST(MatrixComposite, unary_minus)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
+ WOLF_DEBUG("M = " , M);
+
+ MatrixComposite m;
+
+ m = - M;
+
+ WOLF_DEBUG("m = - M = " , m);
+
+ ASSERT_MATRIX_APPROX(m.at('P','P'), - M.at('P','P'), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at('P','O'), - M.at('P','O'), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at('O','P'), - M.at('O','P'), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at('O','O'), - M.at('O','O'), 1e-10);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ // ::testing::GTEST_FLAG(filter) = "VectorComposite.*";
+
+ // restrict to this test only
+ // ::testing::GTEST_FLAG(filter) = "MatrixComposite.propagate";
+// ::testing::GTEST_FLAG(filter) = "MatrixComposite.*";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index 03b1d0582280f294d1fbe5bd217b8294f861ca73..f8f70272b239a06f30ce5fdf6eed32308ae95689 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -1,11 +1,32 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/time_stamp.h"
#include <thread>
+using namespace wolf;
+
+TEST(WolfTestTimeStamp, TimeStampInvalid)
+{
+ auto t = TimeStamp::Invalid();
+ WOLF_DEBUG("t = ", t);
+ ASSERT_FALSE(t.ok());
+
+ t = -1;
+ WOLF_DEBUG("t = ", t);
+ ASSERT_FALSE(t.ok());
+
+ t = 0;
+ WOLF_DEBUG("t = ", t);
+ ASSERT_TRUE(t.ok());
+
+ t = 1;
+ WOLF_DEBUG("t = ", t);
+ ASSERT_TRUE(t.ok());
+}
+
TEST(WolfTestTimeStamp, TimeStampInitNow)
{
- wolf::TimeStamp start;
+ wolf::TimeStamp start = wolf::TimeStamp::Now();
// If we don't sleep, start == time_stamp sometimes.
// And sometimes start <= time_stamp ...
@@ -13,7 +34,7 @@ TEST(WolfTestTimeStamp, TimeStampInitNow)
ASSERT_NE(start.get(), 0);
- wolf::TimeStamp time_stamp;
+ wolf::TimeStamp time_stamp = wolf::TimeStamp::Now();
// std::cout << std::fixed;
// std::cout << std::setprecision(15);
@@ -28,7 +49,7 @@ TEST(WolfTestTimeStamp, TimeStampInitNow)
TEST(WolfTestTimeStamp, TimeStampInitScalar)
{
- wolf::Scalar val(101010);
+ double val(101010);
wolf::TimeStamp start(val);
@@ -46,9 +67,9 @@ TEST(WolfTestTimeStamp, TimeStampInitScalar)
TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano)
{
- wolf::Scalar sec(101010);
- wolf::Scalar nano(202020);
- wolf::Scalar val(101010.000202020);
+ double sec(101010);
+ double nano(202020);
+ double val(101010.000202020);
wolf::TimeStamp start(sec, nano);
@@ -84,7 +105,7 @@ TEST(WolfTestTimeStamp, TimeStampSetNow)
TEST(WolfTestTimeStamp, TimeStampSetScalar)
{
- wolf::Scalar val(101010.000202020);
+ double val(101010.000202020);
wolf::TimeStamp start;
start.set(val);
@@ -137,11 +158,11 @@ TEST(WolfTestTimeStamp, TimeStampEquality)
TEST(WolfTestTimeStamp, TimeStampInequality)
{
- wolf::TimeStamp start;
+ wolf::TimeStamp start = wolf::TimeStamp::Now();
std::this_thread::sleep_for(std::chrono::microseconds(1));
- wolf::TimeStamp time_stamp;
+ wolf::TimeStamp time_stamp = wolf::TimeStamp::Now();
// error: no match for ‘operator!=’
//ASSERT_NE(time_stamp, start);
@@ -162,7 +183,7 @@ TEST(WolfTestTimeStamp, TimeStampSubstraction)
{
wolf::TimeStamp t1;
wolf::TimeStamp t2(t1);
- wolf::Scalar dt(1e-5);
+ double dt(1e-5);
t2+=dt;
@@ -175,7 +196,7 @@ TEST(WolfTestTimeStamp, TimeStampAdding)
{
wolf::TimeStamp t1,t3;
wolf::TimeStamp t2(t1);
- wolf::Scalar dt(1e-5);
+ double dt(1e-5);
t2 +=dt;
t3 = t1+dt;
@@ -186,7 +207,7 @@ TEST(WolfTestTimeStamp, TimeStampAdding)
TEST(WolfTestTimeStamp, TimeStampOperatorOstream)
{
wolf::TimeStamp t(5);
- wolf::Scalar dt = 1e-4;
+ double dt = 1e-4;
t+=dt;
std::ostringstream ss1, ss2;
@@ -203,7 +224,7 @@ 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 t1(double(sec)+double(nano)*1e-9);
wolf::TimeStamp t2(sec,nano);
ASSERT_EQ(t1.getSeconds(),sec);
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 2f956b0846a505568c084147a1121904cc2a80da..a14c436ddc0864f7b55e1ef9db1fd451ecf494ea 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/processor/track_matrix.h"
@@ -16,51 +16,94 @@ class TrackMatrixTest : public testing::Test
public:
TrackMatrix track_matrix;
- Eigen::Vector2s m;
- Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01;
+ Eigen::Vector2d m;
+ Eigen::Matrix2d m_cov = Eigen::Matrix2d::Identity()*0.01;
+ FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
FeatureBasePtr f0, f1, f2, f3, f4;
+ ProblemPtr problem;
- virtual void SetUp()
+ void SetUp() override
{
- 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);
+ problem = Problem::create("PO", 2);
+ // unlinked captures
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
+ C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0);
+ C1 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 1.0);
+ C2 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 2.0);
+ C3 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 3.0);
+ C4 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 4.0);
+
+ // unlinked frames
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
+ F0 = std::make_shared<FrameBase>(0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(1.0, nullptr);
+ F2 = std::make_shared<FrameBase>(2.0, nullptr);
+ F3 = std::make_shared<FrameBase>(3.0, nullptr);
+ F4 = std::make_shared<FrameBase>(4.0, nullptr);
+
+ // unlinked features
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
+ f0 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f1 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+
+ // F0 and F4 are keyframes
+ F0->link(problem);
+ F4->link(problem);
+
+ // link captures
+ C0->link(F0);
+ C1->link(F1);
+ C2->link(F2);
+ C3->link(F3);
+ C4->link(F4);
}
};
TEST_F(TrackMatrixTest, newTrack)
{
- track_matrix.newTrack(C0, f0);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C1);
+ f3->link(C1);
+
+ track_matrix.newTrack(f0);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.newTrack(C0, f1);
+ track_matrix.newTrack(f1);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+
+ track_matrix.newTrack(f2);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3);
+
+ track_matrix.newTrack(f3);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4);
}
TEST_F(TrackMatrixTest, add)
{
- track_matrix.newTrack(C0, f0);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
- track_matrix.add(f0->trackId(), C1, f1);
- /* C0 C1 C2 snapshots
+ track_matrix.newTrack(f0);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+
+ track_matrix.add(f0->trackId(), f1);
+ /* KC0 C1 snapshots
*
* f0---f1 trk 0
*/
ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
ASSERT_EQ(f1->trackId(), f0->trackId());
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.add(f0->trackId(), C2, f2);
- /* C0 C1 C2 snapshots
+ track_matrix.add(f0->trackId(), f2);
+ /* KC0 C1 C2 snapshots
*
* f0---f1---f2 trk 0
*/
@@ -68,10 +111,51 @@ TEST_F(TrackMatrixTest, add)
ASSERT_EQ(f2->trackId(), f0->trackId());
}
+TEST_F(TrackMatrixTest, add2)
+{
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
+ f3->link(C1);
+
+ track_matrix.newTrack(f0);
+
+ track_matrix.add(f0, f1);
+ /* KC0 C1 snapshots
+ *
+ * f0---f1 trk 0
+ */
+ ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
+ ASSERT_EQ(f1->trackId(), f0->trackId());
+
+ track_matrix.add(f1, f2);
+ /* KC0 C1 C2 snapshots
+ *
+ * f0---f1---f2 trk 0
+ */
+ ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
+ ASSERT_EQ(f2->trackId(), f0->trackId());
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+
+ track_matrix.newTrack(f3);
+ /* KC0 C1 C2 snapshots
+ *
+ * f0---f1---f2 trk 0
+ * f3 trk 1
+ */
+ ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1);
+ ASSERT_NE(f3->trackId(), f0->trackId());
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+}
+
TEST_F(TrackMatrixTest, numTracks)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
/* C0 C1 C2 snapshots
*
@@ -80,16 +164,20 @@ TEST_F(TrackMatrixTest, numTracks)
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.add(f0->trackId(), C0, f2);
+ track_matrix.add(f0->trackId(), 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);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -111,9 +199,13 @@ TEST_F(TrackMatrixTest, first_last_Feature)
* f2 trk 1
*/
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C1, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C1);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f0);
ASSERT_EQ(track_matrix.lastFeature (f0->trackId()), f1);
@@ -124,9 +216,13 @@ TEST_F(TrackMatrixTest, first_last_Feature)
TEST_F(TrackMatrixTest, remove_ftr)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -153,15 +249,29 @@ TEST_F(TrackMatrixTest, remove_ftr)
ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1);
track_matrix.remove(f1);
+ /* C0 C1 C2 snapshots
+ *
+ * f2 trk 1
+ */
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
+
+ track_matrix.remove(f2);
+ /* C0 C1 C2 snapshots
+ *
+ */
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
}
TEST_F(TrackMatrixTest, remove_trk)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -182,9 +292,13 @@ TEST_F(TrackMatrixTest, remove_trk)
TEST_F(TrackMatrixTest, remove_snapshot)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -209,9 +323,13 @@ TEST_F(TrackMatrixTest, remove_snapshot)
TEST_F(TrackMatrixTest, track)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -233,9 +351,13 @@ TEST_F(TrackMatrixTest, track)
TEST_F(TrackMatrixTest, snapshot)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -257,9 +379,13 @@ TEST_F(TrackMatrixTest, snapshot)
TEST_F(TrackMatrixTest, trackAsVector)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -277,9 +403,13 @@ TEST_F(TrackMatrixTest, trackAsVector)
TEST_F(TrackMatrixTest, snapshotAsList)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -297,11 +427,17 @@ TEST_F(TrackMatrixTest, snapshotAsList)
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);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
+ f3->link(C0);
+ f4->link(C1);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.add(f0->trackId(), f2);
+ track_matrix.newTrack(f3);
+ track_matrix.add(f3->trackId(), f4);
/* C0 C1 C2 C3 snapshots
*
@@ -321,6 +457,36 @@ TEST_F(TrackMatrixTest, matches)
ASSERT_EQ(pairs.size(), (unsigned int) 0);
}
+TEST_F(TrackMatrixTest, trackAtKeyframes)
+{
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
+ f3->link(C1);
+ f4->link(C4);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.add(f0->trackId(), f2);
+ track_matrix.add(f0->trackId(), f4);
+ track_matrix.newTrack(f3);
+ /* KC0 C1 C2 C3 KC4 snapshots
+ *
+ * f0---f1---f2--------f4 trk 0
+ * |
+ * f3 trk 1
+ */
+
+ wolf::Track trk_kf_0 = track_matrix.trackAtKeyframes(f0->trackId());
+ ASSERT_EQ(trk_kf_0.size(), 2);
+ ASSERT_EQ(trk_kf_0[0.0], f0);
+ ASSERT_EQ(trk_kf_0[4.0], f4);
+
+ wolf::Track trk_kf_1 = track_matrix.trackAtKeyframes(f3->trackId());
+ ASSERT_EQ(trk_kf_1.size(), 0);
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 50a78bc0f7b4705a9e17ddf1429ad6ee81aafe48..7c483113468573388e5cca87ebf1bf243f41a911 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -5,44 +5,19 @@
* Author: jsola
*/
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+
#include "core/problem/problem.h"
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
#include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
#include <iostream>
using namespace wolf;
-struct DummySolverManager : public SolverManager
-{
- DummySolverManager(const ProblemPtr& _problem)
- : SolverManager(_problem)
- {
- //
- }
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool hasConverged(){return true;};
- virtual SizeStd iterations(){return 0;};
- virtual Scalar initialCost(){return 0;};
- virtual Scalar finalCost(){return 0;};
- virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr){};
- virtual void removeFactor(const FactorBasePtr& fac_ptr){};
- virtual void addStateBlock(const StateBlockPtr& state_ptr){};
- virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
-};
-
/// Set to true if you want debug info
bool debug = false;
@@ -57,66 +32,33 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, nullptr, nullptr);
- FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, nullptr, nullptr);
- FrameBasePtr F3 = std::make_shared<FrameBase>(AUXILIARY, 3, nullptr, nullptr);
- FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 4, nullptr, nullptr);
- T->addFrame(F1);
- T->addFrame(F2);
- T->addFrame(F3);
- T->addFrame(F4);
+ FrameBasePtr F1 = P->emplaceFrame( 1, Eigen::Vector3d::Zero() );
+ FrameBasePtr F2 = P->emplaceFrame( 2, Eigen::Vector3d::Zero() );
+ FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+ // P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
+ FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameStructure(),
+ // P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
FrameBasePtr KF; // closest key-frame queried
- KF = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return F1
+ KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1
ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return F1
+ KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1
ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return F2
+ KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
+ KF = T->closestFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
+ KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(4.2); // after the last frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
-}
-
-TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
-{
-
- ProblemPtr P = Problem::create("PO", 2);
- TrajectoryBasePtr T = P->getTrajectory();
-
- // Trajectory status:
- // KF1 KF2 F3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- FrameBasePtr F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, nullptr, nullptr);
- FrameBasePtr F2 = FrameBase::emplace<FrameBase>(T, AUXILIARY, 2, nullptr, nullptr);
- FrameBasePtr F3 = FrameBase::emplace<FrameBase>(T, NON_ESTIMATED, 3, nullptr, nullptr);
-
- FrameBasePtr KF; // closest key-frame queried
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8); // before all keyframes --> return f0
- ASSERT_EQ(KF->id(), F1->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(1.1); // between keyframes --> return F1
- ASSERT_EQ(KF->id(), F1->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(1.9); // between keyframes --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(3.2); // after the last frame --> return F2
+ KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
}
@@ -127,44 +69,39 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- DummySolverManager N(P);
+ SolverManagerDummy N(P);
// Trajectory status:
// KF1 KF2 F3 frames
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-
- // FrameBasePtr f1 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- // FrameBasePtr f2 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- // FrameBasePtr f3 = FrameBase::emplace<FrameBase>(T, NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-
std::cout << __LINE__ << std::endl;
- // add frames and keyframes
- F1->link(T);
+ // add F1
+ FrameBasePtr F1 = P->emplaceFrame(1, Eigen::Vector3d::Zero()); // 2 non-fixed
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
- F2->link(T);
+ // add F2
+ FrameBasePtr F2 = P->emplaceFrame(2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
- F3->link(T);
+ // add F3
+ FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+ // P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F2->id());
std::cout << __LINE__ << std::endl;
N.update();
@@ -174,24 +111,23 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// remove frames and keyframes
F2->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F1->id());
std::cout << __LINE__ << std::endl;
F3->remove(); // F
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F1->id());
F1->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 0);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 0);
std::cout << __LINE__ << std::endl;
N.update();
@@ -200,127 +136,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
}
-TEST(TrajectoryBase, KeyFramesAreSorted)
-{
- using std::make_shared;
-
- ProblemPtr P = Problem::create("PO", 2);
- TrajectoryBasePtr T = P->getTrajectory();
-
- // Trajectory status:
- // KF1 KF2 F3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 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
- F2->link(T);
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F3->link(T);
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F1->link(T);
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F3->setKey(); // set KF3
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- auto F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 1.5);
- // Trajectory status:
- // KF1 KF2 KF3 F4 frames
- // 1 2 3 1.5 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F4->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- F4->setKey();
- // Trajectory status:
- // KF1 KF4 KF2 KF3 frames
- // 1 1.5 2 3 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- F2->setTimeStamp(4);
- // Trajectory status:
- // KF1 KF4 KF3 KF2 frames
- // 1 1.5 3 4 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F4->setTimeStamp(0.5);
- // Trajectory status:
- // KF4 KF2 KF3 KF2 frames
- // 0.5 1 3 4 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
-
- auto F5 = P->emplaceFrame(AUXILIARY, Eigen::Vector3s::Zero(), 1.5);
- // Trajectory status:
- // KF4 KF2 AuxF5 KF3 KF2 frames
- // 0.5 1 1.5 3 4 time stamps
- // --+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F5->setTimeStamp(5);
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 frames
- // 0.5 1 3 4 5 time stamps
- // --+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F5->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- auto F6 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 6);
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 F6 frames
- // 0.5 1 3 4 5 6 time stamps
- // --+-----+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F6->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- auto F7 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 5.5);
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 F7 F6 frames
- // 0.5 1 3 4 5 5.5 6 time stamps
- // --+-----+-----+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F7->id()); //Only auxiliary and key-frames are sorted
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
-}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8b86469e6e68843dc650feb5bea1e38c9db96ce
--- /dev/null
+++ b/test/gtest_tree_manager.cpp
@@ -0,0 +1,106 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "dummy/tree_manager_dummy.h"
+#include "core/yaml/parser_yaml.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(TreeManager, make_shared)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = TreeManagerDummy::create("tree_manager", ParamsGM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerDummy::create("tree_manager", server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, autoConf)
+{
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF
+}
+
+TEST(TreeManager, autoConfNone)
+{
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
+}
+
+TEST(TreeManager, keyFrameCallback)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(GM->n_KF_, 0);
+
+ auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
+ P->keyFrameCallback(F0, nullptr, 0);
+
+ ASSERT_EQ(GM->n_KF_, 1);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ec1cbe4d42bb436cb2521793a074613f6ae4a3c
--- /dev/null
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -0,0 +1,297 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/problem/problem.h"
+#include "core/tree_manager/tree_manager_sliding_window.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_pose_3d.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(TreeManagerSlidingWindow, make_shared)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
+
+ auto GM = std::make_shared<TreeManagerSlidingWindow>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
+
+ auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerSlidingWindow::create("tree_manager", server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, autoConf)
+{
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+}
+
+TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
+{
+ // window size: 3
+ // first 2 frames fixed
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ // FRAME 1 ----------------------------------------------------------
+ auto F1 = P->getTrajectory()->getLastFrame();
+ EXPECT_TRUE(F1 != nullptr);
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ // Check F1 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+
+ // FRAME 3 ----------------------------------------------------------
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ // Check F1 and F2 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_TRUE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+
+ // FRAME 4 ----------------------------------------------------------
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
+
+ // Check F1 (virally) removed
+ EXPECT_TRUE(F1->isRemoving());
+ EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C12->isRemoving()); // Virally removed
+ EXPECT_TRUE(f12->isRemoving()); // Virally removed
+ // Check F2 and F3 fixed
+ EXPECT_TRUE(F2->isFixed());
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+
+ // FRAME 5 ----------------------------------------------------------
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
+
+ // Check F1 and F2 (virally) removed
+ EXPECT_TRUE(F1->isRemoving());
+ EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C12->isRemoving()); // Virally removed
+ EXPECT_TRUE(f12->isRemoving()); // Virally removed
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C2->isRemoving()); // Virally removed
+ EXPECT_TRUE(f2->isRemoving()); // Virally removed
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+ // Check F3 and F4 fixed
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_TRUE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+}
+
+TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
+{
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ // FRAME 1 (prior) ----------------------------------------------------------
+ auto F1 = P->getTrajectory()->getLastFrame();
+ EXPECT_TRUE(F1 != nullptr);
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ // Check no frames fixed
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+
+ // FRAME 3 ----------------------------------------------------------
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ // Check no frames fixed
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+
+ // FRAME 4 ----------------------------------------------------------
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_TRUE(F1->isRemoving());
+ EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C12->isRemoving()); //Not virally removed
+ EXPECT_FALSE(f12->isRemoving()); //Not virally removed
+ // Check no frames fixed
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+
+ // FRAME 5 ----------------------------------------------------------
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_TRUE(F1->isRemoving());
+ EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C23->isRemoving()); //Not virally removed
+ EXPECT_FALSE(f23->isRemoving()); //Not virally removed
+ // Check no frames fixed
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65092ed3b9198ff7e6304750ee0c3e06233cf5a5
--- /dev/null
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -0,0 +1,1081 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/problem/problem.h"
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/capture/capture_void.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_pose_3d.h"
+#include "core/solver/factory_solver.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(TreeManagerSlidingWindowDualRate, make_shared)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+
+ auto GM = std::make_shared<TreeManagerSlidingWindowDualRate>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+
+ auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", ParamsGM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, autoConf)
+{
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
+{
+ /* sliding window dual rate:
+ * n_frames: 5
+ * n_fix_first_frames: 2
+ * n_frames_recent: 3
+ * rate_old_frames: 2
+ */
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ /* FRAME 1 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )( )(F1)
+ * fix fix
+ */
+ auto F1 = P->getTrajectory()->getLastFrame();
+ ASSERT_TRUE(F1 != nullptr);
+ ASSERT_FALSE(F1->getCaptureList().empty());
+ auto C1 = F1->getCaptureList().front();
+ ASSERT_FALSE(C1->getFeatureList().empty());
+ auto f1 = C1->getFeatureList().front();
+ ASSERT_FALSE(f1->getFactorList().empty());
+ auto c1 = f1->getFactorList().front();
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+ // Check no frame fixed
+ EXPECT_FALSE(F1->isFixed());
+
+ /* FRAME 2 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )(F1)(F2)
+ * fix fix
+ */
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+ // Check no frame fixed
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+
+ /* FRAME 3 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) (F1)(F2)(F3)
+ * fix fix
+ */
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ // Check no frame fixed
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+
+ /* FRAME 4 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1)(F2)(F3)(F4)
+ * fix fix
+ */
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ // Check F1 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+
+ /* FRAME 5 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1) (F3)(F4)(F5)
+ * fix fix
+ */
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ // Check F1 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+
+ /* FRAME 6 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3)(F4)(F5)(F6)
+ * fix fix
+ */
+ auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
+ P->keyFrameCallback(F6, nullptr, 0);
+
+ // absolute factor
+ auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov);
+ auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false);
+ // displacement
+ auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
+ auto c56 = FactorBase::emplace<FactorRelativePose3d>(f56, f56, F5, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ // Check F1 and F3 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+
+ /* FRAME 7 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3) (F5)(F6)(F7)
+ * fix fix
+ */
+ auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
+ P->keyFrameCallback(F7, nullptr, 0);
+
+ // absolute factor
+ auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov);
+ auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false);
+ // displacement
+ auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
+ auto c67 = FactorBase::emplace<FactorRelativePose3d>(f67, f67, F6, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C45->isRemoving()); // Virally removed
+ EXPECT_TRUE(f45->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ // Check F1 and F3 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+
+ /* FRAME 8 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F3) (F5)(F6)(F7)(F8)
+ * fix fix
+ */
+ auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
+ P->keyFrameCallback(F8, nullptr, 0);
+
+ // absolute factor
+ auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov);
+ auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false);
+ // displacement
+ auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
+ auto c78 = FactorBase::emplace<FactorRelativePose3d>(f78, f78, F7, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_TRUE(F1->isRemoving()); // First frame removed
+ EXPECT_TRUE(c1->isRemoving());
+ EXPECT_TRUE(C1->isRemoving());
+ EXPECT_TRUE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C45->isRemoving()); // Virally removed
+ EXPECT_TRUE(f45->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ EXPECT_FALSE(F8->isRemoving());
+ EXPECT_FALSE(c8->isRemoving());
+ EXPECT_FALSE(C8->isRemoving());
+ EXPECT_FALSE(f8->isRemoving());
+ EXPECT_FALSE(c78->isRemoving());
+ EXPECT_FALSE(C78->isRemoving());
+ EXPECT_FALSE(f78->isRemoving());
+
+ // Check F1 and F3 fixed
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_TRUE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+ EXPECT_FALSE(F8->isFixed());
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
+{
+ /* sliding window dual rate:
+ * n_frames: 5
+ * n_fix_first_frames: 0
+ * n_frames_recent: 3
+ * rate_old_frames: 2
+ */
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ /* FRAME 1 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )( )(F1)
+ */
+ auto F1 = P->getTrajectory()->getLastFrame();
+ ASSERT_TRUE(F1 != nullptr);
+ ASSERT_FALSE(F1->getCaptureList().empty());
+ auto C1 = F1->getCaptureList().front();
+ ASSERT_FALSE(C1->getFeatureList().empty());
+ auto f1 = C1->getFeatureList().front();
+ ASSERT_FALSE(f1->getFactorList().empty());
+ auto c1 = f1->getFactorList().front();
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+
+ /* FRAME 2 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )(F1)(F2)
+ */
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+
+ /* FRAME 3 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) (F1)(F2)(F3)
+ */
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+
+ /* FRAME 4 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1)(F2)(F3)(F4)
+ */
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+
+ /* FRAME 5 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1) (F3)(F4)(F5)
+ */
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+
+ /* FRAME 6 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3)(F4)(F5)(F6)
+ */
+ auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
+ P->keyFrameCallback(F6, nullptr, 0);
+
+ // absolute factor
+ auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov);
+ auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false);
+ // displacement
+ auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
+ auto c56 = FactorBase::emplace<FactorRelativePose3d>(f56, f56, F5, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+
+ /* FRAME 7 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3) (F5)(F6)(F7)
+ */
+ auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
+ P->keyFrameCallback(F7, nullptr, 0);
+
+ // absolute factor
+ auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov);
+ auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false);
+ // displacement
+ auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
+ auto c67 = FactorBase::emplace<FactorRelativePose3d>(f67, f67, F6, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+ EXPECT_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C45->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f45->isRemoving()); // Not virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+
+ /* FRAME 8 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F3) (F5)(F6)(F7)(F8)
+ */
+ auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
+ P->keyFrameCallback(F8, nullptr, 0);
+
+ // absolute factor
+ auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov);
+ auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false);
+ // displacement
+ auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
+ auto c78 = FactorBase::emplace<FactorRelativePose3d>(f78, f78, F7, nullptr, false, TOP_MOTION);
+
+ // Checks
+ EXPECT_TRUE(F1->isRemoving()); // First frame removed
+ EXPECT_TRUE(c1->isRemoving());
+ EXPECT_TRUE(C1->isRemoving());
+ EXPECT_TRUE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+ EXPECT_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C45->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f45->isRemoving()); // Not virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ EXPECT_FALSE(F8->isRemoving());
+ EXPECT_FALSE(c8->isRemoving());
+ EXPECT_FALSE(C8->isRemoving());
+ EXPECT_FALSE(f8->isRemoving());
+ EXPECT_FALSE(c78->isRemoving());
+ EXPECT_FALSE(C78->isRemoving());
+ EXPECT_FALSE(f78->isRemoving());
+
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+ EXPECT_FALSE(F8->isFixed());
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
+{
+ // SLIDING WINDOW DUAL RATE
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+ ProblemPtr problem = Problem::autoSetup(server);
+ SolverManagerPtr solver = FactorySolver::create("SolverCeres", problem, server);
+
+ // BASELINE
+ ParserYaml parser_bl = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml", wolf_root);
+ ParamsServer server_bl = ParamsServer(parser_bl.getParams());
+ ProblemPtr problem_bl = Problem::autoSetup(server_bl);
+ SolverManagerPtr solver_bl = FactorySolver::create("SolverCeres", problem_bl, server);
+
+ // aux variables
+ Vector7d data;
+ Matrix6d data_cov = Matrix6d::Identity();
+ TimeStamp t(0.0);
+ double dt = 1;
+ CaptureMotionPtr capture = std::make_shared<CaptureOdom3d>(t,
+ problem->getSensor("odom"),
+ data,
+ data_cov);
+ CaptureMotionPtr capture_bl = std::make_shared<CaptureOdom3d>(t,
+ problem_bl->getSensor("odom"),
+ data,
+ data_cov);
+
+ // START MOTION CAPTURES
+ for (int i = 0; i<20; i++)
+ {
+ t += dt;
+ WOLF_INFO("-------------------------- t: ", t);
+
+ // random movement
+ data.setRandom(); data.tail<4>().normalize();
+
+ // sliding window process
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->process();
+
+ // baseline process
+ capture_bl->setTimeStamp(t);
+ capture_bl->setData(data);
+ capture_bl->process();
+
+ WOLF_INFO("state: ", problem->getState().vector("PO").transpose());
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+ problem_bl->getState().vector("PO").transpose(),
+ 1e-12);
+
+ // Solve
+ solver->solve();
+ solver_bl->solve();
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+ problem_bl->getState().vector("PO").transpose(),
+ 1e-12);
+
+ ASSERT_TRUE(problem->check());
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/demos/demo_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
similarity index 64%
rename from demos/demo_yaml_conversions.cpp
rename to test/gtest_yaml_conversions.cpp
index b8e1f41f9c0f8b6ae50e65c4fec943dbb1305a7d..04acb34ee69f1da8f368966ddef0acaa6c93a7c8 100644
--- a/demos/demo_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.cpp
@@ -5,19 +5,23 @@
* \author: jsola
*/
+#include "core/common/wolf.h"
+#include "core/utils/utils_gtest.h"
#include "core/yaml/yaml_conversion.h"
-
#include <yaml-cpp/yaml.h>
-
#include <eigen3/Eigen/Dense>
-
#include <iostream>
//#include <fstream>
-int main()
-{
+using namespace Eigen;
+using namespace wolf;
- using namespace Eigen;
+TEST(MapYaml, save_2d)
+{
+ MatrixXd M23(2,3);
+ MatrixXd M33(3,3);
+ M23 << 1, 2, 3, 4, 5, 6;
+ M33 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
std::cout << "\nTrying different yaml specs for matrix..." << std::endl;
@@ -31,24 +35,35 @@ int main()
MatrixXd Mx = mat_sized_23.as<MatrixXd>();
std::cout << "Dyn-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << Mx << std::endl;
+ ASSERT_MATRIX_APPROX(Mx, M23, 1e-12);
- Matrix<double, 2, Dynamic> M2D = mat_sized_23.as<Matrix<double, 2, Dynamic>>();
- std::cout << "2-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << M2D << std::endl;
+ Matrix<double, 2, Dynamic> M2d = mat_sized_23.as<Matrix<double, 2, Dynamic>>();
+ std::cout << "2-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << M2d << std::endl;
+ ASSERT_MATRIX_APPROX(M2d, M23, 1e-12);
Matrix<double, Dynamic, 3> MD3 = mat_sized_23.as<Matrix<double, Dynamic, 3>>();
std::cout << "Dyn-3 [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << MD3 << std::endl;
+ ASSERT_MATRIX_APPROX(MD3, M23, 1e-12);
Matrix3d M3 = mat_sized_33.as<Matrix3d>();
std::cout << "3-3 [[3,3] ,[1, 2, 3, 4, 5, 6, 7, 8, 9] ] = \n" << M3 << std::endl;
+ ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
- M2D = mat_23.as<Matrix<double, 2, Dynamic>>();
- std::cout << "2-Dyn [1, 2, 3, 4, 5, 6] = \n" << M2D << std::endl;
+ M2d = mat_23.as<Matrix<double, 2, Dynamic>>();
+ std::cout << "2-Dyn [1, 2, 3, 4, 5, 6] = \n" << M2d << std::endl;
+ ASSERT_MATRIX_APPROX(M2d, M23, 1e-12);
MD3 = mat_23.as<Matrix<double, Dynamic, 3>>();
std::cout << "Dyn-3 [1, 2, 3, 4, 5, 6] = \n" << MD3 << std::endl;
+ ASSERT_MATRIX_APPROX(MD3, M23, 1e-12);
M3 = mat_33.as<Matrix3d>();
std::cout << "3-3 [1, 2, 3, 4, 5, 6, 7, 8, 9] = \n" << M3 << std::endl;
+ ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
+}
- return 0;
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/test/params1.yaml b/test/params1.yaml
deleted file mode 100644
index 4ad74198329b61d86b023f17f9a7016224d5489a..0000000000000000000000000000000000000000
--- a/test/params1.yaml
+++ /dev/null
@@ -1 +0,0 @@
- follow: "/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/params3.yaml b/test/params3.yaml
deleted file mode 100644
index 78489d218fad394f90364a1e1d758daa0270e7cd..0000000000000000000000000000000000000000
--- a/test/params3.yaml
+++ /dev/null
@@ -1 +0,0 @@
- jump: "@/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/serialization/CMakeLists.txt b/test/serialization/CMakeLists.txt
deleted file mode 100644
index 6902132bd458245e2e45012662fdcf0b348b7d4a..0000000000000000000000000000000000000000
--- a/test/serialization/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-# cereal
-IF(cereal_FOUND)
- add_subdirectory(cereal)
-ENDIF(cereal_FOUND)
diff --git a/test/serialization/cereal/CMakeLists.txt b/test/serialization/cereal/CMakeLists.txt
deleted file mode 100644
index 733a36e25921c4f7c142d59884fb5c9b7dd9e4e4..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-# NodeBase serialization class test
-wolf_add_gtest(gtest_cereal_serialization_node_base gtest_serialization_node_base.cpp)
-target_link_libraries(gtest_cereal_serialization_node_base ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_local_parametrization
- gtest_serialization_local_parametrization.cpp)
-target_link_libraries(gtest_cereal_serialization_local_parametrization ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_sensor_intrinsic_base
- gtest_serialization_sensor_intrinsic_base.cpp)
-target_link_libraries(gtest_cereal_serialization_sensor_intrinsic_base ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_sensor_odom2d_intrinsic
- gtest_serialization_sensor_odom2d_intrinsic.cpp)
-target_link_libraries(gtest_cereal_serialization_sensor_odom2d_intrinsic ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_save_load
- gtest_serialization_save_load.cpp)
-target_link_libraries(gtest_cereal_serialization_save_load ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_processor_odom3d_params
- gtest_serialization_processor_odom3d_params.cpp)
-target_link_libraries(gtest_cereal_serialization_processor_odom3d_params ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_processor_odom2d_params
- gtest_serialization_processor_odom2d_params.cpp)
-target_link_libraries(gtest_cereal_serialization_processor_odom2d_params ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_time_stamp
- gtest_serialization_time_stamp.cpp)
-target_link_libraries(gtest_cereal_serialization_time_stamp ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_core
- gtest_serialization_eigen_core.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_core ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_geometry
- gtest_serialization_eigen_geometry.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_geometry ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_sparse
- gtest_serialization_eigen_sparse.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_sparse ${PROJECT_NAME})
diff --git a/test/serialization/cereal/gtest_serialization_eigen_core.cpp b/test/serialization/cereal/gtest_serialization_eigen_core.cpp
deleted file mode 100644
index 28859dd7a5b7db706e041489341bf4c010eb126e..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_core.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_core.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationEigen : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationEigen()
- {
- nb_ = f_mat_t::Random();
-
- dnb_ = d_mat_t::Random(10, 10);
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_eigen";
-
- const std::vector<std::string> exts = {".bin"/*, ".xml", ".json"*/};
-
- using f_mat_t = Eigen::Matrix<double, 5, 5>;
-
- using d_mat_t = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
-
- f_mat_t nb_;
-
- d_mat_t dnb_;
-};
-
-TEST_F(WolfTestCerealSerializationEigen,
- CerealSerializationEigenFixedMat)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- WolfTestCerealSerializationEigen::f_mat_t nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- EXPECT_EQ(nb_load, nb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationEigen::"
- "CerealSerializationEigenFixedMat !\n");
-}
-
-TEST_F(WolfTestCerealSerializationEigen,
- CerealSerializationEigenDynamicMat)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, dnb_ ) )
- << "Failed on saving " << full_path;
-
- WolfTestCerealSerializationEigen::d_mat_t dnb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, dnb_load ) )
- << "Failed on loading " << full_path;
-
- EXPECT_EQ(dnb_load, dnb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationEigen::"
- "CerealSerializationEigenDynamicMat !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
deleted file mode 100644
index 87cafb4d5f171bafb74670348cd562ad49b1d306..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_geometry.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationEigenGeo : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationEigenGeo()
- {
- iso_2d_ = Eigen::Isometry2d(Eigen::Rotation2Dd(0.17));
- iso_2d_.translation() << 0.5, 1.8;
-
- q_d_ = Eigen::Vector4d().setRandom().normalized();
-
- iso_3d_ = Eigen::Isometry3d(q_d_);
- iso_3d_.translation() << -7.245, +3.88, 0.0001;
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_eigen_geo";
-
- const std::vector<std::string> exts = {".bin"/*, ".xml", ".json"*/};
-
- Eigen::Isometry2d iso_2d_;
- Eigen::Isometry3d iso_3d_;
- Eigen::Quaterniond q_d_;
-};
-
-TEST_F(WolfTestCerealSerializationEigenGeo,
- CerealSerializationEigenIso2d)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, iso_2d_, iso_3d_, q_d_) )
- << "Failed on saving " << full_path;
-
- Eigen::Isometry2d iso_2d_loaded;
- Eigen::Isometry3d iso_3d_loaded;
- Eigen::Quaterniond q_d_loaded;
-
- ASSERT_NO_THROW( wolf::load( full_path, iso_2d_loaded, iso_3d_loaded, q_d_loaded) )
- << "Failed on loading " << full_path;
-
- ASSERT_MATRIX_APPROX(iso_2d_.matrix(), iso_2d_loaded.matrix(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(iso_3d_.matrix(), iso_3d_loaded.matrix(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(q_d_.coeffs(), q_d_loaded.coeffs(), wolf::Constants::EPS);
- }
-
- PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
deleted file mode 100644
index 0b803afdd30778a58b9536343c5ba5248ec8a148..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_sparse.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <fstream>
-
-class WolfTestCerealSerializationEigenSparse : public testing::Test
-{
-public:
-
- using triplet_t = Eigen::Triplet<double>;
- using sparse_mat_t = Eigen::SparseMatrix<double>;
-
- WolfTestCerealSerializationEigenSparse()
- {
- triplet_list_.reserve(10);
-
- for(int i=0; i<10; ++i)
- triplet_list_.emplace_back(i,i,i*5);
-
- m_.resize(10, 10);
- m_.setFromTriplets(triplet_list_.begin(), triplet_list_.end());
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename_t = "serialization_eigen_triplet";
- const std::string filename_m = "serialization_eigen_sparse";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- triplet_t t_ = Eigen::Triplet<double>(1, 2, 5.5);
-
- std::vector<triplet_t> triplet_list_;
- Eigen::SparseMatrix<double> m_;
-};
-
-TEST_F(WolfTestCerealSerializationEigenSparse,
- CerealSerializationEigenTriplet)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename_t + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, t_) )
- << "Failed on saving " << full_path;
-
- triplet_t t;
-
- ASSERT_NO_THROW( wolf::load( full_path, t) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(t_.row(), t.row());
- ASSERT_EQ(t_.col(), t.col());
- ASSERT_EQ(t_.value(), t.value());
- }
-
- PRINT_TEST_FINISHED;
-}
-
-TEST_F(WolfTestCerealSerializationEigenSparse,
- CerealSerializationEigenSparseMatrix)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename_m + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, m_) )
- << "Failed on saving " << full_path;
-
- sparse_mat_t m;
-
- ASSERT_NO_THROW( wolf::load( full_path, m) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(m_.rows(), m.rows());
- ASSERT_EQ(m_.cols(), m.cols());
-
- std::vector<triplet_t> triplet_list;
- triplet_list.reserve(10);
-
- for (int k=0; k<m.outerSize(); ++k)
- for (sparse_mat_t::InnerIterator it(m, k); it; ++it)
- {
- triplet_list.emplace_back(it.row(), it.col(), it.value());
- }
-
- ASSERT_EQ(triplet_list_.size(), triplet_list.size());
-
- for (int i=0; i<triplet_list_.size(); ++i)
- {
- ASSERT_EQ(triplet_list_[i].row(), triplet_list[i].row());
- ASSERT_EQ(triplet_list_[i].col(), triplet_list[i].col());
- ASSERT_EQ(triplet_list_[i].value(), triplet_list[i].value());
- }
- }
-
- PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
deleted file mode 100644
index 3df34a27aa683fb5099cb8c108a25fe0f473f31b..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
+++ /dev/null
@@ -1,571 +0,0 @@
-/*
- * gtest_node_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_local_parametrization_quaternion.h"
-#include "../../../serialization/cereal/serialization_local_parametrization_homogeneous.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-///////////////////////////////////////
-/// LocalParametrizationHomogeneous ///
-///////////////////////////////////////
-
-const std::string path_to_io = "/tmp/";
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrBIN !\n");
-}
-
-//////////////////////////////////////
-/// LocalParametrizationQuaternion ///
-//////////////////////////////////////
-
-//////////////////////////////////////
-/// LOCAL ///
-//////////////////////////////////////
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrBIN !\n");
-}
-
-//////////////////////////////////////
-/// GLOBAL ///
-//////////////////////////////////////
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrBIN !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_node_base.cpp b/test/serialization/cereal/gtest_serialization_node_base.cpp
deleted file mode 100644
index 8b6140fe70f6afe484249361a516b59d8b5dcf7b..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_node_base.cpp
+++ /dev/null
@@ -1,247 +0,0 @@
-/*
- * gtest_node_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_node_base.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationNodeBase : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationNodeBase() /*:
- nb(nb_class),
- nb_ptr(std::make_shared<wolf::NodeBase>(nb_class))*/
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_class = "Foo";
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_type = "Bar";
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_name = "Dummy";
-
- decltype(std::declval<wolf::NodeBase>().nodeId()) id;
-
-// wolf::NodeBase nb;
-// wolf::NodeBasePtr nb_ptr;
-};
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseXML)
-{
- {
- // This guy has node_id = 1
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- // This guy has node_id = 2
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrXML)
-{
- {
- // This guy has node_id = 3
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseJSON)
-{
- {
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::NodeBase blank("This guy has node_id = 1");
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrJSON)
-{
- {
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseBinary)
-{
- {
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::NodeBase blank("This guy has node_id = 1");
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrBinary)
-{
- {
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp b/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
deleted file mode 100644
index 89fbef3d69dff80688dd234b0cec3742233b9220..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_processor_odom2d_params.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationProcessorParamsOdom2D : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationProcessorParamsOdom2D()
- {
- nb_.name = "NAME";
- nb_.type = "ODOM 2D";
-
- nb_.dist_traveled_th_ = 0.17;
- nb_.theta_traveled_th_ = 0.3;
- nb_.cov_det = 0.4;
- nb_.elapsed_time_th_ = 1.5;
- nb_.unmeasured_perturbation_std = 1e-5;
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_processor_odom2d_params";
- const std::string ptr_ext = "_ptr";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- wolf::ProcessorParamsOdom2D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationProcessorParamsOdom2D,
- CerealSerializationProcessorParamsOdom2D)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- wolf::ProcessorParamsOdom2D nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(nb_load.type, nb_.type) << full_path;
- ASSERT_EQ(nb_load.name, nb_.name) << full_path;
- ASSERT_EQ(nb_load.dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
- ASSERT_EQ(nb_load.theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_load.cov_det, nb_.cov_det) << full_path;
- ASSERT_EQ(nb_load.unmeasured_perturbation_std,
- nb_.unmeasured_perturbation_std) << full_path;
-
- /// Testing BasePtr
-
- const std::string ptr_full_path = path_to_io + filename + ptr_ext + ext;
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom2D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( ptr_full_path, nb ) )
- << "Failed on saving " << ptr_full_path;
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( ptr_full_path, nb ) )
- << "Failed on loading " << ptr_full_path;
-
- wolf::ProcessorParamsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr)
- << "Failed on casting " << ptr_full_path;
-
- ASSERT_EQ(nb_cast->type, nb_.type) << full_path;
- ASSERT_EQ(nb_cast->name, nb_.name) << full_path;
- ASSERT_EQ(nb_cast->dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
- ASSERT_EQ(nb_cast->theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_cast->cov_det, nb_.cov_det) << full_path;
- ASSERT_EQ(nb_cast->unmeasured_perturbation_std,
- nb_.unmeasured_perturbation_std) << full_path;
- }
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorParamsOdom2D::"
- "CerealSerializationProcessorParamsOdom2D !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp b/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp
deleted file mode 100644
index 41b2ca2f1b9c9eb6671ed81b4b04b5d97fb847d1..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_processor_odom3d_params.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationProcessorOdom3DParams : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationProcessorOdom3DParams()
- {
- nb_.name = "NAME";
- //nb_.type = "ODOM 3D";
-
- nb_.max_time_span = 1.5;
- nb_.max_buff_length = 55.;
- nb_.dist_traveled = .25;
- nb_.angle_turned = .17;
- }
-
- const std::string path_to_io = "/tmp/";
-
- wolf::ProcessorParamsOdom3D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsXML)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.xml");
-
- wolf::ProcessorParamsOdom3D nb_save;
- nb_save.name = "NAME2";
- //nb_.type = "ODOM 3D";
-
- nb_save.max_time_span = 2.5;
- nb_save.max_buff_length = 52.;
- nb_save.dist_traveled = .24;
- nb_save.angle_turned = .18;
-
- ASSERT_NO_THROW( wolf::save( filename, nb_, nb_save, 10 ) );
-
- {
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- wolf::ProcessorParamsOdom3D nb_load0, nb_load1;
- int myint;
- ASSERT_NO_THROW( wolf::load( filename, nb_load0, nb_load1, myint ) );
-
- ASSERT_EQ(nb_load0.type, nb_.type);
- ASSERT_EQ(nb_load0.name, nb_.name);
- ASSERT_EQ(nb_load0.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load0.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load0.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load0.angle_turned, nb_.angle_turned);
-
- ASSERT_EQ(nb_load1.type, nb_save.type);
- ASSERT_EQ(nb_load1.name, nb_save.name);
- ASSERT_EQ(nb_load1.max_time_span, nb_save.max_time_span);
- ASSERT_EQ(nb_load1.max_buff_length, nb_save.max_buff_length);
- ASSERT_EQ(nb_load1.dist_traveled, nb_save.dist_traveled);
- ASSERT_EQ(nb_load1.angle_turned, nb_save.angle_turned);
-
- ASSERT_EQ(myint, 10);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrXML)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.xml");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsJSON)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.json");
-
- ASSERT_NO_THROW( wolf::save( filename, nb_ ) );
-
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrJSON)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.json");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsBinary)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.bin");
-
- ASSERT_NO_THROW( wolf::save( filename, nb_ ) );
-
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrBinary)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.bin");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_save_load.cpp b/test/serialization/cereal/gtest_serialization_save_load.cpp
deleted file mode 100644
index 23241af4887a43db6d48f027b30481e3408ade49..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_save_load.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/io.h"
-#include "../../../serialization/cereal/serialization_sensor_odom2d_intrinsic.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-namespace wolf {
-
-using IntrinsicsOdom2DPtr = std::shared_ptr<IntrinsicsOdom2D>;
-
-}
-
-class WolfTestCerealSerializationSaveLoad : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationSaveLoad()
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::IntrinsicsOdom2D>().type) nb_type = "TYPE";
- decltype(std::declval<wolf::IntrinsicsOdom2D>().name) nb_name = "NAME";
- decltype(std::declval<wolf::IntrinsicsOdom2D>().k_disp_to_disp) nb_k_disp_to_disp = 0.54;
- decltype(std::declval<wolf::IntrinsicsOdom2D>().k_rot_to_rot) nb_k_rot_to_rot = 0.18;
-};
-
-TEST_F(WolfTestCerealSerializationSaveLoad, CerealSerializationSaveLoadExtension)
-{
- const std::string xml = "/test/filename.xml";
- const std::string bin = "/test/filename.bin";
- const std::string json = "/test/filename.json";
-
- ASSERT_EQ(wolf::serialization::extension(xml), ".xml");
- ASSERT_EQ(wolf::serialization::extension(bin), ".bin");
- ASSERT_EQ(wolf::serialization::extension(json), ".json");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadXML)
-{
- const std::string filename = path_to_io + "save_load_serialization.xml";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadJSON)
-{
- const std::string filename = path_to_io + "save_load_serialization.json";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadBinary)
-{
- const std::string filename = path_to_io + "save_load_serialization.bin";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadNoExt)
-{
- const std::string filename = path_to_io + "save_load_serialization_no_ext";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadNoExt !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadUnknownExt)
-{
- const std::string filename = path_to_io + "save_load_serialization.foo";
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_THROW( wolf::save( filename, nb ), std::runtime_error );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_THROW( wolf::load( filename, nb ), std::runtime_error );
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadUnknownExt !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp b/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp
deleted file mode 100644
index a8a67dc1977d184a91a26cae2420f1a04ef5a0ee..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * gtest_intrinsics_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_sensor_intrinsic_base.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationIntrinsicsBase : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationIntrinsicsBase()
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::IntrinsicsBase>().type) nb_type = "TYPE";
- decltype(std::declval<wolf::IntrinsicsBase>().name) nb_name = "NAME";
-};
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseXML)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBasePtrXML)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseJSON)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBasePtrJSON)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseBinary)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase, CerealSerializationIntrinsicsBasePtrBinary)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp b/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp
deleted file mode 100644
index c5891c3d91d020293937236e582535ddd2e2beaf..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp
+++ /dev/null
@@ -1,256 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_sensor_odom2d_intrinsic.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-namespace wolf {
-
-using IntrinsicsOdom2DPtr = std::shared_ptr<IntrinsicsOdom2D>;
-
-}
-
-class WolfTestCerealSerializationIntrinsicsOdom2D : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationIntrinsicsOdom2D()
- {
- nb_.k_disp_to_disp = 0.54;
- nb_.k_rot_to_rot = 0.18;
- nb_.name = "NAME";
- nb_.type = "TYPE";
- }
-
- const std::string path_to_io = "/tmp/";
-
- wolf::IntrinsicsOdom2D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DXML)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DPtrXML)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DJSON)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DPtrJSON)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DBinary)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D, CerealSerializationIntrinsicsOdom2DPtrBinary)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_time_stamp.cpp b/test/serialization/cereal/gtest_serialization_time_stamp.cpp
deleted file mode 100644
index 8dc4cd479ad8c3986fbd4286899ab1081b1a9af0..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_time_stamp.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_time_stamp.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationTimeStamp : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationTimeStamp()
- {
- nb_.setToNow();
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_time_stamp";
- const std::string ptr_ext = "_ptr";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- wolf::TimeStamp nb_;
-};
-
-TEST_F(WolfTestCerealSerializationTimeStamp,
- CerealSerializationTimeStamp)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- wolf::TimeStamp nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(nb_load, nb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationTimeStamp::"
- "CerealSerializationTimeStamp !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
index 940d3e5854a5ec612f5223251243e643c7bef79b..b8d6dd7b62bf8c457dd6de8ce1813d48487af0cc 100644
--- a/test/yaml/params1.yaml
+++ b/test/yaml/params1.yaml
@@ -1,30 +1,41 @@
config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
sensors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "odom"
+ plugin: "core"
intrinsic:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
extrinsic:
- pos: [1,2,3]
+ pose: [1,2,3]
-
type: "RANGE BEARING"
name: "rb"
+ plugin: "core"
processors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "processor1"
- sensor name: "odom"
+ sensor_name: "odom"
+ plugin: "core"
-
type: "RANGE BEARING"
name: "rb_processor"
- sensor name: "rb"
+ sensor_name: "rb"
+ plugin: "core"
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "my_proc_test"
- sensor name: "odom"
- follow: "/test/yaml/params3.1.yaml"
-files:
- - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
+ sensor_name: "odom"
+ plugin: "core"
+ follow: "test/yaml/params3.1.yaml"
+ -
+ type: "ODOM 3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ follow: "test/yaml/processor_odom_3d.yaml"
diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml
index d58014fbabc36387be1a96cc4244cff954ac2820..1aadb76249e83db650ee7a3c2d55aa0b0bcce2af 100644
--- a/test/yaml/params2.yaml
+++ b/test/yaml/params2.yaml
@@ -1,21 +1,27 @@
config:
+ problem:
+ frame_structure: "POV"
+ dimension: 2
sensors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "odom"
+ plugin: "core"
intrinsic:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
extrinsic:
- pos: [1,2,3]
+ pose: [1,2,3]
-
type: "RANGE BEARING"
name: "rb"
+ plugin: "core"
processors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "processor1"
- sensor name: "odom"
+ sensor_name: "odom"
+ plugin: "core"
$mymap:
k1: v1
k2: v2
@@ -23,11 +29,10 @@ config:
-
type: "RANGE BEARING"
name: "rb_processor"
- sensor name: "rb"
+ sensor_name: "rb"
+ plugin: "core"
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "my_proc_test"
- sensor name: "odom"
-files:
- - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
+ sensor_name: "odom"
+ plugin: "core"
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
index ac82cf0dd818e6c6dec00c61ff86b75c38a3fc11..e23b24ea4d148394dc87a1789514f3e1c7e71e75 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -1,31 +1,21 @@
config:
- sensors:
+ problem:
+ frame_structure: "POV"
+ dimension: 2
+ sensors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "odom"
+ plugin: "core"
intrinsic:
k_disp_to_disp: 0.1
- k_rot_to_rot: 0.1
+ k_rot_to_rot: 0.1
extrinsic:
- pos: [1,2,3]
- -
- type: "RANGE BEARING"
- name: "rb"
+ pose: [1,2,3]
processors:
-
- type: "ODOM 2D"
- name: "processor1"
- sensor name: "odom"
- -
- type: "RANGE BEARING"
- name: "rb_processor"
- sensor name: "rb"
- -
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "my_proc_test"
- sensor name: "odom"
- follow: "/test/yaml/params3.1.yaml"
- jump: "@/test/yaml/params3.1.yaml"
-files:
- - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
+ plugin: "core"
+ sensor_name: "odom"
+ extern_params: "@test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params_basic.yaml b/test/yaml/params_basic.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..730381520149ef3d51a8428e4cf08778d869a4ea
--- /dev/null
+++ b/test/yaml/params_basic.yaml
@@ -0,0 +1,35 @@
+config:
+ problem:
+ dim: 2
+
+ int_1: -3
+ int_2: 0
+ int_3: "6"
+ uint_1: 2
+ uint_2: 0
+ uint_3: "6"
+ double_1: 3.6
+ double_2: -3
+ double_3: "3.6"
+ string_1: wolf
+ string_2: "Wolf"
+ bool_1: true
+ bool_2: True
+ bool_3: TRUE
+ bool_4: "true"
+ bool_5: false
+ bool_6: False
+ bool_7: FALSE
+ bool_8: "false"
+
+ int_wrong_1: 2.3
+ int_wrong_2: "wolf"
+ int_wrong_3: true
+ uint_wrong_1: -2
+ uint_wrong_2: 3.5
+ uint_wrong_3: "wolf"
+ uint_wrong_4: true
+ double_wrong_1: "wolf"
+ double_wrong_2: true
+ bool_wrong: 1
+
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6c5ed47c2efc2afc3ba960b075e60cea73b58a73
--- /dev/null
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -0,0 +1,46 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fa43fecb397dff295b683cfcd6a282adac61cc44
--- /dev/null
+++ b/test/yaml/params_tree_manager1.yaml
@@ -0,0 +1,52 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ mode: "factor"
+ # state: [0,0,0,0,0,0,1,0,0,0]
+ # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerDummy"
+ toy_param: 0
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
+
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f37e31459d9a883aca9eb12898aa5ac285e63210
--- /dev/null
+++ b/test/yaml/params_tree_manager2.yaml
@@ -0,0 +1,51 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ mode: "factor"
+ # state: [0,0,0,0,0,0,1,0,0,0]
+ # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
+
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..277810464d6f619ed342ce3706bec30d7ca8e5f9
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -0,0 +1,50 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_frames: 3
+ n_fix_first_frames: 2
+ viral_remove_empty_parent: true
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
+
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f22fdde12f065d17accb122ef7f8d1728ef6fb6c
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -0,0 +1,50 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_frames: 3
+ n_fix_first_frames: 0
+ viral_remove_empty_parent: false
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
+
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a7f0f7434fb8a71c74e3aa3f15b8dc9f6ea7c067
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -0,0 +1,20 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindowDualRate"
+ n_frames: 5
+ n_frames_recent: 3
+ rate_old_frames: 2
+ n_fix_first_frames: 2
+ viral_remove_empty_parent: true
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cae3df67f036430481cd936ea31a9d2b4c0bca9a
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -0,0 +1,20 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindowDualRate"
+ n_frames: 5
+ n_frames_recent: 3
+ rate_old_frames: 2
+ n_fix_first_frames: 0
+ viral_remove_empty_parent: false
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8f00f6499df2c96c9993bd6c486554579e7fbab9
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -0,0 +1,58 @@
+config:
+ solver:
+ period: 1
+ verbose: 2
+ update_immediately: false
+ max_num_iterations: 10
+ n_threads: 2
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindowDualRate"
+ n_frames: 5
+ n_frames_recent: 3
+ rate_old_frames: 2
+ n_fix_first_frames: 2
+ viral_remove_empty_parent: true
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 10 # meters
+ angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
+
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..114e865bdc3a86b6d0cddf42e0f5360b7b2d5928
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -0,0 +1,51 @@
+config:
+ solver:
+ period: 1
+ verbose: 2
+ update_immediately: false
+ max_num_iterations: 10
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 10 # meters
+ angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/processor_odom_3D.yaml b/test/yaml/processor_odom_3D.yaml
deleted file mode 100644
index f501e333800ec1bbb4b7c751a32aa67a99bec74c..0000000000000000000000000000000000000000
--- a/test/yaml/processor_odom_3D.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.01 # seconds
-keyframe vote:
- max time span: 0.2 # seconds
- max buffer length: 10 # motion deltas
- dist traveled: 0.5 # meters
- angle turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0fe5e5c6d360e239edf207dba1f88329341a0632
--- /dev/null
+++ b/test/yaml/processor_odom_3d.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorOdom3d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+
+time_tolerance: 0.01 # seconds
+
+keyframe_vote:
+ voting_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+unmeasured_perturbation_std: 0.001
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2d26af901956a324988ac86a67a78dacb5ae8a67
--- /dev/null
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -0,0 +1,4 @@
+type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+
+k_disp_to_disp: 0.02 # m^2 / m
+k_rot_to_rot: 0.01 # rad^2 / rad
diff --git a/test/yaml/sensor_odom_3D.yaml b/test/yaml/sensor_odom_3D.yaml
deleted file mode 100644
index 9ea77803548cfd9d033f54e40b6d92a72710afb8..0000000000000000000000000000000000000000
--- a/test/yaml/sensor_odom_3D.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-motion variances:
- disp_to_disp: 0.02 # m^2 / m
- disp_to_rot: 0.02 # rad^2 / m
- rot_to_rot: 0.01 # rad^2 / rad
- min_disp_var: 0.01 # m^2
- min_rot_var: 0.01 # rad^2
diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8eb2b235011cb3cfe4f35b1f73da6344991af0da
--- /dev/null
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -0,0 +1,7 @@
+type: "SensorOdom3d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+
+k_disp_to_disp: 0.02 # m^2 / m
+k_disp_to_rot: 0.02 # rad^2 / m
+k_rot_to_rot: 0.01 # rad^2 / rad
+min_disp_var: 0.01 # m^2
+min_rot_var: 0.01 # rad^2
diff --git a/wolf_scripts/out b/wolf_scripts/out
index 2b7857b6ba924b96ce256a0a7661b7079c429a2a..493b8dbcb5aaec3863d3634eff897e820206f404 100644
--- a/wolf_scripts/out
+++ b/wolf_scripts/out
@@ -2,7 +2,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-===============================================================
@@ -10,13 +10,13 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
#include "capture_image.h"
@@ -24,8 +24,8 @@
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
#include "capture_GPS.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
-#include "capture_odom_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
+#include "capture_odom_3d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
#include "capture_velocity.h"
@@ -39,18 +39,18 @@
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
#include "capture_GPS_fix.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
-#include "capture_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
+#include "capture_odom_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
#include "capture_wheel_joint_position.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
-#include "capture_laser_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
+#include "capture_laser_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-===============================================================
@@ -58,39 +58,39 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-===============================================================
@@ -134,7 +134,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_processor_factory.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-===============================================================
@@ -307,15 +307,15 @@
+====================== /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
#include "local_parametrization_base.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-===============================================================
@@ -323,7 +323,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-===============================================================
@@ -333,11 +333,11 @@
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
#include "feature_GPS_pseudorange.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
-#include "feature_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
+#include "feature_odom_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
-#include "feature_polyline_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
+#include "feature_polyline_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
#include "feature_diff_drive.h"
@@ -348,16 +348,16 @@
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
#include "feature_motion.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
-#include "feature_corner_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
+#include "feature_corner_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
#include "feature_GPS_fix.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
-#include "feature_corner_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
+#include "feature_corner_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-===============================================================
@@ -365,26 +365,26 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
-#include "landmark_corner_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
+#include "landmark_corner_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
-#include "landmark_line_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
+#include "landmark_line_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
#include "landmark_container.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
-#include "landmark_polyline_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
+#include "landmark_polyline_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
-#include "landmark_point_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
+#include "landmark_point_3d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
#include "landmark_AHP.h"
@@ -395,7 +395,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-===============================================================
@@ -415,7 +415,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-===============================================================
@@ -443,11 +443,11 @@
+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
#include "processor_tracker_landmark.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
-#include "processor_odom_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
+#include "processor_odom_3d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
-#include "processor_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
+#include "processor_odom_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
#include "processor_tracker_landmark_image.h"
@@ -473,11 +473,11 @@
+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
#include "processor_tracker_landmark_polyline.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-===============================================================
@@ -487,7 +487,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
#include "sensor_diff_drive.h"
@@ -495,17 +495,17 @@
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
#include "sensor_GPS.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
-#include "sensor_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
+#include "sensor_odom_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
#include "sensor_IMU.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
-#include "sensor_odom_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
+#include "sensor_odom_3d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
-#include "sensor_laser_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
+#include "sensor_laser_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
#include "sensor_GPS_fix.h"
diff --git a/wolf_scripts/output b/wolf_scripts/output
index 565ac41005f746c4bc411e7776ba78adb2c3024a..2652d6fd005dfc9eaad13d4b8bcf7f5e582eb84c 100644
--- a/wolf_scripts/output
+++ b/wolf_scripts/output
@@ -22,18 +22,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
================================================================
@@ -75,18 +75,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
================================================================
@@ -116,19 +116,19 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
#include <wolf/core/capture_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
================================================================
@@ -154,17 +154,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
================================================================
@@ -216,17 +216,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
================================================================
@@ -242,17 +242,17 @@
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
#include <wolf/sensor/sensor_diff_drive.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
================================================================
@@ -270,19 +270,19 @@
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
#include <wolf/sensor/sensor_IMU.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
================================================================
@@ -329,17 +329,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
================================================================
@@ -354,20 +354,20 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
#include <wolf/core/wolf.h>
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
#include <wolf/capture/capture_IMU.h>
@@ -386,34 +386,34 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
#include <wolf/core/constraint_analytic.h>
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
================================================================
@@ -431,34 +431,34 @@
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
#include <wolf/feature/feature_GPS_pseudorange.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
#include <wolf/sensor/sensor_GPS.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
================================================================
@@ -476,47 +476,47 @@
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
#include <wolf/feature/feature_GPS_pseudorange.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
#include <wolf/sensor/sensor_GPS.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
#include <wolf/capture/capture_wheel_joint_position.h>
@@ -534,50 +534,50 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
+#include <wolf/landmark/landmark_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
+#include <wolf/landmark/landmark_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
================================================================
@@ -630,8 +630,8 @@
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-#include <wolf/constraint/constraint_pose_2D.h>
-#include <wolf/constraint/constraint_pose_3D.h>
+#include <wolf/constraint/constraint_pose_2d.h>
+#include <wolf/constraint/constraint_pose_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
================================================================
@@ -798,17 +798,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
================================================================
@@ -1073,12 +1073,12 @@
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
- * #include <wolf/processor/processor_odom_2D.h> // provides ProcessorOdom2D and ProcessorFactory
- * #include <wolf/processor/processor_odom_2D.h>
+ * #include <wolf/processor/processor_odom_2d.h> // provides ProcessorOdom2d and ProcessorFactory
+ * #include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
- * #include <wolf/sensor/sensor_odom_2D.h> // provides SensorOdom2D and SensorFactory
- * #include <wolf/sensor/sensor_odom_2D.h>
+ * #include <wolf/sensor/sensor_odom_2d.h> // provides SensorOdom2d and SensorFactory
+ * #include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
================================================================
@@ -1471,7 +1471,7 @@
#include <wolf/capture/capture_IMU.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
#include <wolf/core/wolf.h>
@@ -1485,17 +1485,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
#include <wolf/capture/capture_IMU.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
#include <wolf/core/capture_pose.h>
@@ -1618,7 +1618,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
================================================================
@@ -1842,12 +1842,12 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_polyline.h>
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2D.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
================================================================
@@ -1891,20 +1891,20 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
-#include <wolf/capture/capture_laser_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
+#include <wolf/capture/capture_laser_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
================================================================
@@ -1933,7 +1933,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+#include <wolf/landmark/landmark_polyline_2d.h>
#include <wolf/landmark/landmark_AHP.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
@@ -2047,7 +2047,7 @@
#include <wolf/core/constraint_base.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-#include <wolf/feature/feature_odom_2D.h>
+#include <wolf/feature/feature_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
================================================================
@@ -2070,12 +2070,12 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_corner.h>
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2D.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
================================================================
@@ -2120,7 +2120,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
================================================================
@@ -2152,13 +2152,13 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_2D.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_2d.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
#include <wolf/sensor/sensor_camera.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
================================================================
@@ -2178,7 +2178,7 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/constraint/constraint_fix_bias.h>
-#include <wolf/constraint/constraint_pose_3D.h>
+#include <wolf/constraint/constraint_pose_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/core/wolf.h>
@@ -2190,11 +2190,11 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
================================================================
@@ -2211,12 +2211,12 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_corner.h>
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2D.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
================================================================
@@ -2249,10 +2249,10 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_image.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
#include <wolf/sensor/sensor_camera.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
@@ -2269,33 +2269,33 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
================================================================
@@ -2431,11 +2431,11 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
#include <wolf/processor/processor_tracker_feature_dummy.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
================================================================
@@ -2459,7 +2459,7 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/constraint/constraint_fix_bias.h>
-#include <wolf/constraint/constraint_pose_3D.h>
+#include <wolf/constraint/constraint_pose_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/core/wolf.h>
@@ -2471,11 +2471,11 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
================================================================
@@ -2506,40 +2506,40 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
#include <wolf/capture/capture_IMU.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
#include <wolf/core/problem.h>
#include <wolf/core/map_base.h>
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
-#include <wolf/processor/processor_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
+#include <wolf/processor/processor_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
-#include <wolf/sensor/sensor_odom_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
================================================================
@@ -2560,7 +2560,7 @@
#include <wolf/capture/capture_IMU.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
#include <wolf/core/wolf.h>
@@ -2574,11 +2574,11 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
#include <wolf/capture/capture_image.h>
@@ -2600,19 +2600,19 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
#include <wolf/capture/capture_IMU.h>
@@ -2715,33 +2715,33 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
-#include <wolf/constraint/constraint_odom_2D.h>
-#include <wolf/constraint/constraint_odom_2D_analytic.h>
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
+#include <wolf/constraint/constraint_odom_2d.h>
+#include <wolf/constraint/constraint_odom_2d_analytic.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
#include <wolf/capture/capture_motion.h>
@@ -2772,18 +2772,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
================================================================
@@ -2814,7 +2814,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-#include <wolf/constraint/constraint_GPS_2D.h>
+#include <wolf/constraint/constraint_GPS_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
#include <wolf/core/feature_base.h>
@@ -2825,18 +2825,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
================================================================
@@ -2875,29 +2875,29 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
================================================================
@@ -2935,17 +2935,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
================================================================
@@ -2959,30 +2959,30 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
================================================================
@@ -3024,69 +3024,69 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
#include <wolf/core/landmark_base.h>
#include <wolf/core/wolf.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
================================================================
@@ -3101,41 +3101,41 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
-#include <wolf/constraint/constraint_point_2D.h>
-#include <wolf/constraint/constraint_point_to_line_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
+#include <wolf/constraint/constraint_point_2d.h>
+#include <wolf/constraint/constraint_point_to_line_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-#include <wolf/core/state_homogeneous_3D.h>
+#include <wolf/core/state_homogeneous_3d.h>
#include <wolf/core/factory.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
@@ -3187,58 +3187,58 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
-#include <wolf/capture/capture_odom_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
+#include <wolf/capture/capture_odom_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
-#include <wolf/constraint/constraint_odom_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
+#include <wolf/constraint/constraint_odom_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
#include <wolf/core/processor_motion.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/capture/capture_laser_2D.h>
+#include <wolf/capture/capture_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/constraint/constraint_corner_2D.h>
+#include <wolf/constraint/constraint_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
#include <wolf/core/state_block.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/feature/feature_corner_2D.h>
+#include <wolf/feature/feature_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/capture/capture_laser_2D.h>
+#include <wolf/capture/capture_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/constraint/constraint_corner_2D.h>
+#include <wolf/constraint/constraint_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/processor_tracker_feature.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/feature/feature_corner_2D.h>
+#include <wolf/feature/feature_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
================================================================
@@ -3285,23 +3285,23 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/capture/capture_laser_2D.h>
+#include <wolf/capture/capture_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/constraint/constraint_point_2D.h>
-#include <wolf/constraint/constraint_point_to_line_2D.h>
+#include <wolf/constraint/constraint_point_2d.h>
+#include <wolf/constraint/constraint_point_to_line_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
#include <wolf/core/state_block.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+#include <wolf/landmark/landmark_polyline_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
================================================================
@@ -3349,22 +3349,22 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
-#include <wolf/capture/capture_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
+#include <wolf/capture/capture_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
#include <wolf/core/processor_motion.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
-#include <wolf/sensor/sensor_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
#include <wolf/capture/capture_IMU.h>
@@ -3439,7 +3439,7 @@
#include <wolf/capture/capture_GPS.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-#include <wolf/constraint/constraint_GPS_pseudorange_2D.h>
+#include <wolf/constraint/constraint_GPS_pseudorange_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
#include <wolf/core/processor_factory.h>
@@ -3502,31 +3502,31 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
#include <wolf/core/processor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
#include <wolf/core/processor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
#include <wolf/capture/capture_image.h>
@@ -3555,14 +3555,14 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-#include <wolf/constraint/constraint_corner_2D.h>
+#include <wolf/constraint/constraint_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
@@ -3615,7 +3615,7 @@
#include <wolf/capture/capture_velocity.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include <wolf/constraint/constraint_odom_2D.h>
+#include <wolf/constraint/constraint_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
#include <wolf/core/rotations.h>
@@ -3636,7 +3636,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-#include <wolf/feature/feature_corner_2D.h>
+#include <wolf/feature/feature_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
================================================================
@@ -3655,18 +3655,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
#include <wolf/core/sensor_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
================================================================
@@ -3681,18 +3681,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
#include <wolf/core/sensor_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
================================================================
@@ -3748,18 +3748,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
#include <wolf/core/sensor_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
#include <wolf/capture/capture_motion.h>
@@ -3791,20 +3791,20 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/state_angle.h>
#include <wolf/core/sensor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
================================================================
@@ -3821,34 +3821,34 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/state_quaternion.h>
#include <wolf/core/sensor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/sensor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
================================================================
diff --git a/wolf_scripts/rename.sh b/wolf_scripts/rename.sh
new file mode 100755
index 0000000000000000000000000000000000000000..7d778f475f19523e8cd868c72f0349e4f78dcf30
--- /dev/null
+++ b/wolf_scripts/rename.sh
@@ -0,0 +1,62 @@
+#!/bin/bash
+file2type()
+{
+ type=$(echo $1 | sed -r "s/([^_]+)/\U\1\\ /g" | sed -r "s/_([^_]+)/\U\1/g" | sed -r "s/\\ $//" )
+}
+camel2snake ()
+{
+ camel=$1
+ new_snake=$(echo $camel | sed -r "s/([A-Z][^A-Z]+)/\L\1_/g" | sed -r "s/_$//")
+}
+snake2camel ()
+{
+ snake=$1
+ new_camel=$(echo $snake | sed -r "s/([^_]+)/\u\1/g" | sed -r "s/_([^_]+)/\u\1/g")
+}
+
+# type=$(echo $1 | sed -r "s/([^_]+)/\U\1\\ /g" | sed -r "s/_([^_]+)/\U\1/g" )
+# file2type $1
+# echo $type
+# exit 1
+getTypes ()
+{
+ for file in $(find include/ src/ test/ -type f); do
+ name=$(echo $file | rev | cut -d '/' -f1 | rev | cut -d '.' -f1)
+ extension=$(echo $name | cut -d '_' -f2- )
+ # echo $extension
+ snake2camel $extension
+ camel_extension=$new_camel
+ snake2camel $name
+ camel_file=$new_camel
+ file2type $extension
+ # echo $file " %%% " $type " %%% " $camel_file
+ # echo "VVV "$type
+ # sed -rn "s/(Feature|Capture|Landmark|Processor|Factor)(.*)\"${type}\"/\1\2\"${camel_file}\"/p" $file
+ # echo sed -rn "s/(Capture)(.*)\"${type}\"/\1\2\"${camel_file}\"/p" $file
+ # type="ODOM 3d"
+ matching=$(ag -l "\"${type}\"" --ignore "demos" . | wc -l)
+ if [ $matching -gt "0" ]; then
+ echo ${type}
+ fi
+ # sed -ri "s/\"${type}\"/\"${camel_file}\"/" $file
+ done
+}
+IFS=$'\n'
+types=$(getTypes | sort | uniq)
+# echo $types
+# for ctype in $types; do
+# ctype=$(echo $ctype | sed -r "s/([^\ 0-9])([^\ 0-9]+)\ ?/\1\L\2/g")
+# echo $ctype
+# done
+for target in $(find include/ src/ test/ -type f); do
+ for ctype in $types; do
+ filename=$(echo $target | rev | cut -d '/' -f1 | cut -d '.' -f2 | rev)
+ snake2camel $filename
+ oldtype=$ctype
+ ctype=$(echo $ctype | sed -r "s/([^\ 0-9])([^\ 0-9]+)\ ?/\1\L\2/g")
+ # echo $target $oldtype $ctype
+ # sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+ sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+ sed -ri "s%\"${oldtype}\"%\"${new_camel}\"%" $target
+ done
+done
diff --git a/wolf_scripts/sed_rename.sh b/wolf_scripts/sed_rename.sh
new file mode 100755
index 0000000000000000000000000000000000000000..820d02cddc66cfdea78eb6f3a7bc979b095c4534
--- /dev/null
+++ b/wolf_scripts/sed_rename.sh
@@ -0,0 +1,28 @@
+#!/bin/bash
+#$1 flag telling whether we are in test mode or not
+
+modify=$1
+apply=false
+
+if [[ "$modify" = true || "$modify" = t ]]; then
+ read -p "Are you sure? " -n 1 -r
+ echo # (optional) move to a new line
+ if [[ $REPLY =~ ^[Yy]$ ]]
+ then
+ apply=true
+ echo "Modifying..."
+ fi
+fi
+
+string="loadYAML"
+replace="loadYaml"
+
+for file in $(find include/ src/ test/ demos/ -type f); do
+ if [ "$apply" = true ]; then
+ # echo "APPLYING"
+ sed -Ei 's%'$string'%'$replace'%g' $file
+ else
+ # echo "NOT APPLYING"
+ sed -En 's%'$string'%'$replace'%gp' $file
+ fi
+done
diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp
index c797c1b5ad21d8c09ce89d3cb116cb6da299bacb..2acdaa25f334608ea1e9b9ce2cdec7e4642f6b84 100644
--- a/wolf_scripts/templates/gtest_template.cpp
+++ b/wolf_scripts/templates/gtest_template.cpp
@@ -1,7 +1,7 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "wolf.h"
-#include "logging.h"
+//#include "logging.h"
#include "header_file"
diff --git a/wolf_scripts/wolf_uninstall.sh b/wolf_scripts/wolf_uninstall.sh
new file mode 100755
index 0000000000000000000000000000000000000000..ff022e0b25ebc31bc0d7d0e50124569896c8f6ba
--- /dev/null
+++ b/wolf_scripts/wolf_uninstall.sh
@@ -0,0 +1,42 @@
+#!/bin/bash
+
+if [ -z "$1" ]
+then
+ echo “=â€
+ echo “====================== UNINSTALLING WOLF ======================â€
+ echo “=â€
+
+ echo "cd /usr/local/include/iri-algorithms/"
+ cd /usr/local/include/iri-algorithms/
+ echo "sudo rm -rf wolf"
+ sudo rm -rf wolf
+
+ echo "cd /usr/local/lib/iri-algorithms/"
+ echo "sudo rm libwolf*.*"
+ cd /usr/local/lib/iri-algorithms/
+ sudo rm libwolf*.*
+
+ echo "cd /usr/local/lib/cmake/"
+ echo "sudo rm -rf wolf*"
+ cd /usr/local/lib/cmake/
+ sudo rm -rf wolf*
+else
+ echo “=â€
+ echo “====================== UNINSTALLING PLUGIN $1 ======================â€
+ echo “=â€
+
+ echo "cd /usr/local/include/iri-algorithms/wolf"
+ cd /usr/local/include/iri-algorithms/wolf/
+ echo "sudo rm -rf plugin_$1"
+ sudo rm -rf plugin_$1
+
+ echo "cd /usr/local/lib/iri-algorithms/"
+ echo "sudo rm libwolf$1.*"
+ cd /usr/local/lib/iri-algorithms/
+ sudo rm libwolf$1.*
+
+ echo "cd /usr/local/lib/cmake/"
+ echo "sudo rm -rf wolf$1"
+ cd /usr/local/lib/cmake/
+ sudo rm -rf wolf$1
+fi
diff --git a/wolf_scripts/wolf_update.sh b/wolf_scripts/wolf_update.sh
new file mode 100755
index 0000000000000000000000000000000000000000..7a0ceac5be717ff17fbe5acab186fda636de7ec4
--- /dev/null
+++ b/wolf_scripts/wolf_update.sh
@@ -0,0 +1,111 @@
+#!/bin/bash
+. ./generic_func/functions.sh
+# First parameter $1 is the path to the folder with the plugins
+declare -A success
+cd $1
+abs_path=$(pwd)
+# for folder in $(cd $abs_path && ls -d *); do
+for folder in wolf IMU laser gnss vision apriltag; do
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${CYAN} Updating & installing $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ cd $abs_path
+ echo $(pwd)
+ cd $folder
+ is_repo=$(git rev-parse --is-inside-work-tree 2>/dev/null)
+ success[$folder]=false
+ if [ $is_repo ]; then
+ current_branch=$(git status | head -n 1 | cut -d' ' -f 3)
+ # if git checkout devel; then
+ if git pull; then
+ if [ ! -d build ];
+ then
+ mkdir build
+ fi
+ cd build
+ if ! cmake ..; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${YELLOW} CMake has failed, avorting process for $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ continue
+ fi
+ if ! make -j"$(nproc)"; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${RED} Make has failed, avorting process for $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ continue
+ fi
+ if ! ctest -j"$(nproc)"; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${RED} Tests have not succeeded, avorting installation ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ continue
+ fi
+ if ! sudo make install; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${RED} Installation for $folder failed ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ continue
+ fi
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${GREEN} Everything went smoothly on $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ success[$folder]=true
+ else
+ # echo "No devel branch to checkout, skipping $folder"
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${ORANGE} Something went wrong when pulling... skipping $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ fi
+ else
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${CYAN} Skipping $folder... Not a git repo ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ fi
+ # echo ""
+ # echo "==========================================================================================================================="
+ # echo "${YELLOW} Finished with $folder ${NC}"
+ # echo "==========================================================================================================================="
+ # echo ""
+done
+echo ""
+echo "==========================================================================================================================="
+echo "${CYAN} SUMMARY ${NC}"
+echo "==========================================================================================================================="
+echo ""
+for fl in "${!success[@]}";
+do
+ if "${success[$fl]}"; then
+ echo "${GREEN} Plugin ${YELLOW}$fl${GREEN} compiles, passess test and installs correctly ${NC}"
+ else
+ echo "${RED} Something went wrong with plugin ${YELLOW}$fl${RED}. Either it is not a plugin, it doesn't compile, it fails some test or there has been some error installing. ${NC}"
+ fi
+done
\ No newline at end of file