diff --git a/.gitignore b/.gitignore
index d683ba3c89247c8f1af43ea1cf25a7975f220dd8..8fd4190ee6d52f3aac6f090728b06fe6a862513b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -29,3 +29,15 @@ src/CMakeCache.txt
src/CMakeFiles/cmake.check_cache
src/examples/map_apriltag_save.yaml
+
+\.vscode/
+build_release/
+
+laser.found
+.clangd/
+Testing/
+
+.ccls
+.ccls-root
+.ccls-cache
+compile_commands.json
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 3876350c19bdaadb50a554973053d816ff61e2ad..6a09f31ceaa2365cb06d9058c2962f364b890a91 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,70 +1,440 @@
-image: segaleran/ceres
+stages:
+ - license
+ - none
+ - csm
+ - falko
+ - csm_falko
-before_script:
- - ls
+############ YAML ANCHORS ############
+.preliminaries_template: &preliminaries_definition
+ ## Install ssh-agent if not already installed, it is required by Docker.
+ ## (change apt-get to yum if you use an RPM-based image)
+ - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
+
+ ## Run ssh-agent (inside the build environment)
+ - eval $(ssh-agent -s)
+
+ ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
+ ## We're using tr to fix line endings which makes ed25519 keys work
+ ## without extra base64 encoding.
+ ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
+ - mkdir -p ~/.ssh
+ - chmod 700 ~/.ssh
+ - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
+ # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
+ - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
+
+ # update apt
- 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
+
+ # create 'ci_deps' folder (if not exists)
+ - mkdir -pv ci_deps
+
+.install_wolf_template: &install_wolf_definition
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - if [ -d wolf ]; then
+ - echo "directory wolf exists"
+ - cd wolf
+ - git checkout devel
+ - git pull
+ - git checkout $WOLF_CORE_BRANCH
- else
- - echo "directory inexistent"
- - git clone https://github.com/gabime/spdlog.git
- - cd spdlog
+ - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+ - cd wolf
+ - git checkout $WOLF_CORE_BRANCH
- fi
- - git fetch
- - git checkout v0.17.0
- 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=OFF -DBUILD_TESTS=OFF ..
+ - make -j$(nproc)
- 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
+ - ldconfig
+
+.license_header_template: &license_header_definition
+ - cd $CI_PROJECT_DIR
+
+ # configure git
+ - export CI_NEW_BRANCH=ci_processing$RANDOM
+ - echo creating new temporary branch... $CI_NEW_BRANCH
+ - git config --global user.email "${CI_EMAIL}"
+ - git config --global user.name "${CI_USERNAME}"
+ - git checkout -b $CI_NEW_BRANCH # temporary branch
+
+ # license headers
+ - export CURRENT_YEAR=$( date +'%Y' )
+ - echo "current year:" ${CURRENT_YEAR}
+ - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
+ # add license headers to new files
+ - echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
+ - ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
- else
- - echo "directory inexistent"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
+ # update license headers of all files
+ - export PREV_YEAR=$(( CURRENT_YEAR-1 ))
+ - echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
+ - git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
+ - sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
+ - ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+ - fi
+
+ # push changes (if any)
+ - if git commit -a -m "[skip ci] license headers added or modified" ; then
+ - git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+ - git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
+ - else
+ - echo "No changes, nothing to commit!"
+ - fi
+
+.install_laserscanutils_template: &install_laserscanutils_definition
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - if [ -d laser_scan_utils ]; then
+ - echo "directory laser_scan_utils exists"
+ - cd laser_scan_utils
+ - git pull
+ - else
+ - git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/laser_scan_utils.git
+ - cd laser_scan_utils
- fi
- mkdir -pv build
- cd build
- - ls
- - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
+ - rm -rf *
+ - cmake -DCMAKE_BUILD_TYPE=release ..
+ - make -j$(nproc)
- make install
- - cd ../..
+ - ldconfig
-wolf_build_and_test:
- stage: build
+.install_csm_template: &install_csm_definition
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - apt-get install -y libgsl-dev
+ - if [ -d csm ]; then
+ - echo "directory csm exists"
+ - cd csm
+ - git pull
+ - else
+ - git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/csm.git
+ - cd csm
+ - fi
+ - cmake .
+ - make -j$(nproc)
+ - make install
+ - ldconfig
+
+.install_falko_template: &install_falko_definition
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - if [ -d falkolib ]; then
+ - echo "directory falkolib exists"
+ - cd falkolib
+ - git pull
+ - else
+ - git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/falkolib.git
+ - cd falkolib
+ - fi
+ - mkdir -pv build
+ - cd build
+ - cmake ..
+ - make -j$(nproc)
+ - make install
+ - ldconfig
+
+.build_and_test_template: &build_and_test_definition
+ - cd ${CI_PROJECT_DIR}
+ - mkdir -pv build
+ - cd build
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=ON ..
+ - make -j$(nproc)
+ - ctest -j$(nproc)
+ - make install
+
+############ LICENSE HEADERS ############
+license_headers:
+ stage: license
+ image: labrobotica/wolf_deps:16.04
+ cache:
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ script:
+ - *license_header_definition
+
+############ UBUNTU 16.04 TESTS ############
+build_and_test_none:xenial:
+ stage: none
+ image: labrobotica/wolf_deps:16.04
+ cache:
+ - key: laserscanutils-xenial
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_csm:xenial:
+ stage: csm
+ image: labrobotica/wolf_deps:16.04
+ cache:
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-xenial
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: csm-xenial
+ paths:
+ - ci_deps/csm/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_csm_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_falko:xenial:
+ stage: falko
+ image: labrobotica/wolf_deps:16.04
+ cache:
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-xenial
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: falko-xenial
+ paths:
+ - ci_deps/falkolib/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_falko_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_csm_falko:xenial:
+ stage: csm_falko
+ image: labrobotica/wolf_deps:16.04
+ cache:
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-xenial
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: csm-xenial
+ paths:
+ - ci_deps/csm/
+ - key: falko-xenial
+ paths:
+ - ci_deps/falkolib/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_falko_definition
+ - *install_csm_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+############ UBUNTU 18.04 TESTS ############
+build_and_test_none:bionic:
+ stage: none
+ image: labrobotica/wolf_deps:18.04
+ cache:
+ - key: wolf-bionic
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-bionic
+ paths:
+ - ci_deps/laser_scan_utils/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_csm:bionic:
+ stage: csm
+ image: labrobotica/wolf_deps:18.04
+ cache:
+ - key: wolf-bionic
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-bionic
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: csm-bionic
+ paths:
+ - ci_deps/csm/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_csm_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_falko:bionic:
+ stage: falko
+ image: labrobotica/wolf_deps:18.04
+ cache:
+ - key: wolf-bionic
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-bionic
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: falko-bionic
+ paths:
+ - ci_deps/falkolib/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_falko_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_csm_falko:bionic:
+ stage: csm_falko
+ image: labrobotica/wolf_deps:18.04
+ cache:
+ - key: wolf-bionic
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-bionic
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: csm-bionic
+ paths:
+ - ci_deps/csm/
+ - key: falko-bionic
+ paths:
+ - ci_deps/falkolib/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_falko_definition
+ - *install_csm_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+############ UBUNTU 20.04 TESTS ############
+build_and_test_none:focal:
+ stage: none
+ image: labrobotica/wolf_deps:20.04
+ cache:
+ - key: wolf-focal
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-focal
+ paths:
+ - ci_deps/laser_scan_utils/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_csm:focal:
+ stage: csm
+ image: labrobotica/wolf_deps:20.04
+ cache:
+ - key: wolf-focal
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-focal
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: csm-focal
+ paths:
+ - ci_deps/csm/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_csm_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_falko:focal:
+ stage: falko
+ image: labrobotica/wolf_deps:20.04
+ cache:
+ - key: wolf-focal
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-focal
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: falko-focal
+ paths:
+ - ci_deps/falkolib/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_falko_definition
+ - *install_laserscanutils_definition
+ script:
+ - *build_and_test_definition
+
+build_and_test_csm_falko:focal:
+ stage: csm_falko
+ image: labrobotica/wolf_deps:20.04
+ cache:
+ - key: wolf-focal
+ paths:
+ - ci_deps/wolf/
+ - key: laserscanutils-focal
+ paths:
+ - ci_deps/laser_scan_utils/
+ - key: csm-focal
+ paths:
+ - ci_deps/csm/
+ - key: falko-focal
+ paths:
+ - ci_deps/falkolib/
except:
- master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_falko_definition
+ - *install_csm_definition
+ - *install_laserscanutils_definition
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
- - ctest
+ - *build_and_test_definition
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6623a8cedf9cbf687712a197ebded6f619d2b3b8..88c9fea397911a5317573b7376ac4e3c9cdeec03 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,17 +1,18 @@
# 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 CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
-
# The project name
PROJECT(laser)
+set(PLUGIN_NAME wolf${PROJECT_NAME})
+MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
@@ -26,29 +27,36 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
-#Set compiler according C++11 support
+#Set compiler according C++14 support
include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+if(COMPILER_SUPPORTS_CXX14)
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
else()
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
if(UNIX)
# GCC is not strict enough by default, so enable most of the warnings.
- set(CMAKE_CXX_FLAGS
- "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
+ set(CMAKE_CXX_FLAGS
+ "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
-#OPTION(BUILD_DOC "Build Documentation" OFF)
-OPTION(BUILD_TESTS "Build Unit tests" ON)
+IF(NOT BUILD_TESTS)
+ OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(NOT BUILD_DEMOS)
+ OPTION(BUILD_DEMOS "Build Demos" OFF)
+ENDIF(NOT BUILD_DEMOS)
+
+IF(NOT BUILD_DOC)
+ OPTION(BUILD_DOC "Build Documentation" ON)
+ENDIF(NOT BUILD_DOC)
+
+
#############
## Testing ##
#############
@@ -61,71 +69,29 @@ if(BUILD_TESTS)
enable_testing()
endif()
-MESSAGE("Starting ${PROJECT_NAME} 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)
-
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-
-#find dependencies.
-# ============EXAMPLE==================
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
-
-FIND_PACKAGE(Threads REQUIRED)
-
-FIND_PACKAGE(wolf REQUIRED)
-
+# ============ DEPENDENCIES ============
+FIND_PACKAGE(wolfcore REQUIRED)
FIND_PACKAGE(laser_scan_utils REQUIRED)
+FIND_PACKAGE(csm QUIET)
+FIND_PACKAGE(falkolib QUIET)
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-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)
-
-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)
+# ============ CONFIG.H ============
+string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
+set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
# Define the directory where will be the configured config.h
-SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/internal)
+SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
# Create the specified output directory if it does not exist.
IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
@@ -141,215 +107,195 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
-#INCLUDES SECTION
-# ============EXAMPLE==================
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIRS})
-include_directories(BEFORE "include")
-include_directories(.)
+# ============ INCLUDES ============
+INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-
+INCLUDE_DIRECTORIES(BEFORE "include")
-#HEADERS
-SET(HDRS_COMMON
- )
-SET(HDRS_MATH
- )
-SET(HDRS_UTILS
- )
-SET(HDRS_STATE_BLOCK
- include/laser/state_block/local_parametrization_polyline_extreme.h
- )
+# ============ HEADERS ============
SET(HDRS_CAPTURE
- include/laser/capture/capture_laser_2D.h
+ include/laser/capture/capture_laser_2d.h
)
SET(HDRS_FACTOR
- include/laser/factor/factor_point_2D.h
- include/laser/factor/factor_point_to_line_2D.h
+ include/laser/factor/factor_point_2d.h
+ include/laser/factor/factor_point_to_line_2d.h
)
SET(HDRS_FEATURE
- include/laser/feature/feature_polyline_2D.h
- include/laser/feature/feature_match_polyline_2D.h
+ include/laser/feature/feature_polyline_2d.h
+ include/laser/feature/feature_match_polyline_2d.h
)
SET(HDRS_LANDMARK
- include/laser/landmark/landmark_polyline_2D.h
- include/laser/landmark/landmark_match_polyline_2D.h
+ include/laser/landmark/landmark_polyline_2d.h
+ include/laser/landmark/landmark_match_polyline_2d.h
+ )
+SET(HDRS_MATH
+ include/laser/math/laser_tools.h
)
SET(HDRS_PROCESSOR
- include/laser/processor/polyline_2D_utils.h
- include/laser/processor/processor_polyline.h
- include/laser/processor/processor_tracker_feature_polyline_2D.h
+ include/laser/processor/polyline_2d_utils.h
+ include/laser/processor/processor_tracker_feature_polyline_2d.h
)
SET(HDRS_SENSOR
- include/laser/sensor/sensor_laser_2D.h
- )
-SET(HDRS_SOLVER
+ include/laser/sensor/sensor_laser_2d.h
)
-SET(HDRS_DTASSC
+SET(HDRS_STATE_BLOCK
+ include/laser/state_block/local_parametrization_polyline_extreme.h
)
-#SOURCES
-SET(SRCS_COMMON
- )
-SET(SRCS_MATH
- )
-SET(SRCS_UTILS
- )
-SET(SRCS_STATE_BLOCK
- src/state_block/local_parametrization_polyline_extreme.cpp
- )
+# ============ SOURCES ============
SET(SRCS_CAPTURE
- src/capture/capture_laser_2D.cpp
- )
-SET(SRCS_FACTOR
+ src/capture/capture_laser_2d.cpp
)
SET(SRCS_FEATURE
- src/feature/feature_polyline_2D.cpp
+ src/feature/feature_polyline_2d.cpp
+ src/feature/feature_match_polyline_2d.cpp
)
SET(SRCS_LANDMARK
- src/landmark/landmark_polyline_2D.cpp
- src/landmark/landmark_match_polyline_2D.cpp
+ src/landmark/landmark_polyline_2d.cpp
+ src/landmark/landmark_match_polyline_2d.cpp
)
SET(SRCS_PROCESSOR
- src/processor/polyline_2D_utils.cpp
- src/processor/processor_polyline.cpp
- src/processor/processor_tracker_feature_polyline_2D.cpp
- src/processor/processor_polyline.cpp
+ src/processor/polyline_2d_utils.cpp
+ src/processor/processor_tracker_feature_polyline_2d.cpp
)
SET(SRCS_SENSOR
- src/sensor/sensor_laser_2D.cpp
- )
-SET(SRCS_DTASSC
+ src/sensor/sensor_laser_2d.cpp
)
-SET(SRCS_SOLVER
+SET(SRCS_STATE_BLOCK
+ src/state_block/local_parametrization_polyline_extreme.cpp
)
SET(SRCS_YAML
- src/yaml/sensor_laser_2D_yaml.cpp
+ src/yaml/sensor_laser_2d_yaml.cpp
)
-#OPTIONALS
-#optional HDRS and SRCS
-# ==================EXAMPLE===============
-# IF (Ceres_FOUND)
-# SET(HDRS_WRAPPER
-# include/base/solver_suitesparse/sparse_utils.h
-# include/base/solver/solver_manager.h
-# include/base/ceres_wrapper/ceres_manager.h
-# include/base/ceres_wrapper/cost_function_wrapper.h
-# include/base/ceres_wrapper/create_numeric_diff_cost_function.h
-# include/base/ceres_wrapper/local_parametrization_wrapper.h
-# )
-# SET(SRCS_WRAPPER
-# src/solver/solver_manager.cpp
-# src/ceres_wrapper/ceres_manager.cpp
-# src/ceres_wrapper/local_parametrization_wrapper.cpp
-# )
-# ELSE(Ceres_FOUND)
-# SET(HDRS_WRAPPER)
-# SET(SRCS_WRAPPER)
-# ENDIF(Ceres_FOUND)
+
+# ============ OPTIONAL ============
+# falko
+if (falkolib_FOUND)
+ message("Found Falkolib. Compiling some extra classes.")
+ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ include/laser/processor/processor_loop_closure_falko.h
+ )
+ # falko & CSM
+ if (csm_FOUND)
+ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ include/laser/processor/processor_loop_closure_falko_icp.h
+ )
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_loop_closure_falko_icp.cpp
+ )
+
+ endif()
+ SET(HDRS_FEATURE ${HDRS_FEATURE}
+ include/laser/feature/feature_scene_falko.h
+ )
+ SET(SRCS_FEATURE ${SRCS_FEATURE}
+ src/feature/feature_scene_falko.cpp
+ )
+
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_loop_closure_falko.cpp
+ )
+endif()
+
+# CSM
+if(csm_FOUND)
+ message("Found CSM. Compiling some extra classes.")
+ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ include/laser/processor/params_icp.h
+ include/laser/processor/processor_loop_closure_icp.h
+ include/laser/processor/processor_odom_icp.h
+ )
+ SET(HDRS_FEATURE ${HDRS_FEATURE}
+ include/laser/feature/feature_icp_align.h
+ )
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_loop_closure_icp.cpp
+ src/processor/processor_odom_icp.cpp
+ )
+ SET(SRCS_FEATURE ${SRCS_FEATURE}
+ src/feature/feature_icp_align.cpp
+ )
+ SET(SRCS_YAML ${SRCS_YAML}
+ src/yaml/processor_odom_icp_yaml.cpp
+ )
+endif(csm_FOUND)
# create the shared library
-ADD_LIBRARY(${PROJECT_NAME}
+ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS_CAPTURE}
- ${SRCS_COMMON}
- ${SRCS_DTASSC}
- ${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_LANDMARK}
- ${SRCS_MATH}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
- ${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
- ${SRCS_UTILS}
- ${SRCS_WRAPPER}
${SRCS_YAML}
)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+# 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()
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
#Link the created libraries
#===============EXAMPLE=========================
-# IF (Ceres_FOUND)
-# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-# ENDIF(Ceres_FOUND)
-
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${laser_scan_utils_LIBRARY})
+IF (falkolib_FOUND)
+ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES} ${laser_scan_utils_LIBRARY} ${falkolib_LIBRARY})
+ENDIF(falkolib_FOUND)
#Build tests
#===============EXAMPLE=========================
-IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
-IF (GLOG_FOUND)
- IF(BUILD_TESTS)
- MESSAGE("Building tests.")
- add_subdirectory(test)
- ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
+IF(BUILD_TESTS)
+ MESSAGE("Building tests.")
+ add_subdirectory(test)
+ENDIF(BUILD_TESTS)
#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)
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
-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_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/serialization)
-INSTALL(FILES ${HDRS_YAML}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
+INSTALL(FILES ${HDRS_MATH}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
+INSTALL(FILES ${HDRS_PROCESSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+INSTALL(FILES ${HDRS_SENSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
+FILE(WRITE laser.found "")
INSTALL(FILES laser.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
- DESTINATION "lib/cmake/wolf${PROJECT_NAME}")
-FILE(WRITE laser.found "")
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolf${PROJECT_NAME}Config.cmake"
- "${CMAKE_BINARY_DIR}/wolf${PROJECT_NAME}Config.cmake" @ONLY)
-# configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
-# "${CMAKE_BINARY_DIR}/Findwolf${PROJECT_NAME}.cmake" @ONLY)
-
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-DESTINATION include/iri-algorithms/wolf/internal)
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
-# INSTALL(FILES "${CMAKE_BINARY_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
-# DESTINATION "lib/cmake/${PROJECT_NAME}")
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolf${PROJECT_NAME}Config.cmake" DESTINATION "lib/cmake/wolf${PROJECT_NAME}")
+INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-export(PACKAGE wolf_${PROJECT_NAME})
+export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen)
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/README.md b/README.md
index 69055e1fda60d7623404761607c6bc5f531c7b80..ec729e820a55027329a8a8d9d2fbfe41aa03bdf7 100644
--- a/README.md
+++ b/README.md
@@ -1,381 +1,4 @@
-WOLF - Windowed Localization Frames
+WOLF - Windowed Localization Frames | Laser Plugin
===================================
-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/).
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
deleted file mode 100644
index d44ea8903d4c5a72af468d603dc1d8e5a6bbf542..0000000000000000000000000000000000000000
--- a/cmake_modules/FindEigen3.cmake
+++ /dev/null
@@ -1,263 +0,0 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * Redistributions of source code must retain the above copyright notice,
-# this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-# this list of conditions and the following disclaimer in the documentation
-# and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-# used to endorse or promote products derived from this software without
-# specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Author: alexs.mac@gmail.com (Alex Stewart)
-#
-
-# 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/FindGlog.cmake b/cmake_modules/FindGlog.cmake
deleted file mode 100644
index 979dceda4b8ec5e1a5457f4e32dbdc9a27834a1e..0000000000000000000000000000000000000000
--- a/cmake_modules/FindGlog.cmake
+++ /dev/null
@@ -1,346 +0,0 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * Redistributions of source code must retain the above copyright notice,
-# this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-# this list of conditions and the following disclaimer in the documentation
-# and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-# used to endorse or promote products derived from this software without
-# specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Author: alexs.mac@gmail.com (Alex Stewart)
-#
-
-# FindGlog.cmake - Find Google glog logging library.
-#
-# This module defines the following variables:
-#
-# GLOG_FOUND: TRUE iff glog is found.
-# GLOG_INCLUDE_DIRS: Include directories for glog.
-# GLOG_LIBRARIES: Libraries required to link glog.
-# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found
-# was built & installed / exported
-# as a CMake package.
-#
-# The following variables control the behaviour of this module:
-#
-# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then
-# then prefer using an exported CMake configuration
-# generated by glog > 0.3.4 over searching for the
-# glog components manually. Otherwise (FALSE)
-# ignore any exported glog CMake configurations and
-# always perform a manual search for the components.
-# Default: TRUE iff user does not define this variable
-# before we are called, and does NOT specify either
-# GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS
-# otherwise FALSE.
-# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to
-# search for glog includes, e.g: /timbuktu/include.
-# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to
-# search for glog libraries, e.g: /timbuktu/lib.
-#
-# The following variables are also defined by this module, but in line with
-# CMake recommended FindPackage() module style should NOT be referenced directly
-# by callers (use the plural variables detailed above instead). These variables
-# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
-# are NOT re-called (i.e. search for library is not repeated) if these variables
-# are set with valid values _in the CMake cache_. This means that if these
-# variables are set directly in the cache, either by the user in the CMake GUI,
-# or by the user passing -DVAR=VALUE directives to CMake when called (which
-# explicitly defines a cache variable), then they will be used verbatim,
-# bypassing the HINTS variables and other hard-coded search locations.
-#
-# GLOG_INCLUDE_DIR: Include directory for glog, not including the
-# include directory of any dependencies.
-# GLOG_LIBRARY: glog library, not including the libraries of any
-# dependencies.
-
-# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when
-# FindGlog was invoked.
-macro(GLOG_RESET_FIND_LIBRARY_PREFIX)
- if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES)
- set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}")
- endif()
-endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)
-
-# Called if we failed to find glog or any of it's required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
-macro(GLOG_REPORT_NOT_FOUND REASON_MSG)
- unset(GLOG_FOUND)
- unset(GLOG_INCLUDE_DIRS)
- unset(GLOG_LIBRARIES)
- # Make results of search visible in the CMake GUI if glog has not
- # been found so that user does not have to toggle to advanced view.
- mark_as_advanced(CLEAR GLOG_INCLUDE_DIR
- GLOG_LIBRARY)
-
- glog_reset_find_library_prefix()
-
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
- # use the camelcase library name, not uppercase.
- if (Glog_FIND_QUIETLY)
- message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN})
- elseif (Glog_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use no priority which emits a message
- # but continues configuration and allows generation.
- message("-- Failed to find glog - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(GLOG_REPORT_NOT_FOUND)
-
-# Protect against any alternative find_package scripts for this library having
-# been called previously (in a client project) which set GLOG_FOUND, but not
-# the other variables we require / set here which could cause the search logic
-# here to fail.
-unset(GLOG_FOUND)
-
-# -----------------------------------------------------------------
-# By default, if the user has expressed no preference for using an exported
-# glog CMake configuration over performing a search for the installed
-# components, and has not specified any hints for the search locations, then
-# prefer a glog exported configuration if available.
-if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION
- AND NOT GLOG_INCLUDE_DIR_HINTS
- AND NOT GLOG_LIBRARY_DIR_HINTS)
- message(STATUS "No preference for use of exported glog CMake configuration "
- "set, and no hints for include/library directories provided. "
- "Defaulting to preferring an installed/exported glog CMake configuration "
- "if available.")
- set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE)
-endif()
-
-if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
- # Try to find an exported CMake configuration for glog, as generated by
- # glog versions > 0.3.4
- #
- # We search twice, s/t we can invert the ordering of precedence used by
- # find_package() for exported package build directories, and installed
- # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7)
- # respectively in [1].
- #
- # By default, exported build directories are (in theory) detected first, and
- # this is usually the case on Windows. However, on OS X & Linux, the install
- # path (/usr/local) is typically present in the PATH environment variable
- # which is checked in item 4) in [1] (i.e. before both of the above, unless
- # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed
- # packages are usually detected in preference to exported package build
- # directories.
- #
- # To ensure a more consistent response across all OSs, and as users usually
- # want to prefer an installed version of a package over a locally built one
- # where both exist (esp. as the exported build directory might be removed
- # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which
- # means any build directories exported by the user are ignored, and thus
- # installed directories are preferred. If this fails to find the package
- # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any
- # exported build directories will now be detected.
- #
- # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which
- # is item 5) in [1]), to not preferentially use projects that were built
- # recently with the CMake GUI to ensure that we always prefer an installed
- # version if available.
- #
- # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its
- # project name when built with CMake, but exports itself as just 'glog'.
- # On Linux/OS X this does not break detection as the project name is
- # not used as part of the install path for the CMake package files,
- # e.g. /usr/local/lib/cmake/glog, where the <glog> suffix is hardcoded
- # in glog's CMakeLists. However, on Windows the project name *is*
- # part of the install prefix: C:/Program Files/google-glog/[include,lib].
- # However, by default CMake checks:
- # C:/Program Files/<FIND_PACKAGE_ARGUMENT_NAME='glog'> which does not
- # exist and thus detection fails. Thus we use the NAMES to force the
- # search to use both google-glog & glog.
- #
- # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package
- find_package(glog QUIET
- NAMES google-glog glog
- NO_MODULE
- NO_CMAKE_PACKAGE_REGISTRY
- NO_CMAKE_BUILDS_PATH)
- if (glog_FOUND)
- message(STATUS "Found installed version of glog: ${glog_DIR}")
- else()
- # Failed to find an installed version of glog, repeat search allowing
- # exported build directories.
- message(STATUS "Failed to find installed glog CMake configuration, "
- "searching for glog build directories exported with CMake.")
- # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and
- # do not want to treat projects built with the CMake GUI preferentially.
- find_package(glog QUIET
- NAMES google-glog glog
- NO_MODULE
- NO_CMAKE_BUILDS_PATH)
- if (glog_FOUND)
- message(STATUS "Found exported glog build directory: ${glog_DIR}")
- endif(glog_FOUND)
- endif(glog_FOUND)
-
- set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND})
-
- if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
- message(STATUS "Detected glog version: ${glog_VERSION}")
- set(GLOG_FOUND ${glog_FOUND})
- # glog wraps the include directories into the exported glog::glog target.
- set(GLOG_INCLUDE_DIR "")
- set(GLOG_LIBRARY glog::glog)
- else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
- message(STATUS "Failed to find an installed/exported CMake configuration "
- "for glog, will perform search for installed glog components.")
- endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
-endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
-
-if (NOT GLOG_FOUND)
- # Either failed to find an exported glog CMake configuration, or user
- # told us not to use one. Perform a manual search for all glog components.
-
- # Handle possible presence of lib prefix for libraries on MSVC, see
- # also GLOG_RESET_FIND_LIBRARY_PREFIX().
- if (MSVC)
- # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES
- # s/t we can set it back before returning.
- set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}")
- # The empty string in this list is important, it represents the case when
- # the libraries have no prefix (shared libraries / DLLs).
- set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}")
- endif (MSVC)
-
- # Search user-installed locations first, so that we prefer user installs
- # to system installs where both exist.
- list(APPEND GLOG_CHECK_INCLUDE_DIRS
- /usr/local/include
- /usr/local/homebrew/include # Mac OS X
- /opt/local/var/macports/software # Mac OS X.
- /opt/local/include
- /usr/include)
- # Windows (for C:/Program Files prefix).
- list(APPEND GLOG_CHECK_PATH_SUFFIXES
- glog/include
- glog/Include
- Glog/include
- Glog/Include
- google-glog/include # CMake installs with project name prefix.
- google-glog/Include)
-
- list(APPEND GLOG_CHECK_LIBRARY_DIRS
- /usr/local/lib
- /usr/local/homebrew/lib # Mac OS X.
- /opt/local/lib
- /usr/lib)
- # Windows (for C:/Program Files prefix).
- list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES
- glog/lib
- glog/Lib
- Glog/lib
- Glog/Lib
- google-glog/lib # CMake installs with project name prefix.
- google-glog/Lib)
-
- # Search supplied hint directories first if supplied.
- find_path(GLOG_INCLUDE_DIR
- NAMES glog/logging.h
- HINTS ${GLOG_INCLUDE_DIR_HINTS}
- PATHS ${GLOG_CHECK_INCLUDE_DIRS}
- PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES})
- if (NOT GLOG_INCLUDE_DIR OR
- NOT EXISTS ${GLOG_INCLUDE_DIR})
- glog_report_not_found(
- "Could not find glog include directory, set GLOG_INCLUDE_DIR "
- "to directory containing glog/logging.h")
- endif (NOT GLOG_INCLUDE_DIR OR
- NOT EXISTS ${GLOG_INCLUDE_DIR})
-
- find_library(GLOG_LIBRARY NAMES glog
- HINTS ${GLOG_LIBRARY_DIR_HINTS}
- PATHS ${GLOG_CHECK_LIBRARY_DIRS}
- PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES})
- if (NOT GLOG_LIBRARY OR
- NOT EXISTS ${GLOG_LIBRARY})
- glog_report_not_found(
- "Could not find glog library, set GLOG_LIBRARY "
- "to full path to libglog.")
- endif (NOT GLOG_LIBRARY OR
- NOT EXISTS ${GLOG_LIBRARY})
-
- # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets
- # if called.
- set(GLOG_FOUND TRUE)
-
- # Glog does not seem to provide any record of the version in its
- # source tree, thus cannot extract version.
-
- # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and
- # thus FIND_[PATH/LIBRARY] are not called, but specified locations are
- # invalid, otherwise we would report the library as found.
- if (GLOG_INCLUDE_DIR AND
- NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
- glog_report_not_found(
- "Caller defined GLOG_INCLUDE_DIR:"
- " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.")
- endif (GLOG_INCLUDE_DIR AND
- NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
- # TODO: This regex for glog library is pretty primitive, we use lowercase
- # for comparison to handle Windows using CamelCase library names, could
- # this check be better?
- string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY)
- if (GLOG_LIBRARY AND
- NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
- glog_report_not_found(
- "Caller defined GLOG_LIBRARY: "
- "${GLOG_LIBRARY} does not match glog.")
- endif (GLOG_LIBRARY AND
- NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
-
- glog_reset_find_library_prefix()
-
-endif(NOT GLOG_FOUND)
-
-# Set standard CMake FindPackage variables if found.
-if (GLOG_FOUND)
- set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
- set(GLOG_LIBRARIES ${GLOG_LIBRARY})
-endif (GLOG_FOUND)
-
-# If we are using an exported CMake glog target, the include directories are
-# wrapped into the target itself, and do not have to be (and are not)
-# separately specified. In which case, we should not add GLOG_INCLUDE_DIRS
-# to the list of required variables in order that glog be reported as found.
-if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
- set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES)
-else()
- set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES)
-endif()
-
-# Handle REQUIRED / QUIET optional arguments.
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(Glog DEFAULT_MSG
- ${GLOG_REQUIRED_VARIABLES})
-
-# Only mark internal variables as advanced if we found glog, otherwise
-# leave them visible in the standard GUI for the user to set manually.
-if (GLOG_FOUND)
- mark_as_advanced(FORCE GLOG_INCLUDE_DIR
- GLOG_LIBRARY
- glog_DIR) # Autogenerated by find_package(glog)
-endif (GLOG_FOUND)
diff --git a/cmake_modules/FindYamlCpp.cmake b/cmake_modules/FindYamlCpp.cmake
deleted file mode 100644
index 196c4754e2787a9fa4f47eb8716fd2525bc84fd4..0000000000000000000000000000000000000000
--- a/cmake_modules/FindYamlCpp.cmake
+++ /dev/null
@@ -1,99 +0,0 @@
-# Locate yaml-cpp
-#
-# This module defines
-# YAMLCPP_FOUND, if false, do not try to link to yaml-cpp
-# YAMLCPP_LIBNAME, name of yaml library
-# YAMLCPP_LIBRARY, where to find yaml-cpp
-# YAMLCPP_LIBRARY_RELEASE, where to find Release or RelWithDebInfo yaml-cpp
-# YAMLCPP_LIBRARY_DEBUG, where to find Debug yaml-cpp
-# YAMLCPP_INCLUDE_DIR, where to find yaml.h
-# YAMLCPP_LIBRARY_DIR, the directories to find YAMLCPP_LIBRARY
-#
-# By default, the dynamic libraries of yaml-cpp will be found. To find the static ones instead,
-# you must set the YAMLCPP_USE_STATIC_LIBS variable to TRUE before calling find_package(YamlCpp ...)
-
-# attempt to find static library first if this is set
-if(YAMLCPP_USE_STATIC_LIBS)
- set(YAMLCPP_STATIC libyaml-cpp.a)
- set(YAMLCPP_STATIC_DEBUG libyaml-cpp-dbg.a)
-endif()
-
-if(${CMAKE_SYSTEM_NAME} MATCHES "Windows") ### Set Yaml libary name for Windows
- set(YAMLCPP_LIBNAME "libyaml-cppmd" CACHE STRING "Name of YAML library")
- set(YAMLCPP_LIBNAME optimized ${YAMLCPP_LIBNAME} debug ${YAMLCPP_LIBNAME}d)
-else() ### Set Yaml libary name for Unix, Linux, OS X, etc
- set(YAMLCPP_LIBNAME "yaml-cpp" CACHE STRING "Name of YAML library")
-endif()
-
-# find the yaml-cpp include directory
-find_path(YAMLCPP_INCLUDE_DIR
- NAMES yaml-cpp/yaml.h
- PATH_SUFFIXES include
- PATHS
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/include
- ~/Library/Frameworks/yaml-cpp/include/
- /Library/Frameworks/yaml-cpp/include/
- /usr/local/include/
- /usr/include/
- /sw/yaml-cpp/ # Fink
- /opt/local/yaml-cpp/ # DarwinPorts
- /opt/csw/yaml-cpp/ # Blastwave
- /opt/yaml-cpp/)
-
-# find the release yaml-cpp library
-find_library(YAMLCPP_LIBRARY_RELEASE
- NAMES ${YAMLCPP_STATIC} yaml-cpp libyaml-cppmd.lib
- PATH_SUFFIXES lib64 lib Release RelWithDebInfo
- PATHS
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/build
- ~/Library/Frameworks
- /Library/Frameworks
- /usr/local
- /usr
- /sw
- /opt/local
- /opt/csw
- /opt)
-
-# find the debug yaml-cpp library
-find_library(YAMLCPP_LIBRARY_DEBUG
- NAMES ${YAMLCPP_STATIC_DEBUG} yaml-cpp-dbg libyaml-cppmdd.lib
- PATH_SUFFIXES lib64 lib Debug
- PATHS
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/
- ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/build
- ~/Library/Frameworks
- /Library/Frameworks
- /usr/local
- /usr
- /sw
- /opt/local
- /opt/csw
- /opt)
-
-# set library vars
-set(YAMLCPP_LIBRARY ${YAMLCPP_LIBRARY_RELEASE})
-if(CMAKE_BUILD_TYPE MATCHES Debug AND EXISTS ${YAMLCPP_LIBRARY_DEBUG})
- set(YAMLCPP_LIBRARY ${YAMLCPP_LIBRARY_DEBUG})
-endif()
-
-get_filename_component(YAMLCPP_LIBRARY_RELEASE_DIR ${YAMLCPP_LIBRARY_RELEASE} PATH)
-get_filename_component(YAMLCPP_LIBRARY_DEBUG_DIR ${YAMLCPP_LIBRARY_DEBUG} PATH)
-set(YAMLCPP_LIBRARY_DIR ${YAMLCPP_LIBRARY_RELEASE_DIR} ${YAMLCPP_LIBRARY_DEBUG_DIR})
-
-# handle the QUIETLY and REQUIRED arguments and set YAMLCPP_FOUND to TRUE if all listed variables are TRUE
-include(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(YamlCpp DEFAULT_MSG
- YAMLCPP_INCLUDE_DIR
- YAMLCPP_LIBRARY
- YAMLCPP_LIBRARY_DIR)
-mark_as_advanced(
- YAMLCPP_INCLUDE_DIR
- YAMLCPP_LIBRARY_DIR
- YAMLCPP_LIBRARY
- YAMLCPP_LIBRARY_RELEASE
- YAMLCPP_LIBRARY_RELEASE_DIR
- YAMLCPP_LIBRARY_DEBUG
- YAMLCPP_LIBRARY_DEBUG_DIR)
-
diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake
new file mode 100755
index 0000000000000000000000000000000000000000..6dafd97ded05ef9df2b4b19b318639855fa48af5
--- /dev/null
+++ b/cmake_modules/Findcsm.cmake
@@ -0,0 +1,64 @@
+FIND_PATH(
+ csm_INCLUDE_DIR
+ NAMES algos.h
+ PATHS /usr/local/include/csm)
+IF(csm_INCLUDE_DIR)
+ MESSAGE("Found csm include dirs: ${csm_INCLUDE_DIR}")
+ELSE(csm_INCLUDE_DIR)
+ MESSAGE("Couldn't find csm include dirs")
+ENDIF(csm_INCLUDE_DIR)
+
+FIND_LIBRARY(
+ csm_LIBRARY
+ NAMES libcsm.so libcsm.dylib
+ PATHS /usr/local/lib)
+IF(csm_LIBRARY)
+ MESSAGE("Found csm lib: ${csm_LIBRARY}")
+ELSE(csm_LIBRARY)
+ MESSAGE("Couldn't find csm lib")
+ENDIF(csm_LIBRARY)
+
+IF (csm_INCLUDE_DIR AND csm_LIBRARY)
+ SET(csm_FOUND TRUE)
+ ELSE(csm_INCLUDE_DIR AND csm_LIBRARY)
+ set(csm_FOUND FALSE)
+ENDIF (csm_INCLUDE_DIR AND csm_LIBRARY)
+
+IF (csm_FOUND)
+ IF (NOT csm_FIND_QUIETLY)
+ MESSAGE(STATUS "Found csm: ${csm_LIBRARY}")
+ ENDIF (NOT csm_FIND_QUIETLY)
+ELSE (csm_FOUND)
+ IF (csm_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find csm")
+ ENDIF (csm_FIND_REQUIRED)
+ENDIF (csm_FOUND)
+
+
+macro(csm_report_not_found REASON_MSG)
+ set(csm_FOUND FALSE)
+ unset(csm_INCLUDE_DIR)
+ unset(csm_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 (csm_FIND_QUIETLY)
+ message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN})
+ elseif (csm_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find csm - " ${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 csm - " ${REASON_MSG} ${ARGN})
+ endif ()
+ return()
+endmacro(csm_report_not_found)
+
+if(NOT csm_FOUND)
+ csm_report_not_found("Something went wrong while setting up csm.")
+endif(NOT csm_FOUND)
+# Set the include directories for csm (itself).
+set(csm_FOUND TRUE)
\ No newline at end of file
diff --git a/cmake_modules/Findwolf.cmake b/cmake_modules/Findwolf.cmake
deleted file mode 100644
index 469f3372a9869fb66b9ffea159ec252b933ece18..0000000000000000000000000000000000000000
--- a/cmake_modules/Findwolf.cmake
+++ /dev/null
@@ -1,28 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolf_INCLUDE_DIRS
- NAMES wolf.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
-#change INCLUDE_DIRS to its parent directory
-# get_filename_component(wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIRS} DIRECTORY)
-MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}")
-
-FIND_LIBRARY(
- wolf_LIBRARY
- NAMES wolf
- PATHS /usr/lib /usr/local/lib /usr/local/lib/iri-algorithms)
-
-IF (wolf_INCLUDE_DIRS AND wolf_LIBRARY)
- SET(wolf_FOUND TRUE)
-ENDIF (wolf_INCLUDE_DIRS 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)
-
diff --git a/cmake_modules/Findwolflaser.cmake b/cmake_modules/Findwolflaser.cmake
deleted file mode 100644
index 193b942845db2a8854ecc498f48b883ce50ca4ba..0000000000000000000000000000000000000000
--- a/cmake_modules/Findwolflaser.cmake
+++ /dev/null
@@ -1,36 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- laser_INCLUDE_DIRS
- NAMES laser.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_laser)
-#change INCLUDE_DIRS to its parent directory
-# get_filename_component(laser_INCLUDE_DIRS ${example_INCLUDE_DIRS} DIRECTORY)
-IF(laser_INCLUDE_DIRS)
- MESSAGE("Found laser include dirs: ${laser_INCLUDE_DIRS}")
-ELSE
- MESSAGE("Couldn't find laser include dirs")
-ENDIF
-
-FIND_LIBRARY(
- laser_LIBRARY
- NAMES libwolflaser.so
- PATHS /usr/lib /usr/local/lib /usr/local/lib/iri-algorithms)
-IF(laser_LIBRARY)
- MESSAGE("Found laser lib: ${laser_LIBRARY}")
-ELSE
- MESSAGE("Couldn't find laser lib")
-ENDIF
-IF (laser_INCLUDE_DIRS AND laser_LIBRARY)
- SET(laser_FOUND TRUE)
-ENDIF (laser_INCLUDE_DIRS AND laser_LIBRARY)
-
-IF (laser_FOUND)
- IF (NOT laser_FIND_QUIETLY)
- MESSAGE(STATUS "Found laser: ${laser_LIBRARY}")
- ENDIF (NOT laser_FIND_QUIETLY)
-ELSE (laser_FOUND)
- IF (wolf_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find laser")
- ENDIF (wolf_FIND_REQUIRED)
-ENDIF (laser_FOUND)
-
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
deleted file mode 100644
index 82bb7cf0a5523302eb3d6c7fd5064e382a6c59f5..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfConfig.cmake
+++ /dev/null
@@ -1,222 +0,0 @@
-# This file was copied and adapted from the ceres_solver project
-# http://ceres-solver.org/
-
-# wolf - Windowed Localization Frames
-# Copyright 2016
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * Redistributions of source code must retain the above copyright notice,
-# this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-# this list of conditions and the following disclaimer in the documentation
-# and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-# used to endorse or promote products derived from this software without
-# specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Authors:
-#
-
-# Config file for wolf - Find wolf & dependencies.
-#
-# This file is used by CMake when find_package(wolf) is invoked and either
-# the directory containing this file either is present in CMAKE_MODULE_PATH
-# (if wolf was installed), or exists in the local CMake package registry if
-# the wolf build directory was exported.
-#
-# This module defines the following variables:
-#
-# wolf_FOUND / wolf_FOUND: True if wolf has been successfully
-# found. Both variables are set as although
-# FindPackage() only references wolf_FOUND
-# in Config mode, given the conventions for
-# <package>_FOUND when FindPackage() is
-# called in Module mode, users could
-# reasonably expect to use wolf_FOUND
-# instead.
-#
-# wolf_VERSION: Version of wolf found.
-#
-# wolf_INCLUDE_DIRS: Include directories for wolf and the
-# dependencies which appear in the wolf public
-# API and are thus required to use wolf.
-#
-# wolf_LIBRARIES: Libraries for wolf and all
-# dependencies against which wolf was
-# compiled. This will not include any optional
-# dependencies that were disabled when wolf was
-# compiled.
-#
-# The following variables are also defined for legacy compatibility
-# only. Any new code should not use them as they do not conform to
-# the standard CMake FindPackage naming conventions.
-#
-# wolf_INCLUDES = ${wolf_INCLUDE_DIRS}.
-
-# Called if we failed to find Ceres or any of its required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
-macro(wolf_report_not_found REASON_MSG)
- # FindPackage() only references Ceres_FOUND, and requires it to be
- # explicitly set FALSE to denote not found (not merely undefined).
- set(wolf_FOUND FALSE)
- set(wolf_FOUND FALSE)
- unset(wolf_INCLUDE_DIRS)
- unset(wolf_LIBRARIES)
-
- # Reset the CMake module path to its state when this script was called.
- set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
- # FindPackage() use the camelcase library name, not uppercase.
- if (wolf_FIND_QUIETLY)
- message(STATUS "Failed to find wolf - " ${REASON_MSG} ${ARGN})
- else (wolf_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
- # that prevents generation, but continues configuration.
- message(SEND_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(wolf_report_not_found)
-
-# Record the state of the CMake module path when this script was
-# called so that we can ensure that we leave it in the same state on
-# exit as it was on entry, but modify it locally.
-set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
-
-# Get the (current, i.e. installed) directory containing this file.
-get_filename_component(wolf_CURRENT_CONFIG_DIR
- "${CMAKE_CURRENT_LIST_FILE}" PATH)
-
-# Reset CMake module path to the installation directory of this
-# script, thus we will use the FindPackage() scripts shipped with
-# wolf to find wolf' dependencies, even if the user has equivalently
-# named FindPackage() scripts in their project.
-set(CMAKE_MODULE_PATH ${wolf_CURRENT_CONFIG_DIR})
-
-# Build the absolute root install directory as a relative path
-# (determined when wolf was configured & built) from the current
-# install directory for this this file. This allows for the install
-# tree to be relocated, after wolf was built, outside of CMake.
-get_filename_component(CURRENT_ROOT_INSTALL_DIR
- ${wolf_CURRENT_CONFIG_DIR}/../../../
- ABSOLUTE)
-if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
- wolf_report_not_found(
- "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
- "determined from relative path from wolfConfig.cmake install location: "
- "${wolf_CURRENT_CONFIG_DIR}, does not exist. Either the install "
- "directory was deleted, or the install tree was only partially relocated "
- "outside of CMake after wolf was built.")
-endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
-
-# Set the include directories for wolf (itself).
-set(wolf_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include/iri-algorithms")
-
-# if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
-if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
- wolf_report_not_found(
- "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
- "determined from relative path from wolfConfig.cmake install location: "
- "${wolf_CURRENT_CONFIG_DIR}, does not contain wolf headers. "
- "Either the install directory was deleted, or the install tree was only "
- "partially relocated outside of CMake after wolf was built.")
-# endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
-endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
-list(APPEND wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIR}/wolf)
-
-# Set the version.
-set(wolf_VERSION 0.0.1)
-
-# Eigen.
-
-# Flag set during configuration and build of wolf.
-set(wolf_EIGEN_VERSION @EIGEN_VERSION@)
-# Append the locations of Eigen when wolf was built to the search path hints.
-list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3)
-# Search quietly to control the timing of the error message if not found. The
-# search should be for an exact match, but for usability reasons do a soft
-# match and reject with an explanation below.
-
-find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET)
-
-# Flag set with currently found Eigen version.
-set(EIGEN_VERSION @EIGEN_VERSION@)
-if (EIGEN3_FOUND)
- if (NOT EIGEN_VERSION VERSION_EQUAL wolf_EIGEN_VERSION)
- # CMake's VERSION check in FIND_PACKAGE() will accept any version >= the
- # specified version. However, only version = is supported. Improve
- # usability by explaining why we don't accept non-exact version matching.
- wolf_report_not_found("Found Eigen dependency, but the version of Eigen "
- "found (${EIGEN_VERSION}) does not exactly match the version of Eigen "
- "wolf was compiled with (${wolf_EIGEN_VERSION}). This can cause subtle "
- "bugs by triggering violations of the One Definition Rule. See the "
- "Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule "
- "for more details")
- endif ()
- message(STATUS "Found required wolf dependency: "
- "Eigen version ${wolf_EIGEN_VERSION} in ${EIGEN3_INCLUDE_DIR}")
-else (EIGEN3_FOUND)
- wolf_report_not_found("Missing required wolf "
- "dependency: Eigen version ${wolf_EIGEN_VERSION}, please set "
- "EIGEN3_INCLUDE_DIR.")
-endif (EIGEN3_FOUND)
-list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
-
-# Import exported wolf targets, if they have not already been imported.
-if (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
- include(${wolf_CURRENT_CONFIG_DIR}/wolfTargets.cmake)
-endif (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
-# Set the expected XX_LIBRARIES variable for FindPackage().
-set(wolf_LIBRARIES wolf)
-
-# Set legacy library variable for backwards compatibility.
-set(wolf_LIBRARY ${wolf_LIBRARIES})
-
-# Make user aware of any compile flags that will be added to their targets
-# which use wolf (i.e. flags exported in the wolf target). Only CMake
-# versions >= 2.8.12 support target_compile_options().
-if (TARGET ${wolf_LIBRARIES} AND
- NOT CMAKE_VERSION VERSION_LESS "2.8.12")
- get_target_property(wolf_INTERFACE_COMPILE_OPTIONS
- ${wolf_LIBRARIES} INTERFACE_COMPILE_OPTIONS)
-
- set(wolf_LOCATION "${CURRENT_ROOT_INSTALL_DIR}")
-
- # Check for -std=c++11 flags.
- if (wolf_INTERFACE_COMPILE_OPTIONS MATCHES ".*std=c\\+\\+11.*")
- message(STATUS "wolf version ${wolf_VERSION} detected here: "
- "${wolf_LOCATION} was built with C++11. wolf target will add "
- "C++11 flags to compile options for targets using it.")
- endif()
-endif()
-
-# Reset CMake module path to its state when this script was called.
-set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-# As we use wolf_REPORT_NOT_FOUND() to abort, if we reach this point we have
-# found wolf and all required dependencies.
-message(STATUS "Found wolf version: ${wolf_VERSION} installed in: ${CURRENT_ROOT_INSTALL_DIR}")
-
-# Set wolf_FOUND to be equivalent to wolf_FOUND, which is set to
-# TRUE by FindPackage() if this file is found and run, and after which
-# wolf_FOUND is not (explicitly, i.e. undefined does not count) set
-# to FALSE.
-set(wolf_FOUND TRUE)
diff --git a/cmake_modules/wolflaserConfig.cmake b/cmake_modules/wolflaserConfig.cmake
index 82bb7cf0a5523302eb3d6c7fd5064e382a6c59f5..87cf4b3f6b749b1e7f4300bebe998b694a45a0e7 100644
--- a/cmake_modules/wolflaserConfig.cmake
+++ b/cmake_modules/wolflaserConfig.cmake
@@ -1,222 +1,91 @@
-# This file was copied and adapted from the ceres_solver project
-# http://ceres-solver.org/
+#edit the following line to add the librarie's header files
+FIND_PATH(
+ wolflaser_INCLUDE_DIRS
+ NAMES laser.found
+ PATHS /usr/local/include/iri-algorithms/wolf/plugin_laser)
+IF(wolflaser_INCLUDE_DIRS)
+ MESSAGE("Found laser include dirs: ${wolflaser_INCLUDE_DIRS}")
+ELSE(wolflaser_INCLUDE_DIRS)
+ MESSAGE("Couldn't find laser include dirs")
+ENDIF(wolflaser_INCLUDE_DIRS)
+
+FIND_LIBRARY(
+ wolflaser_LIBRARIES
+ NAMES libwolflaser.so
+ PATHS /usr/local/lib)
+IF(wolflaser_LIBRARIES)
+ MESSAGE("Found laser lib: ${wolflaser_LIBRARIES}")
+ELSE(wolflaser_LIBRARIES)
+ MESSAGE("Couldn't find wolf laser lib")
+ENDIF(wolflaser_LIBRARIES)
+
+IF (wolflaser_INCLUDE_DIRS AND wolflaser_LIBRARIES)
+ SET(wolflaser_FOUND TRUE)
+ ELSE(wolflaser_INCLUDE_DIRS AND wolflaser_LIBRARIES)
+ set(wolflaser_FOUND FALSE)
+ENDIF (wolflaser_INCLUDE_DIRS AND wolflaser_LIBRARIES)
+
+IF (wolflaser_FOUND)
+ IF (NOT wolflaser_FIND_QUIETLY)
+ MESSAGE(STATUS "Found laser: ${wolflaser_LIBRARIES}")
+ ENDIF (NOT wolflaser_FIND_QUIETLY)
+ELSE (wolflaser_FOUND)
+ IF (wolflaser_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find wolf laser")
+ ENDIF (wolflaser_FIND_REQUIRED)
+ENDIF (wolflaser_FOUND)
-# wolf - Windowed Localization Frames
-# Copyright 2016
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * Redistributions of source code must retain the above copyright notice,
-# this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-# this list of conditions and the following disclaimer in the documentation
-# and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-# used to endorse or promote products derived from this software without
-# specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Authors:
-#
-
-# Config file for wolf - Find wolf & dependencies.
-#
-# This file is used by CMake when find_package(wolf) is invoked and either
-# the directory containing this file either is present in CMAKE_MODULE_PATH
-# (if wolf was installed), or exists in the local CMake package registry if
-# the wolf build directory was exported.
-#
-# This module defines the following variables:
-#
-# wolf_FOUND / wolf_FOUND: True if wolf has been successfully
-# found. Both variables are set as although
-# FindPackage() only references wolf_FOUND
-# in Config mode, given the conventions for
-# <package>_FOUND when FindPackage() is
-# called in Module mode, users could
-# reasonably expect to use wolf_FOUND
-# instead.
-#
-# wolf_VERSION: Version of wolf found.
-#
-# wolf_INCLUDE_DIRS: Include directories for wolf and the
-# dependencies which appear in the wolf public
-# API and are thus required to use wolf.
-#
-# wolf_LIBRARIES: Libraries for wolf and all
-# dependencies against which wolf was
-# compiled. This will not include any optional
-# dependencies that were disabled when wolf was
-# compiled.
-#
-# The following variables are also defined for legacy compatibility
-# only. Any new code should not use them as they do not conform to
-# the standard CMake FindPackage naming conventions.
-#
-# wolf_INCLUDES = ${wolf_INCLUDE_DIRS}.
-
-# Called if we failed to find Ceres or any of its required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(wolf_report_not_found REASON_MSG)
- # FindPackage() only references Ceres_FOUND, and requires it to be
- # explicitly set FALSE to denote not found (not merely undefined).
- set(wolf_FOUND FALSE)
- set(wolf_FOUND FALSE)
- unset(wolf_INCLUDE_DIRS)
- unset(wolf_LIBRARIES)
+ set(wolflaser_FOUND FALSE)
+ unset(wolflaser_INCLUDE_DIRS)
+ unset(wolflaser_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})
+ if (wolflaser_FIND_QUIETLY)
+ message(STATUS "Failed to find wolf laser- " ${REASON_MSG} ${ARGN})
+ else (wolflaser_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find wolf laser - " ${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})
+ message(SEND_ERROR "Failed to find wolf laser - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(wolf_report_not_found)
-# Record the state of the CMake module path when this script was
-# called so that we can ensure that we leave it in the same state on
-# exit as it was on entry, but modify it locally.
-set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
-
-# Get the (current, i.e. installed) directory containing this file.
-get_filename_component(wolf_CURRENT_CONFIG_DIR
- "${CMAKE_CURRENT_LIST_FILE}" PATH)
-
-# Reset CMake module path to the installation directory of this
-# script, thus we will use the FindPackage() scripts shipped with
-# wolf to find wolf' dependencies, even if the user has equivalently
-# named FindPackage() scripts in their project.
-set(CMAKE_MODULE_PATH ${wolf_CURRENT_CONFIG_DIR})
-
-# Build the absolute root install directory as a relative path
-# (determined when wolf was configured & built) from the current
-# install directory for this this file. This allows for the install
-# tree to be relocated, after wolf was built, outside of CMake.
-get_filename_component(CURRENT_ROOT_INSTALL_DIR
- ${wolf_CURRENT_CONFIG_DIR}/../../../
- ABSOLUTE)
-if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
- wolf_report_not_found(
- "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
- "determined from relative path from wolfConfig.cmake install location: "
- "${wolf_CURRENT_CONFIG_DIR}, does not exist. Either the install "
- "directory was deleted, or the install tree was only partially relocated "
- "outside of CMake after wolf was built.")
-endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
-
+if(NOT wolflaser_FOUND)
+ wolf_report_not_found("Something went wrong while setting up wolf vision.")
+endif(NOT wolflaser_FOUND)
# Set the include directories for wolf (itself).
-set(wolf_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include/iri-algorithms")
-
-# if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
-if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
- wolf_report_not_found(
- "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
- "determined from relative path from wolfConfig.cmake install location: "
- "${wolf_CURRENT_CONFIG_DIR}, does not contain wolf headers. "
- "Either the install directory was deleted, or the install tree was only "
- "partially relocated outside of CMake after wolf was built.")
-# endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
-endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
-list(APPEND wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIR}/wolf)
+set(wolflaser_FOUND TRUE)
-# Set the version.
-set(wolf_VERSION 0.0.1)
+# Now we gather all the required dependencies for Wolf Laser
-# Eigen.
+FIND_PACKAGE(laser_scan_utils REQUIRED)
+list(APPEND wolflaser_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
+list(APPEND wolflaser_LIBRARIES ${laser_scan_utils_LIBRARY})
-# Flag set during configuration and build of wolf.
-set(wolf_EIGEN_VERSION @EIGEN_VERSION@)
-# Append the locations of Eigen when wolf was built to the search path hints.
-list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3)
-# Search quietly to control the timing of the error message if not found. The
-# search should be for an exact match, but for usability reasons do a soft
-# match and reject with an explanation below.
+#Making sure Wolf is looked for
+if(NOT wolf_FOUND)
+ FIND_PACKAGE(wolfcore REQUIRED)
-find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET)
+ #We reverse in order to insert at the start
+ list(REVERSE wolflaser_INCLUDE_DIRS)
+ list(APPEND wolflaser_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
+ list(REVERSE wolflaser_INCLUDE_DIRS)
-# Flag set with currently found Eigen version.
-set(EIGEN_VERSION @EIGEN_VERSION@)
-if (EIGEN3_FOUND)
- if (NOT EIGEN_VERSION VERSION_EQUAL wolf_EIGEN_VERSION)
- # CMake's VERSION check in FIND_PACKAGE() will accept any version >= the
- # specified version. However, only version = is supported. Improve
- # usability by explaining why we don't accept non-exact version matching.
- wolf_report_not_found("Found Eigen dependency, but the version of Eigen "
- "found (${EIGEN_VERSION}) does not exactly match the version of Eigen "
- "wolf was compiled with (${wolf_EIGEN_VERSION}). This can cause subtle "
- "bugs by triggering violations of the One Definition Rule. See the "
- "Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule "
- "for more details")
- endif ()
- message(STATUS "Found required wolf dependency: "
- "Eigen version ${wolf_EIGEN_VERSION} in ${EIGEN3_INCLUDE_DIR}")
-else (EIGEN3_FOUND)
- wolf_report_not_found("Missing required wolf "
- "dependency: Eigen version ${wolf_EIGEN_VERSION}, please set "
- "EIGEN3_INCLUDE_DIR.")
-endif (EIGEN3_FOUND)
-list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
-
-# Import exported wolf targets, if they have not already been imported.
-if (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
- include(${wolf_CURRENT_CONFIG_DIR}/wolfTargets.cmake)
-endif (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
-# Set the expected XX_LIBRARIES variable for FindPackage().
-set(wolf_LIBRARIES wolf)
-
-# Set legacy library variable for backwards compatibility.
-set(wolf_LIBRARY ${wolf_LIBRARIES})
-
-# Make user aware of any compile flags that will be added to their targets
-# which use wolf (i.e. flags exported in the wolf target). Only CMake
-# versions >= 2.8.12 support target_compile_options().
-if (TARGET ${wolf_LIBRARIES} AND
- NOT CMAKE_VERSION VERSION_LESS "2.8.12")
- get_target_property(wolf_INTERFACE_COMPILE_OPTIONS
- ${wolf_LIBRARIES} INTERFACE_COMPILE_OPTIONS)
+ list(REVERSE wolflaser_LIBRARIES)
+ list(APPEND wolflaser_LIBRARIES ${wolfcore_LIBRARIES})
+ list(REVERSE wolflaser_LIBRARIES)
- set(wolf_LOCATION "${CURRENT_ROOT_INSTALL_DIR}")
-
- # Check for -std=c++11 flags.
- if (wolf_INTERFACE_COMPILE_OPTIONS MATCHES ".*std=c\\+\\+11.*")
- message(STATUS "wolf version ${wolf_VERSION} detected here: "
- "${wolf_LOCATION} was built with C++11. wolf target will add "
- "C++11 flags to compile options for targets using it.")
- endif()
endif()
-# Reset CMake module path to its state when this script was called.
-set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-# As we use wolf_REPORT_NOT_FOUND() to abort, if we reach this point we have
-# found wolf and all required dependencies.
-message(STATUS "Found wolf version: ${wolf_VERSION} installed in: ${CURRENT_ROOT_INSTALL_DIR}")
-
-# Set wolf_FOUND to be equivalent to wolf_FOUND, which is set to
-# TRUE by FindPackage() if this file is found and run, and after which
-# wolf_FOUND is not (explicitly, i.e. undefined does not count) set
-# to FALSE.
-set(wolf_FOUND TRUE)
+# provide both INCLUDE_DIR and INCLUDE_DIRS
+SET(wolflaser_INCLUDE_DIR ${wolflaser_INCLUDE_DIRS})
+# provide both LIBRARY and LIBRARIES
+SET(wolflaser_LIBRARY ${wolflaser_LIBRARIES})
\ No newline at end of file
diff --git a/codetemplates eclipse.xml b/codetemplates eclipse.xml
index a83cc56bd43bb272eff012dd89a401c33283610f..85f43146f9e51b952d6bc33e75bcda962262dd0a 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/src/examples/.gitignore b/demos/.gitignore
similarity index 100%
rename from src/examples/.gitignore
rename to demos/.gitignore
diff --git a/src/examples/test_2_lasers_offline.cpp b/demos/demo_2_lasers_offline.cpp
similarity index 83%
rename from src/examples/test_2_lasers_offline.cpp
rename to demos/demo_2_lasers_offline.cpp
index 58e91c6d8a63e81ff0337ee1ec4a7b601cdb9de7..8ec341798b6dc098dee0b298983d36f1e8bbb87e 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/demos/demo_2_lasers_offline.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
//std includes
#include "core/sensor/sensor_GPS_fix.h"
#include <cstdlib>
@@ -37,7 +58,7 @@
#include "faramotics/rangeScan2D.h"
#include "btr-headers/pose3d.h"
-void extractVector(std::ifstream& text_file, Eigen::VectorXs& vector, wolf::Scalar& ts)
+void extractVector(std::ifstream& text_file, Eigen::VectorXd& vector, double& ts)
{
std::string line;
std::getline(text_file, line);
@@ -47,7 +68,7 @@ void extractVector(std::ifstream& text_file, Eigen::VectorXs& vector, wolf::Scal
line_stream >> vector(i);
}
-void extractScan(std::ifstream& text_file, std::vector<float>& scan, wolf::Scalar& ts)
+void extractScan(std::ifstream& text_file, std::vector<float>& scan, double& ts)
{
std::string line;
std::getline(text_file, line);
@@ -92,27 +113,27 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
- Scalar odom_std_factor = 0.5;
+ double odom_std_factor = 0.5;
std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<double> 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);
+ Eigen::VectorXd odom_data = Eigen::VectorXd::Zero(2);
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::VectorXd ground_truth_pose(3); //last true pose
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(6);
clock_t t1, t2;
- Scalar timestamp;
+ double 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
+ Eigen::VectorXd odom_pose = Eigen::VectorXd::Zero(3);
+ //Eigen::VectorXd gps_position = Eigen::VectorXd::Zero(2);
+ Eigen::VectorXd laser_1_params(9), laser_2_params(9);
+ Eigen::VectorXd laser_1_pose(4), laser_2_pose(4); //xyz + theta
+ Eigen::VectorXd laser_1_pose2D(3), laser_2_pose2D(3); //xy + theta
// odometry intrinsics
IntrinsicsOdom2D odom_intrinsics;
@@ -160,7 +181,7 @@ int main(int argc, char** argv)
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);
+ CaptureMotion* odom_capture = new CaptureMotion(ts, odom_sensor, odom_data, Eigen::Matrix2d::Identity() * odom_std_factor * odom_std_factor, nullptr);
// Initial pose
ground_truth_pose << 2, 8, 0;
@@ -168,18 +189,17 @@ int main(int argc, char** argv)
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);
+ problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts);
// 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;
+ SolverCeres ceres_manager(&problem);
+ ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false;
+ // ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS;
+ // ceres_manager.getSolverOptions().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;
@@ -262,7 +282,7 @@ int main(int argc, char** argv)
// Print Final result in a file -------------------------
// Vehicle poses
int i = 0;
- Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
+ Eigen::VectorXd state_poses = Eigen::VectorXd::Zero(n_execution * 3);
for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
@@ -271,7 +291,7 @@ int main(int argc, char** argv)
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ Eigen::VectorXd landmarks = Eigen::VectorXd::Zero(problem.getMap()->getLandmarkList()->size() * 2);
for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
landmarks.segment(i, 2) = landmark->getP()->getVector();
diff --git a/src/examples/test_capture_laser_2D.cpp b/demos/demo_capture_laser_2d.cpp
similarity index 86%
rename from src/examples/test_capture_laser_2D.cpp
rename to demos/demo_capture_laser_2d.cpp
index cd5e40239fd42ca972a0e96405dc609ab707e9ca..756fd0eea126eb6451e9827e8cd86b79caf55531 100644
--- a/src/examples/test_capture_laser_2D.cpp
+++ b/demos/demo_capture_laser_2d.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
//std
#include <random>
@@ -17,7 +38,7 @@ int main(int argc, char *argv[])
std::cout << "========================================================" << std::endl;
//scan ranges
- Eigen::VectorXs ranges(720);
+ Eigen::VectorXd 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,
@@ -90,11 +111,11 @@ int main(int argc, char *argv[])
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);
+ Eigen::VectorXd 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;
+ std::list<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > corner_list;
//Create Device objects
//SensorLaser2D device(device_pose, ranges.size(), M_PI, 0.2, 30.0, 0.01);
@@ -103,7 +124,7 @@ int main(int argc, char *argv[])
//init a noise generator
std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise
+ std::normal_distribution<double> distribution_range(0.,device.getRangeStdDev()); //odometry noise
//Create a Capture object
CaptureLaser2D capture(time_stamp, &device, ranges);
diff --git a/src/examples/test_ceres_2_lasers.cpp b/demos/demo_ceres_2_lasers.cpp
similarity index 86%
rename from src/examples/test_ceres_2_lasers.cpp
rename to demos/demo_ceres_2_lasers.cpp
index b090a8a5a4acf010959c4e53bf006855326a5629..05ec4631f9c20f4b0a3a0bc93400272d781c00db 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/demos/demo_ceres_2_lasers.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
//std includes
#include "core/sensor/sensor_GPS_fix.h"
#include <cstdlib>
@@ -48,10 +69,10 @@ class FaramoticsRobot
string modelFileName;
CrangeScan2D* myScanner;
CdynamicSceneRender* myRender;
- Eigen::Vector3s ground_truth_pose_;
- Eigen::Vector4s laser_1_pose_, laser_2_pose_;
+ Eigen::Vector3d ground_truth_pose_;
+ Eigen::Vector4d laser_1_pose_, laser_2_pose_;
- FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
+ FaramoticsRobot(int argc, char** argv, const Eigen::Vector4d& _laser_1_pose, const Eigen::Vector4d& _laser_2_pose) :
modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
laser_1_pose_(_laser_1_pose),
laser_2_pose_(_laser_2_pose)
@@ -72,7 +93,7 @@ class FaramoticsRobot
}
//function travel around
- Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
+ Eigen::Vector3d 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; }
@@ -126,7 +147,7 @@ class FaramoticsRobot
}
}
- void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
+ void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3d& estimated_pose)
{
// detected corners
//std::cout << " drawCorners: " << feature_list.size() << std::endl;
@@ -146,7 +167,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -195,27 +216,27 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
- Scalar odom_std_factor = 0.5;
- Scalar gps_std = 1;
+ double odom_std_factor = 0.5;
+ double 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
+ std::normal_distribution<double> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<double> 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);
+ Eigen::Vector2d odom_data;
+ Eigen::Vector2d gps_fix_reading;
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::Vector3d ground_truth_pose; //last true pose
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
clock_t t1, t2;
- Scalar dt = 0.05;
+ double 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
+ 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
@@ -236,7 +257,7 @@ int main(int argc, char** argv)
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);
+ CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2d::Identity() * odom_std_factor * odom_std_factor, nullptr);
// Simulated robot
FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
@@ -247,25 +268,25 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame
- FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+ 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);
+ CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3d::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;
+ SolverCeres ceres_manager(&problem);
+ ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false;
+ // ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS;
+ // ceres_manager.getSolverOptions().max_num_iterations = 100;
google::InitGoogleLogging(argv[0]);
- CeresManager ceres_manager(&problem, ceres_options);
+ SolverCeres ceres_manager(&problem, ceres_options);
std::ofstream log_file, landmark_file; //output file
//std::cout << "START TRAJECTORY..." << std::endl;
@@ -313,7 +334,7 @@ int main(int argc, char** argv)
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)));
+ //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXd::Identity(3,3)));
}
mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
@@ -366,7 +387,7 @@ int main(int argc, char** argv)
// Print Final result in a file -------------------------
// Vehicle poses
int i = 0;
- Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
+ Eigen::VectorXd state_poses = Eigen::VectorXd::Zero(n_execution * 3);
for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
@@ -375,7 +396,7 @@ int main(int argc, char** argv)
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ Eigen::VectorXd landmarks = Eigen::VectorXd::Zero(problem.getMap()->getLandmarkList()->size() * 2);
for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
landmarks.segment(i, 2) = landmark->getP()->getVector();
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/demos/demo_ceres_2_lasers_polylines.cpp
similarity index 85%
rename from src/examples/test_ceres_2_lasers_polylines.cpp
rename to demos/demo_ceres_2_lasers_polylines.cpp
index 7e83dec251a85252b14d1014114a1089c912ba27..8dad5d18bace977ea250ec0664a8693b93620ffc 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/demos/demo_ceres_2_lasers_polylines.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
//std includes
#include "core/sensor/sensor_GPS_fix.h"
#include <cstdlib>
@@ -48,10 +69,10 @@ class FaramoticsRobot
string modelFileName;
CrangeScan2D* myScanner;
CdynamicSceneRender* myRender;
- Eigen::Vector3s ground_truth_pose_;
- Eigen::Vector4s laser_1_pose_, laser_2_pose_;
+ Eigen::Vector3d ground_truth_pose_;
+ Eigen::Vector4d laser_1_pose_, laser_2_pose_;
- FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
+ FaramoticsRobot(int argc, char** argv, const Eigen::Vector4d& _laser_1_pose, const Eigen::Vector4d& _laser_2_pose) :
modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
laser_1_pose_(_laser_1_pose),
laser_2_pose_(_laser_2_pose)
@@ -72,7 +93,7 @@ class FaramoticsRobot
}
//function travel around
- Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
+ Eigen::Vector3d 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; }
@@ -126,7 +147,7 @@ class FaramoticsRobot
}
}
- void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
+ void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3d& estimated_pose)
{
// detected corners
std::cout << " drawCorners: " << feature_list.size() << std::endl;
@@ -146,7 +167,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -195,27 +216,27 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
- Scalar odom_std_factor = 0.5;
- Scalar gps_std = 1;
+ double odom_std_factor = 0.5;
+ double 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
+ std::normal_distribution<double> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<double> 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);
+ Eigen::Vector2d odom_data;
+ Eigen::Vector2d gps_fix_reading;
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::Vector3d ground_truth_pose; //last true pose
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
clock_t t1, t2;
- Scalar dt = 0.05;
+ double 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
+ 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
@@ -243,7 +264,7 @@ int main(int argc, char** argv)
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);
+ CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2d::Identity() * odom_std_factor * odom_std_factor, nullptr);
// Simulated robot
FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
@@ -254,25 +275,25 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame
- FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+ 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);
+ CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3d::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;
+ SolverCeres ceres_manager(&problem);
+ ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false;
+ // ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS;
+ // ceres_manager.getSolverOptions().max_num_iterations = 100;
google::InitGoogleLogging(argv[0]);
- CeresManager ceres_manager(&problem, ceres_options);
+ SolverCeres ceres_manager(&problem, ceres_options);
std::ofstream log_file, landmark_file; //output file
//std::cout << "START TRAJECTORY..." << std::endl;
@@ -320,7 +341,7 @@ int main(int argc, char** argv)
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)));
+ //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXd::Identity(3,3)));
}
mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
diff --git a/include/laser/capture/capture_laser_2D.h b/include/laser/capture/capture_laser_2D.h
deleted file mode 100644
index 03faa523dec2a7290aa05b448a80c9dedf28d136..0000000000000000000000000000000000000000
--- a/include/laser/capture/capture_laser_2D.h
+++ /dev/null
@@ -1,52 +0,0 @@
-
-#ifndef CAPTURE_LASER_2D_H_
-#define CAPTURE_LASER_2D_H_
-
-//wolf forward declarations
-namespace wolf{
-class SensorLaser2D;
-}
-
-//wolf includes
-#include "core/capture/capture_base.h"
-#include "laser/sensor/sensor_laser_2D.h"
-
-//laserscanutils includes
-#include "laser_scan_utils/laser_scan.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureLaser2D);
-
-class CaptureLaser2D : public CaptureBase
-{
- public:
- /** \brief Constructor with ranges
- **/
- CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges);
- virtual ~CaptureLaser2D() = default;
-
- laserscanutils::LaserScan& getScan();
-
- void setSensor(const SensorBasePtr sensor_ptr);
-
- private:
- SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
- laserscanutils::LaserScan scan_;
-
-};
-
-inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
-{
- return scan_;
-}
-
-inline void CaptureLaser2D::setSensor(const SensorBasePtr sensor_ptr)
-{
- CaptureBase::setSensor(sensor_ptr);
- laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr);
-}
-
-} // namespace wolf
-
-#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/include/laser/capture/capture_laser_2d.h b/include/laser/capture/capture_laser_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..579f4475b3e2bd3a59fd41c4fb3fb9dc24db883a
--- /dev/null
+++ b/include/laser/capture/capture_laser_2d.h
@@ -0,0 +1,73 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#ifndef CAPTURE_LASER_2d_H_
+#define CAPTURE_LASER_2d_H_
+
+//wolf forward declarations
+namespace wolf{
+class SensorLaser2d;
+}
+
+//wolf includes
+#include "laser/sensor/sensor_laser_2d.h"
+#include "core/capture/capture_base.h"
+
+//laserscanutils includes
+#include "laser_scan_utils/laser_scan.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureLaser2d);
+
+class CaptureLaser2d : public CaptureBase
+{
+ public:
+ /** \brief Constructor with ranges
+ **/
+ CaptureLaser2d(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges);
+ ~CaptureLaser2d() override = default;
+
+ laserscanutils::LaserScan& getScan();
+
+ void setSensor(const SensorBasePtr sensor_ptr) override;
+
+ private:
+ SensorLaser2dPtr laser_ptr_; //specific pointer to sensor laser 2d object
+ laserscanutils::LaserScan scan_;
+
+};
+
+inline laserscanutils::LaserScan& CaptureLaser2d::getScan()
+{
+ return scan_;
+}
+
+inline void CaptureLaser2d::setSensor(const SensorBasePtr sensor_ptr)
+{
+ CaptureBase::setSensor(sensor_ptr);
+ laser_ptr_ = std::static_pointer_cast<SensorLaser2d>(sensor_ptr);
+}
+
+} // namespace wolf
+
+#endif /* CAPTURE_LASER_2d_H_ */
diff --git a/include/laser/factor/factor_point_2D.h b/include/laser/factor/factor_point_2D.h
deleted file mode 100644
index 5d8525a4a902a4d124d065779c17fd99942af3a4..0000000000000000000000000000000000000000
--- a/include/laser/factor/factor_point_2D.h
+++ /dev/null
@@ -1,140 +0,0 @@
-#ifndef FACTOR_POINT_2D_THETA_H_
-#define FACTOR_POINT_2D_THETA_H_
-
-//Wolf includes
-#include "core/factor/factor_autodiff.h"
-#include "laser/feature/feature_polyline_2D.h"
-#include "laser/landmark/landmark_polyline_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPoint2D);
-
-/**
- * @brief The FactorPoint2D class
- */
-class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
-{
- protected:
- unsigned int feature_point_id_;
- int landmark_point_id_;
- StateBlockPtr point_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
-
- public:
-
- FactorPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
- feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
- {
- //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
- //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
- }
-
- virtual ~FactorPoint2D() = default;
-
- /**
- * @brief getLandmarkPtr
- * @return
- */
- LandmarkPolyline2DPtr getLandmark()
- {
- return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
- }
-
- /**
- * @brief getLandmarkPointId
- * @return
- */
- int getLandmarkPointId()
- {
- return landmark_point_id_;
- }
-
- /**
- * @brief getFeaturePointId
- * @return
- */
- unsigned int getFeaturePointId()
- {
- return feature_point_id_;
- }
-
- /**
- * @brief getLandmarkPointPtr
- * @return
- */
- StateBlockPtr getLandmarkPoint()
- {
- return point_state_ptr_;
- }
-
- /**
- *
- */
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXs& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
-};
-
-template<typename T>
-inline bool FactorPoint2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const
-{
- //std::cout << "FactorPointToLine2D::operator" << std::endl;
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,2,1>> residuals_map(_residuals);
-
- // Landmark point global position
- Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginO) * landmark_position_map;
-
- // sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_point - robot_position_map) - sensor_position);
-
- // Residuals
- residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_measurement_position - getMeasurement().head<2>().cast<T>());
-
- //std::cout << "residuals_map" << residuals_map[0] << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..30b4884d08fa0874c97722f47f0804b3b0e3b3c4
--- /dev/null
+++ b/include/laser/factor/factor_point_2d.h
@@ -0,0 +1,161 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_POINT_2d_THETA_H_
+#define FACTOR_POINT_2d_THETA_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "laser/feature/feature_polyline_2d.h"
+#include "laser/landmark/landmark_polyline_2d.h"
+#include "core/math/covariance.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPoint2d);
+
+/**
+ * @brief The FactorPoint2d class
+ */
+class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
+{
+ protected:
+ unsigned int feature_point_id_;
+ int landmark_point_id_;
+ StateBlockPtr point_state_ptr_;
+
+ public:
+
+ FactorPoint2d(const FeaturePolyline2dPtr& _ftr_ptr,
+ const LandmarkPolyline2dPtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ unsigned int _ftr_point_id,
+ int _lmk_point_id,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPoint2d,2,2,1,2,1,2>("FactorPoint2d",
+ TOP_LMK,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO(),
+ _lmk_ptr->getPointStateBlock(_lmk_point_id)),
+ feature_point_id_(_ftr_point_id),
+ landmark_point_id_(_lmk_point_id),
+ point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id))
+ {
+ //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
+ //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
+
+ auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
+
+ // measurement
+ measurement_ = ftr->getPoints().col(feature_point_id_);
+
+ // covariance (ensuring symmetry)
+ Eigen::Matrix2d measurement_covariance = ftr->getPointsCov().middleCols(feature_point_id_*2,2).selfadjointView<Eigen::Upper>();
+
+ // square root information upper matrix
+ measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance.inverse());
+ }
+
+ ~FactorPoint2d() override = default;
+
+ /**
+ * @brief getLandmarkPtr
+ * @return
+ */
+ LandmarkPolyline2dPtr getLandmark()
+ {
+ return std::static_pointer_cast<LandmarkPolyline2d>(getLandmarkOther());
+ }
+
+ /**
+ * @brief getLandmarkPointId
+ * @return
+ */
+ int getLandmarkPointId()
+ {
+ return landmark_point_id_;
+ }
+
+ /**
+ * @brief getFeaturePointId
+ * @return
+ */
+ unsigned int getFeaturePointId()
+ {
+ return feature_point_id_;
+ }
+
+ /**
+ * @brief getLandmarkPointPtr
+ * @return
+ */
+ StateBlockPtr getLandmarkPoint()
+ {
+ return point_state_ptr_;
+ }
+
+ /**
+ *
+ */
+ template <typename T>
+ bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorPoint2d::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const
+{
+ //std::cout << "FactorPointToLine2d::operator" << std::endl;
+ // Mapping
+ Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginP);
+ Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
+ Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
+ Eigen::Map<Eigen::Matrix<T,2,1>> residuals_map(_residuals);
+
+ // Landmark point global position
+ Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginO) * landmark_position_map;
+
+ // sensor transformation
+ Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
+ // robot transformation
+ Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
+
+ // Expected measurement
+ Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_point - robot_position_map) - sensor_position);
+
+ // Residuals
+ residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_measurement_position - getMeasurement().head<2>().cast<T>());
+
+ //std::cout << "residuals_map" << residuals_map[0] << std::endl;
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/laser/factor/factor_point_to_line_2D.h b/include/laser/factor/factor_point_to_line_2d.h
similarity index 51%
rename from include/laser/factor/factor_point_to_line_2D.h
rename to include/laser/factor/factor_point_to_line_2d.h
index 561526c67af7f4b5d2a27671f26c1f4289c6d508..cc975d802a098f3dc610bb5b185e5184e489c69c 100644
--- a/include/laser/factor/factor_point_to_line_2D.h
+++ b/include/laser/factor/factor_point_to_line_2d.h
@@ -1,17 +1,39 @@
-#ifndef FACTOR_POINT_TO_LINE_2D_H_
-#define FACTOR_POINT_TO_LINE_2D_H_
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_POINT_TO_LINE_2d_H_
+#define FACTOR_POINT_TO_LINE_2d_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
-#include "laser/feature/feature_polyline_2D.h"
-#include "laser/landmark/landmark_polyline_2D.h"
+#include "laser/feature/feature_polyline_2d.h"
+#include "laser/landmark/landmark_polyline_2d.h"
+#include "core/math/covariance.h"
namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPointToLine2D);
-
+
+WOLF_PTR_TYPEDEFS(FactorPointToLine2d);
+
//class
-class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>
+class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>
{
protected:
int landmark_point_id_;
@@ -19,34 +41,59 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
unsigned int feature_point_id_;
StateBlockPtr point_state_ptr_;
StateBlockPtr point_aux_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
+ Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
public:
- FactorPointToLine2D(const FeaturePolyline2DPtr& _ftr_ptr,
- const LandmarkPolyline2DPtr& _lmk_ptr,
+ FactorPointToLine2d(const FeaturePolyline2dPtr& _ftr_ptr,
+ const LandmarkPolyline2dPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
- landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>("FactorPointToLine2d",
+ TOP_LMK,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO(),
+ _lmk_ptr->getPointStateBlock(_lmk_point_id),
+ _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
+ landmark_point_id_(_lmk_point_id),
+ landmark_point_aux_id_(_lmk_point_aux_id),
+ feature_point_id_(_ftr_point_id),
+ point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)),
+ point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id))
{
- //std::cout << "FactorPointToLine2D" << std::endl;
+ //std::cout << "FactorPointToLine2d" << std::endl;
//std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
assert(_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_));
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
+
+ // cast
+ auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
+
+ // measurement
+ measurement_ = ftr->getPoints().col(feature_point_id_);
+
+ // covariance (ensuring symmetry)
+ measurement_covariance_ = ftr->getPointsCov().middleCols(feature_point_id_*2,2).selfadjointView<Eigen::Upper>();
+
+ // square root information upper matrix
+ measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
}
- virtual ~FactorPointToLine2D() = default;
+ ~FactorPointToLine2d() override = default;
- LandmarkPolyline2DPtr getLandmark()
+ LandmarkPolyline2dPtr getLandmark()
{
- return std::static_pointer_cast<LandmarkPolyline2D>( landmark_other_ptr_.lock() );
+ return std::static_pointer_cast<LandmarkPolyline2d>( getLandmarkOther() );
}
int getLandmarkPointId()
@@ -76,33 +123,12 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
template <typename T>
bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXs& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
};
template<typename T>
-inline bool FactorPointToLine2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const
+inline bool FactorPointToLine2d::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const
{
- //std::cout << "FactorPointToLine2D::operator" << std::endl;
+ //std::cout << "FactorPointToLine2d::operator" << std::endl;
// Mapping
Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginPosition);
Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
diff --git a/include/laser/feature/feature_icp_align.h b/include/laser/feature/feature_icp_align.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c292c3ddba48e852cba83dbde21fc14a3a93a6a
--- /dev/null
+++ b/include/laser/feature/feature_icp_align.h
@@ -0,0 +1,77 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FEATURE_ICP_ALIGN_H_
+#define FEATURE_ICP_ALIGN_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/feature/feature_base.h"
+#include "laser_scan_utils/icp.h"
+
+
+//std includes
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FeatureICPAlign);
+
+//class FeatureOdom2d
+class FeatureICPAlign : public FeatureBase
+{
+ public:
+
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ FeatureICPAlign(const laserscanutils::icpOutput& _icp_align_res);
+
+ ~FeatureICPAlign() override;
+
+ /** \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();
+
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ const laserscanutils::icpOutput& getQualityAlign() const;
+
+ Eigen::Vector3s getTransf();
+
+ private:
+ laserscanutils::icpOutput icp_align_res_;
+
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/laser/feature/feature_match_polyline_2D.h b/include/laser/feature/feature_match_polyline_2D.h
deleted file mode 100644
index cbf21ad893abc55184014902d2352b184b7289e2..0000000000000000000000000000000000000000
--- a/include/laser/feature/feature_match_polyline_2D.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef FEATURE_MATCH_POLYLINE_2D_H_
-#define FEATURE_MATCH_POLYLINE_2D_H_
-
-// Wolf includes
-#include "core/common/wolf.h"
-#include "core/feature/feature_match.h"
-
-//wolf nampseace
-namespace wolf {
-
-//forward declaration to typedef class pointers
-WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatchPolyline2D);
-
-/** \brief Match between a feature_polyline_2D and a feature_polyline_2D
- *
- * Match between a feature_polyline_2D and a feature_polyline_2D (feature-feature correspondence)
- *
- */
-struct FeatureMatchPolyline2D : public FeatureMatch
-{
- int feature_last_from_id_;
- int feature_last_to_id_;
- int feature_incoming_from_id_;
- int feature_incoming_to_id_;
- Eigen::Matrix3s T_incoming_last_;
-};
-
-}//end namespace
-
-#endif
-
diff --git a/include/laser/feature/feature_match_polyline_2d.h b/include/laser/feature/feature_match_polyline_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8142b25477de8ace40e2c372264204e62e76238e
--- /dev/null
+++ b/include/laser/feature/feature_match_polyline_2d.h
@@ -0,0 +1,73 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FEATURE_MATCH_POLYLINE_2d_H_
+#define FEATURE_MATCH_POLYLINE_2d_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/feature/feature_match.h"
+
+//wolf nampseace
+namespace wolf {
+
+//forward declaration to typedef class pointers
+WOLF_PTR_TYPEDEFS(FeaturePolyline2d);
+WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatchPolyline2d);
+
+/** \brief Match between a feature_polyline_2d and a feature_polyline_2d
+ *
+ * Match between a feature_polyline_2d and a feature_polyline_2d (feature-feature correspondence)
+ *
+ */
+struct FeatureMatchPolyline2d : public FeatureMatch
+{
+ int feature_last_from_id_;
+ int feature_last_to_id_;
+ int feature_incoming_from_id_;
+ int feature_incoming_to_id_;
+ Eigen::Matrix3d T_incoming_last_;
+ FeaturePolyline2dPtr pl_incoming_;
+ FeaturePolyline2dPtr pl_last_;
+
+ bool check(bool check_partial_match = true) const;
+
+ /* compute square distances
+ *
+ * Returns an array containing the squared distances of each point of the match
+ */
+ Eigen::Array<double,Eigen::Dynamic,1> computeSqDistArray() const;
+
+ void print() const;
+
+ bool fromDefined() const;
+
+ bool toDefined() const;
+
+ unsigned int getNPoints() const;
+
+ unsigned int getNDefinedPoints() const;
+};
+
+}//end namespace
+
+#endif
+
diff --git a/include/laser/feature/feature_polyline_2D.h b/include/laser/feature/feature_polyline_2D.h
deleted file mode 100644
index 4dff63dcd8609768b7ea00d965921139be400301..0000000000000000000000000000000000000000
--- a/include/laser/feature/feature_polyline_2D.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * \file feature_polyline_2D.h
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#ifndef FEATURE_POLYLINE_2D_H_
-#define FEATURE_POLYLINE_2D_H_
-
-#include "core/feature/feature_base.h"
-
-namespace wolf
-{
-WOLF_PTR_TYPEDEFS(FeaturePolyline2D);
-WOLF_LIST_TYPEDEFS(FeaturePolyline2D);
-
-//class
-class FeaturePolyline2D : public FeatureBase
-{
- protected:
- Eigen::MatrixXs points_; // homogeneous 2D points (each column is a point)
- Eigen::MatrixXs points_cov_;
- bool first_defined_;
- bool last_defined_;
-
- public:
- FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined);
- virtual ~FeaturePolyline2D();
- const Eigen::MatrixXs& getPoints() const;
- const Eigen::MatrixXs& getPointsCov() const;
- bool isFirstDefined() const;
- bool isLastDefined() const;
- int getNPoints() const;
-};
-
-inline FeaturePolyline2D::FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined) :
- FeatureBase("POLYLINE 2D", Eigen::Vector1s::Zero(), Eigen::Vector1s::Ones()),
- points_(_points),
- points_cov_(_points_cov),
- first_defined_(_first_defined),
- last_defined_(_last_defined)
-{
- assert(points_.rows() == 3 && points_cov_.rows() == 2 && points_cov_.cols() == 2*points_.cols() && "FeaturePolyline2D::FeaturePolyline2D: Bad points or covariance matrix size");
-}
-
-inline FeaturePolyline2D::~FeaturePolyline2D()
-{
- //
-}
-
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPoints() const
-{
- return points_;
-}
-
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPointsCov() const
-{
- return points_cov_;
-}
-
-inline bool FeaturePolyline2D::isFirstDefined() const
-{
- return first_defined_;
-}
-
-inline bool FeaturePolyline2D::isLastDefined() const
-{
- return last_defined_;
-}
-
-inline int FeaturePolyline2D::getNPoints() const
-{
- return points_.cols();
-}
-
-} /* namespace wolf */
-
-#endif /* FEATURE_POLYLINE_2D_H_ */
diff --git a/include/laser/feature/feature_polyline_2d.h b/include/laser/feature/feature_polyline_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..b889c49b827b3f76f197b9f4fa7f801285cecf82
--- /dev/null
+++ b/include/laser/feature/feature_polyline_2d.h
@@ -0,0 +1,105 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file feature_polyline_2d.h
+ *
+ * Created on: May 26, 2016
+ * \author: jvallve
+ */
+
+#ifndef FEATURE_POLYLINE_2d_H_
+#define FEATURE_POLYLINE_2d_H_
+
+#include "core/feature/feature_base.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(FeaturePolyline2d);
+WOLF_LIST_TYPEDEFS(FeaturePolyline2d);
+
+//class
+class FeaturePolyline2d : public FeatureBase
+{
+ protected:
+ Eigen::MatrixXd points_; // homogeneous 2d points (each column is a point)
+ Eigen::MatrixXd points_cov_;
+ bool first_defined_;
+ bool last_defined_;
+
+ public:
+ FeaturePolyline2d(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined);
+ ~FeaturePolyline2d() override;
+ const Eigen::MatrixXd& getPoints() const;
+ const Eigen::MatrixXd& getPointsCov() const;
+ bool isFirstDefined() const;
+ bool isLastDefined() const;
+ int getNPoints() const;
+};
+
+//<<<<<<< HEAD
+inline FeaturePolyline2d::FeaturePolyline2d(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
+ FeatureBase("FeaturePolyline2d", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
+//=======
+//inline FeaturePolyline2d::FeaturePolyline2d(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
+// FeatureBase("POLYLINE 2d", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
+//>>>>>>> 8-replace-scalar-to-double
+ points_(_points),
+ points_cov_(_points_cov),
+ first_defined_(_first_defined),
+ last_defined_(_last_defined)
+{
+ assert(points_.rows() == 3 && points_cov_.rows() == 2 && points_cov_.cols() == 2*points_.cols() && "FeaturePolyline2d::FeaturePolyline2d: Bad points or covariance matrix size");
+}
+
+inline FeaturePolyline2d::~FeaturePolyline2d()
+{
+ //
+}
+
+inline const Eigen::MatrixXd& FeaturePolyline2d::getPoints() const
+{
+ return points_;
+}
+
+inline const Eigen::MatrixXd& FeaturePolyline2d::getPointsCov() const
+{
+ return points_cov_;
+}
+
+inline bool FeaturePolyline2d::isFirstDefined() const
+{
+ return first_defined_;
+}
+
+inline bool FeaturePolyline2d::isLastDefined() const
+{
+ return last_defined_;
+}
+
+inline int FeaturePolyline2d::getNPoints() const
+{
+ return points_.cols();
+}
+
+} /* namespace wolf */
+
+#endif /* FEATURE_POLYLINE_2d_H_ */
diff --git a/include/laser/feature/feature_scene_falko.h b/include/laser/feature/feature_scene_falko.h
new file mode 100644
index 0000000000000000000000000000000000000000..c571db55da8177dadbd4e63818317ce2271a76d3
--- /dev/null
+++ b/include/laser/feature/feature_scene_falko.h
@@ -0,0 +1,77 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FEATURE_SCENE_FALKO_H_
+#define FEATURE_SCENE_FALKO_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/feature/feature_base.h"
+#include "laser_scan_utils/scene_falko.h"
+
+// std includes
+
+namespace wolf {
+
+template <typename D> class FeatureSceneFalko : public FeatureBase
+{
+ public:
+ FeatureSceneFalko(const std::shared_ptr<laserscanutils::SceneFalko<D>> _falko_scene)
+ : FeatureBase("FeatureSceneFalko", Eigen::VectorXd::Zero(1), Eigen::MatrixXd::Ones(1, 1))
+ {
+ scene_ = _falko_scene;
+ }
+
+ ~FeatureSceneFalko() override{};
+
+ std::shared_ptr<laserscanutils::SceneFalko<D>> getScene() { return scene_; }
+
+ private:
+ std::shared_ptr<laserscanutils::SceneFalko<D>> scene_;
+};
+
+WOLF_PTR_TYPEDEFS(FeatureSceneFalkoBsc);
+
+class FeatureSceneFalkoBsc : public FeatureSceneFalko<laserscanutils::bsc>
+{
+ public:
+ FeatureSceneFalkoBsc(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::bsc>> _falko_scene)
+ : FeatureSceneFalko(_falko_scene)
+ {
+ }
+ ~FeatureSceneFalkoBsc();
+};
+
+WOLF_PTR_TYPEDEFS(FeatureSceneFalkoCgh);
+
+class FeatureSceneFalkoCgh : public FeatureSceneFalko<laserscanutils::cgh>
+{
+ public:
+ FeatureSceneFalkoCgh(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::cgh>> _falko_scene)
+ : FeatureSceneFalko(_falko_scene)
+ {
+ }
+ ~FeatureSceneFalkoCgh() override{};
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/laser/landmark/landmark_match_polyline_2D.h b/include/laser/landmark/landmark_match_polyline_2D.h
deleted file mode 100644
index 4e846418e5aae970f93d62def47a7f96a15ff0af..0000000000000000000000000000000000000000
--- a/include/laser/landmark/landmark_match_polyline_2D.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef LANDMARK_MATCH_POLYLINE_2D_H_
-#define LANDMARK_MATCH_POLYLINE_2D_H_
-
-// Wolf includes
-#include "core/common/wolf.h"
-#include "core/landmark/landmark_match.h"
-
-//wolf nampseace
-namespace wolf {
-
-// Map of Feature - Landmark matches
-WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatchPolyline2D);
-typedef std::map<FeatureBasePtr, LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DMap;
-
-/** \brief Match between a feature_polyline_2D and a landmark_polyline_2D
- *
- * Match between a feature_polyline_2D and a landmark_polyline_2D
- *
- **/
-struct LandmarkMatchPolyline2D : public LandmarkMatch
-{
- int landmark_from_id_;
- int feature_from_id_;
- int landmark_to_id_;
- int feature_to_id_;
- int landmark_version_;
- Eigen::Matrix3s T_feature_landmark_;
-
- bool check() const;
-};
-
-}//end namespace
-
-#endif
diff --git a/include/laser/landmark/landmark_match_polyline_2d.h b/include/laser/landmark/landmark_match_polyline_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..d5d10114faa6c9865f3ca75166f1f5f92fbc0ab2
--- /dev/null
+++ b/include/laser/landmark/landmark_match_polyline_2d.h
@@ -0,0 +1,89 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef LANDMARK_MATCH_POLYLINE_2d_H_
+#define LANDMARK_MATCH_POLYLINE_2d_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/landmark/landmark_match.h"
+
+//wolf nampseace
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FeaturePolyline2d);
+WOLF_PTR_TYPEDEFS(LandmarkPolyline2d);
+
+// Map of Feature - Landmark matches
+WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatchPolyline2d);
+typedef std::map<FeatureBasePtr, LandmarkMatchPolyline2dPtr> LandmarkMatchPolyline2dMap;
+
+/** \brief Match between a feature_polyline_2d and a landmark_polyline_2d
+ *
+ * Match between a feature_polyline_2d and a landmark_polyline_2d
+ *
+ **/
+struct LandmarkMatchPolyline2d : public LandmarkMatch
+{
+ int landmark_from_id_;
+ int feature_from_id_;
+ int landmark_to_id_;
+ int feature_to_id_;
+ int landmark_version_;
+ Eigen::Matrix3d T_feature_landmark_;
+ FeaturePolyline2dPtr pl_feature_;
+ LandmarkPolyline2dPtr pl_landmark_;
+
+ bool check(bool check_partial_match = true) const;
+
+ /* is a partial match
+ *
+ * Returns if there are not matched points for both landmark and feature in any or both extremes
+ */
+ bool isPartialFront() const;
+ bool isPartialBack() const;
+
+ /* is a deprecated match
+ *
+ * Returns if the landmark version is newer than when matched or if landmark was merged in another landmark
+ */
+ bool isDeprecated() const;
+
+ /* compute square distances
+ *
+ * Returns an array containing the squared distances of each point of the match
+ */
+ Eigen::Array<double,Eigen::Dynamic,1> computeSqDistArray() const;
+
+ void print() const;
+
+ bool fromDefined() const;
+
+ bool toDefined() const;
+
+ unsigned int getNPoints() const;
+
+ unsigned int getNDefinedPoints() const;
+};
+
+}//end namespace
+
+#endif
diff --git a/include/laser/landmark/landmark_polyline_2D.h b/include/laser/landmark/landmark_polyline_2d.h
similarity index 60%
rename from include/laser/landmark/landmark_polyline_2D.h
rename to include/laser/landmark/landmark_polyline_2d.h
index 16dcdb9c5a65d4aee4a5d0fd9486312e68b4a5d6..4d1574cf3bb765c674435ba65cd2afb1c632b584 100644
--- a/include/laser/landmark/landmark_polyline_2D.h
+++ b/include/laser/landmark/landmark_polyline_2d.h
@@ -1,19 +1,37 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
/**
- * \file landmark_poliyline_2D.h
+ * \file landmark_poliyline_2d.h
*
* Created on: May 26, 2016
* \author: jvallve
*/
-#ifndef LANDMARK_POLYLINE_2D_H_
-#define LANDMARK_POLYLINE_2D_H_
+#ifndef LANDMARK_POLYLINE_2d_H_
+#define LANDMARK_POLYLINE_2d_H_
// Wolf
#include "core/landmark/landmark_base.h"
-#include "laser/processor/polyline_2D_utils.h"
-
-// STL
-#include <deque>
+#include "laser/processor/polyline_2d_utils.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
@@ -34,9 +52,9 @@ typedef enum
struct PolylineRectangularClass
{
PolylineClassType type; // name of the type
- Scalar L; // long side length
- Scalar W; // short side length
- Scalar D; // diagonal length
+ double L; // long side length
+ double W; // short side length
+ double D; // diagonal length
PolylineRectangularClass() :
type(UNCLASSIFIED),
@@ -46,7 +64,7 @@ struct PolylineRectangularClass
{
};
- PolylineRectangularClass(const PolylineClassType& _type, const Scalar& _L, const Scalar& _W) :
+ PolylineRectangularClass(const PolylineClassType& _type, const double& _L, const double& _W) :
type(_type),
L(_L),
W(_W),
@@ -67,22 +85,20 @@ struct PolylineClassPallet : public PolylineRectangularClass
PolylineClassPallet() : PolylineRectangularClass(PALLET, 1.2, 0.9){};
};
-WOLF_PTR_TYPEDEFS(LandmarkPolyline2D);
-WOLF_LIST_TYPEDEFS(LandmarkPolyline2D);
+WOLF_PTR_TYPEDEFS(LandmarkPolyline2d);
+WOLF_LIST_TYPEDEFS(LandmarkPolyline2d);
//class
-class LandmarkPolyline2D : public LandmarkBase
+class LandmarkPolyline2d : public LandmarkBase
{
protected:
std::map<int,StateBlockPtr> point_state_ptr_map_; ///< polyline points state blocks
bool first_defined_; ///< Wether the first point is an extreme of a line or the line may continue
bool last_defined_; ///< Wether the last point is an extreme of a line or the line may continue
- int first_id_;
- int last_id_;
bool closed_; ///< Wether the polyline is closed or not
PolylineRectangularClass classification_; ///< The classification of the landmark
int version_; ///< Integer increased each time a modification in landmark occurs (added points, merged points, closed, classified)
- LandmarkPolyline2DPtr merged_in_lmk_;
+ LandmarkPolyline2dPtr merged_in_lmk_;
private:
@@ -90,9 +106,9 @@ class LandmarkPolyline2D : public LandmarkBase
StateBlockPtr& lastStateBlock();
public:
- LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0);
- LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class);
- virtual ~LandmarkPolyline2D();
+ LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0);
+ LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class);
+ ~LandmarkPolyline2d() override;
/** \brief Gets a const reference to the point state block pointer vector
**/
@@ -110,11 +126,11 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Get the landmark pointer in which was merged (nullptr in case of no merged)
**/
- LandmarkPolyline2DPtr getMergedInLandmark() const;
+ LandmarkPolyline2dPtr getMergedInLandmark() const;
/** \brief Set the landmark pointer in which was merged
**/
- void setMergedInLandmark(const LandmarkPolyline2DPtr& lmk_ptr);
+ void setMergedInLandmark(const LandmarkPolyline2dPtr& lmk_ptr);
/** \brief Gets whether the given state block point is defined or not (assumes the state block is in the landmark)
**/
@@ -122,12 +138,13 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Sets the first/last extreme point
**/
- void setFirst(const Eigen::VectorXs& _point, bool _defined);
- void setLast(const Eigen::VectorXs& _point, bool _defined);
+ void setFirst(const Eigen::VectorXd& _point, bool _defined);
+ void setLast(const Eigen::VectorXd& _point, bool _defined);
int getNPoints() const;
int getNDefinedPoints() const;
std::vector<int> getValidIds() const;
+ int id2idx(int) const;
int getFirstId() const;
int getLastId() const;
bool isValidId(const int& i) const;
@@ -135,10 +152,11 @@ class LandmarkPolyline2D : public LandmarkBase
int getPrevValidId(const int& i) const;
int getVersion() const;
- const Eigen::VectorXs getPointVector(int _i) const;
+ const Eigen::VectorXd getPointVector(int _i) const;
- Eigen::MatrixXs computePointsGlobal() const;
- Eigen::MatrixXs computePointsCovGlobal() const;
+ Eigen::MatrixXd computePointsGlobal() const;
+ Eigen::MatrixXd computePointsGlobal(const int& _from_id, const int& _to_id) const;
+ Eigen::MatrixXd computePointsCovGlobal() const;
StateBlockPtr getPointStateBlock(int _i);
@@ -151,7 +169,7 @@ class LandmarkPolyline2D : public LandmarkBase
* \param _extreme: if its extreme or not
* \param _back: if it have to be added in the back (true) or in the front (false)
**/
- void addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back);
+ void addPoint(const Eigen::VectorXd& _point, const bool& _defined, const bool& _back);
/** \brief Adds new points to the landmark
* \param _points: a matrix containing points, some of them to be added
@@ -159,7 +177,7 @@ class LandmarkPolyline2D : public LandmarkBase
* \param _extreme: if last point to be added is extreme or not
* \param _back: if the points have to be added in the back (true) or in the front (false)
**/
- void addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined, const bool& _back);
+ void addPoints(const Eigen::MatrixXd& _points, const unsigned int& _idx, const bool& _defined, const bool& _back);
/** \brief define extreme point
**/
@@ -175,7 +193,7 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Try to close the polyline (if it has overlapped points)
**/
- virtual bool tryClose(const Scalar& _dist_tol);
+ virtual bool tryClose(const double& _dist_tol);
/** \brief Set the polyline as closed
**/
@@ -191,11 +209,11 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Try to classify polyline as a known object
**/
- bool tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes);
+ bool tryClassify(const double& _dist_tol, std::vector<PolylineRectangularClass> _classes);
/** \brief Try to classify polyline as a known object
**/
- void mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, const int& _from, const int& _to, const int& _merged_from, const int& _merged_to);
+ void mergeLandmark(const LandmarkPolyline2dPtr _merged_lmk, const int& _from, const int& _to, const int& _merged_from, const int& _merged_to);
/** \brief get classification
**/
@@ -203,54 +221,61 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
**/
- virtual void registerNewStateBlocks();
+ virtual void registerNewStateBlocks() const;
virtual void removeStateBlocks();
/** Factory method to create new landmarks from YAML nodes
*/
static LandmarkBasePtr create(const YAML::Node& _lmk_node);
- YAML::Node saveToYaml() const;
+ YAML::Node saveToYaml() const override;
- static void tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol);
+ static void tryMergeLandmarks(LandmarkPolyline2dPtrList& _lmk_list,
+ const double& max_sq_error,
+ const double& min_overlapping_dist,
+ const int& min_N_overlapped,
+ const int& min_N_defined_overlapped);
};
-inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock()
+// STL
+#include <iterator>
+
+inline StateBlockPtr& LandmarkPolyline2d::firstStateBlock()
{
return point_state_ptr_map_.begin()->second;
}
-inline StateBlockPtr& LandmarkPolyline2D::lastStateBlock()
+inline StateBlockPtr& LandmarkPolyline2d::lastStateBlock()
{
return point_state_ptr_map_.rbegin()->second;
}
-inline std::map<int,StateBlockPtr>& LandmarkPolyline2D::getPointStatePtrMap()
+inline std::map<int,StateBlockPtr>& LandmarkPolyline2d::getPointStatePtrMap()
{
return point_state_ptr_map_;
}
-inline bool LandmarkPolyline2D::isFirstDefined() const
+inline bool LandmarkPolyline2d::isFirstDefined() const
{
return first_defined_;
}
-inline bool LandmarkPolyline2D::isLastDefined() const
+inline bool LandmarkPolyline2d::isLastDefined() const
{
return last_defined_;
}
-inline bool LandmarkPolyline2D::isValidId(const int& i) const
+inline bool LandmarkPolyline2d::isValidId(const int& i) const
{
- return point_state_ptr_map_.find(i) != point_state_ptr_map_.end();
+ return point_state_ptr_map_.count(i) != 0;
};
-inline bool LandmarkPolyline2D::isClosed() const
+inline bool LandmarkPolyline2d::isClosed() const
{
return closed_;
}
-inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const
+inline LandmarkPolyline2dPtr LandmarkPolyline2d::getMergedInLandmark() const
{
// recursive call
if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
@@ -258,7 +283,7 @@ inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const
return merged_in_lmk_;
}
-inline bool LandmarkPolyline2D::isDefined(StateBlockPtr _state_block) const
+inline bool LandmarkPolyline2d::isDefined(StateBlockPtr _state_block) const
{
if (_state_block == point_state_ptr_map_.begin()->second)
return first_defined_;
@@ -269,17 +294,17 @@ inline bool LandmarkPolyline2D::isDefined(StateBlockPtr _state_block) const
return true;
}
-inline int LandmarkPolyline2D::getNPoints() const
+inline int LandmarkPolyline2d::getNPoints() const
{
return (int)point_state_ptr_map_.size();
}
-inline int LandmarkPolyline2D::getNDefinedPoints() const
+inline int LandmarkPolyline2d::getNDefinedPoints() const
{
return (int)point_state_ptr_map_.size() - (first_defined_ ? 0 : 1) - (last_defined_ ? 0 : 1);
}
-inline std::vector<int> LandmarkPolyline2D::getValidIds() const
+inline std::vector<int> LandmarkPolyline2d::getValidIds() const
{
std::vector<int> valid_ids;
for (auto st_pair : point_state_ptr_map_)
@@ -288,41 +313,49 @@ inline std::vector<int> LandmarkPolyline2D::getValidIds() const
return valid_ids;
}
-inline int LandmarkPolyline2D::getFirstId() const {
+inline int LandmarkPolyline2d::id2idx(int id) const
+{
+ assert(isValidId(id));
+ return std::distance(point_state_ptr_map_.begin(),point_state_ptr_map_.find(id));
+}
+
+inline int LandmarkPolyline2d::getFirstId() const
+{
return point_state_ptr_map_.begin()->first;
}
-inline int LandmarkPolyline2D::getLastId() const {
+inline int LandmarkPolyline2d::getLastId() const
+{
return point_state_ptr_map_.rbegin()->first;
}
-inline int LandmarkPolyline2D::getNextValidId(const int& i) const
+inline int LandmarkPolyline2d::getNextValidId(const int& i) const
{
- assert(!(i == last_id_ && !closed_) && "Calling getNextValidId of last_id in an open landmark");
+ assert(!(i == getLastId() && !closed_) && "Calling getNextValidId of last_id in an open landmark");
assert(isValidId(i) && "Calling getNextValidId of an invalid id");
- if (i == last_id_ && closed_)
- return first_id_;
+ if (i == getLastId() && closed_)
+ return getFirstId();
return std::next(point_state_ptr_map_.find(i))->first;
}
-inline int LandmarkPolyline2D::getPrevValidId(const int& i) const
+inline int LandmarkPolyline2d::getPrevValidId(const int& i) const
{
- assert(!(i == first_id_ && !closed_) && "Calling getPrevValidId of first_id in an open landmark");
+ assert(!(i == getFirstId() && !closed_) && "Calling getPrevValidId of first_id in an open landmark");
assert(isValidId(i) && "Calling getPrevValidId of an invalid id");
- if (i == first_id_ && closed_)
- return last_id_;
+ if (i == getFirstId() && closed_)
+ return getLastId();
return std::prev(point_state_ptr_map_.find(i))->first;
}
-inline int LandmarkPolyline2D::getVersion() const
+inline int LandmarkPolyline2d::getVersion() const
{
return version_;
}
-inline std::vector<StateBlockPtr> LandmarkPolyline2D::getPointsStateBlockVector() const
+inline std::vector<StateBlockPtr> LandmarkPolyline2d::getPointsStateBlockVector() const
{
std::vector<StateBlockPtr> state_block_vec;
for (auto st_pair : point_state_ptr_map_)
@@ -331,21 +364,21 @@ inline std::vector<StateBlockPtr> LandmarkPolyline2D::getPointsStateBlockVector(
return state_block_vec;
}
-inline void LandmarkPolyline2D::classify(PolylineRectangularClass _class)
+inline void LandmarkPolyline2d::classify(PolylineRectangularClass _class)
{
classification_ = _class;
}
-inline PolylineRectangularClass LandmarkPolyline2D::getClassification() const
+inline PolylineRectangularClass LandmarkPolyline2d::getClassification() const
{
return classification_;
}
-inline void LandmarkPolyline2D::setMergedInLandmark(const LandmarkPolyline2DPtr& lmk_ptr)
+inline void LandmarkPolyline2d::setMergedInLandmark(const LandmarkPolyline2dPtr& lmk_ptr)
{
merged_in_lmk_ = lmk_ptr;
}
} /* namespace wolf */
-#endif /* LANDMARK_POLYLINE_2D_H_ */
+#endif /* LANDMARK_POLYLINE_2d_H_ */
diff --git a/include/laser/math/laser_tools.h b/include/laser/math/laser_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e5ae69aecf741a05b5025dcb39c18183cb683b7
--- /dev/null
+++ b/include/laser/math/laser_tools.h
@@ -0,0 +1,70 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * laser_tools.h
+ *
+ * Created on: Jun 17, 2020
+ * Author: jsola
+ */
+
+#ifndef INCLUDE_LASER_MATH_LASER_TOOLS_H_
+#define INCLUDE_LASER_MATH_LASER_TOOLS_H_
+
+#include <core/common/wolf.h>
+#include <core/state_block/state_composite.h>
+
+#include <laser_scan_utils/laser_scan_utils.h>
+#include <laser_scan_utils/icp.h>
+
+using namespace Eigen;
+
+namespace wolf
+{
+namespace laser
+{
+
+inline Eigen::Isometry2d trf2isometry(Vector2d translation, Vector1d rotation)
+{
+ Isometry2d T = Translation2d(translation) * Rotation2Dd(rotation(0));
+
+ return T;
+}
+
+inline Eigen::Isometry2d trf2isometry(Vector3d trf)
+{
+ Isometry2d T = Translation2d(trf.head<2>()) * Rotation2Dd(trf(2));
+
+ return T;
+}
+
+inline Eigen::Isometry2d trf2isometry(VectorComposite trf)
+{
+ return trf2isometry(trf.at('P'), trf.at('O'));
+}
+
+} // namespace laser
+
+} // namespace wolf
+
+
+
+#endif /* INCLUDE_LASER_MATH_LASER_TOOLS_H_ */
diff --git a/include/laser/processor/params_icp.h b/include/laser/processor/params_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..bfafa5dc03a2a341bbca112289316834bb95b8d0
--- /dev/null
+++ b/include/laser/processor/params_icp.h
@@ -0,0 +1,119 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_
+#define INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_
+
+#include "core/common/wolf.h"
+#include "laser_scan_utils/icp.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomIcp);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp);
+
+struct ParamsIcp
+{
+ // ICP Params
+ laserscanutils::icpParams icp_params;
+
+ ParamsIcp() = default;
+ ParamsIcp(std::string _prefix_and_unique_name, const ParamsServer &_server)
+ {
+ icp_params.verbose = _server.getParam<bool> (_prefix_and_unique_name + "/icp/verbose");
+
+ icp_params.use_point_to_line_distance = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_point_to_line_distance");
+ icp_params.max_angular_correction_deg = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_angular_correction_deg");
+ icp_params.max_linear_correction = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_linear_correction");
+ icp_params.max_iterations = _server.getParam<int> (_prefix_and_unique_name + "/icp/max_iterations");
+ icp_params.epsilon_xy = _server.getParam<double> (_prefix_and_unique_name + "/icp/epsilon_xy");
+ icp_params.epsilon_theta = _server.getParam<double> (_prefix_and_unique_name + "/icp/epsilon_theta");
+
+ icp_params.max_correspondence_dist = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_correspondence_dist");
+ icp_params.use_corr_tricks = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_corr_tricks");
+ icp_params.debug_verify_tricks = _server.getParam<bool> (_prefix_and_unique_name + "/icp/debug_verify_tricks");
+
+ icp_params.restart = _server.getParam<bool> (_prefix_and_unique_name + "/icp/restart");
+ icp_params.restart_threshold_mean_error = _server.getParam<double> (_prefix_and_unique_name + "/icp/restart_threshold_mean_error");
+ icp_params.restart_dt = _server.getParam<double> (_prefix_and_unique_name + "/icp/restart_dt");
+ icp_params.restart_dtheta = _server.getParam<double> (_prefix_and_unique_name + "/icp/restart_dtheta");
+
+ icp_params.min_reading = _server.getParam<double> (_prefix_and_unique_name + "/icp/min_reading");
+ icp_params.max_reading = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_reading");
+ icp_params.outliers_maxPerc = _server.getParam<double> (_prefix_and_unique_name + "/icp/outliers_maxPerc");
+ icp_params.outliers_adaptive_order = _server.getParam<double> (_prefix_and_unique_name + "/icp/outliers_adaptive_order");
+ icp_params.outliers_adaptive_mult = _server.getParam<double> (_prefix_and_unique_name + "/icp/outliers_adaptive_mult");
+ icp_params.outliers_remove_doubles = _server.getParam<bool> (_prefix_and_unique_name + "/icp/outliers_remove_doubles");
+
+ icp_params.do_visibility_test = _server.getParam<bool> (_prefix_and_unique_name + "/icp/do_visibility_test");
+
+ icp_params.do_alpha_test = _server.getParam<bool> (_prefix_and_unique_name + "/icp/do_alpha_test");
+ icp_params.do_alpha_test_thresholdDeg = _server.getParam<double> (_prefix_and_unique_name + "/icp/do_alpha_test_thresholdDeg");
+
+ icp_params.clustering_threshold = _server.getParam<double> (_prefix_and_unique_name + "/icp/clustering_threshold");
+ icp_params.orientation_neighbourhood = _server.getParam<int> (_prefix_and_unique_name + "/icp/orientation_neighbourhood");
+
+ icp_params.use_ml_weights = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_ml_weights");
+ icp_params.use_sigma_weights = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_sigma_weights");
+ icp_params.sigma = _server.getParam<double> (_prefix_and_unique_name + "/icp/sigma");
+
+ icp_params.do_compute_covariance = _server.getParam<bool> (_prefix_and_unique_name + "/icp/do_compute_covariance");
+ icp_params.cov_factor = _server.getParam<double> (_prefix_and_unique_name + "/icp/cov_factor");
+ }
+
+ std::string print() const
+ {
+ return "/icp/verbose" + std::to_string(icp_params.verbose) + "\n"
+ + "/icp/use_point_to_line_distance" + std::to_string(icp_params.use_point_to_line_distance) + "\n"
+ + "/icp/max_angular_correction_deg" + std::to_string(icp_params.max_angular_correction_deg) + "\n"
+ + "/icp/max_linear_correction" + std::to_string(icp_params.max_linear_correction) + "\n"
+ + "/icp/max_iterations" + std::to_string(icp_params.max_iterations) + "\n"
+ + "/icp/epsilon_xy" + std::to_string(icp_params.epsilon_xy) + "\n"
+ + "/icp/epsilon_theta" + std::to_string(icp_params.epsilon_theta) + "\n"
+ + "/icp/max_correspondence_dist" + std::to_string(icp_params.max_correspondence_dist) + "\n"
+ + "/icp/use_corr_tricks" + std::to_string(icp_params.use_corr_tricks) + "\n"
+ + "/icp/debug_verify_tricks" + std::to_string(icp_params.debug_verify_tricks) + "\n"
+ + "/icp/restart" + std::to_string(icp_params.restart) + "\n"
+ + "/icp/restart_threshold_mean_error" + std::to_string(icp_params.restart_threshold_mean_error) + "\n"
+ + "/icp/restart_dt" + std::to_string(icp_params.restart_dt) + "\n"
+ + "/icp/restart_dtheta" + std::to_string(icp_params.restart_dtheta) + "\n"
+ + "/icp/min_reading" + std::to_string(icp_params.min_reading) + "\n"
+ + "/icp/max_reading" + std::to_string(icp_params.max_reading) + "\n"
+ + "/icp/outliers_maxPerc" + std::to_string(icp_params.outliers_maxPerc) + "\n"
+ + "/icp/outliers_adaptive_order" + std::to_string(icp_params.outliers_adaptive_order) + "\n"
+ + "/icp/outliers_adaptive_mult" + std::to_string(icp_params.outliers_adaptive_mult) + "\n"
+ + "/icp/outliers_remove_doubles" + std::to_string(icp_params.outliers_remove_doubles) + "\n"
+ + "/icp/do_visibility_test" + std::to_string(icp_params.do_visibility_test) + "\n"
+ + "/icp/do_alpha_test" + std::to_string(icp_params.do_alpha_test) + "\n"
+ + "/icp/do_alpha_test_thresholdDeg" + std::to_string(icp_params.do_alpha_test_thresholdDeg) + "\n"
+ + "/icp/clustering_threshold" + std::to_string(icp_params.clustering_threshold) + "\n"
+ + "/icp/orientation_neighbourhood" + std::to_string(icp_params.orientation_neighbourhood) + "\n"
+ + "/icp/use_ml_weights" + std::to_string(icp_params.use_ml_weights) + "\n"
+ + "/icp/use_sigma_weights" + std::to_string(icp_params.use_sigma_weights) + "\n"
+ + "/icp/sigma" + std::to_string(icp_params.sigma) + "\n"
+ + "/icp/do_compute_covariance" + std::to_string(icp_params.do_compute_covariance) + "\n"
+ + "/icp/cov_factor" + std::to_string(icp_params.cov_factor) + "\n";
+ }
+};
+
+}
+
+#endif /* INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_ */
diff --git a/include/laser/processor/polyline_2D_utils.h b/include/laser/processor/polyline_2D_utils.h
deleted file mode 100644
index 4c08d4af17d5572e1f0bbe1ddcf76090be01e4c2..0000000000000000000000000000000000000000
--- a/include/laser/processor/polyline_2D_utils.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * polyline_2D_utils.h
- *
- * Created on: Mar 14, 2019
- * Author: jvallve
- */
-
-#ifndef POLYLINE_2D_UTILS_H_
-#define POLYLINE_2D_UTILS_H_
-
-#include "core/common/wolf.h"
-#include "laser/landmark/landmark_match_polyline_2D.h"
-#include "laser/landmark/landmark_polyline_2D.h"
-#include "laser/feature/feature_match_polyline_2D.h"
-#include "laser/feature/feature_polyline_2D.h"
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(MatchPolyline2D);
-typedef std::map<Scalar,MatchPolyline2DPtr> MatchPolyline2DMap;
-typedef std::list<MatchPolyline2DPtr> MatchPolyline2DList;
-
-/** \brief Match between a two polylines (not specialized to landmark nor feature)
- *
- * Match between a two polylines (not specialized to landmark nor feature)
- *
- **/
-struct MatchPolyline2D
-{
- int from_A_id_;
- int to_A_id_;
- int from_B_id_;
- int to_B_id_;
- int normalized_score_;
- Eigen::Matrix3s T_A_B_;
-};
-
-Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta);
-Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose);
-Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T);
-
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last, const bool& first_defined = true, const bool& last_defined=true);
-
-/** \brief Computes the squared distance from a point to a segment defined by two points
- **/
-Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b);
-
-Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict=false);
-
-MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error );
-
-MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error );
-
-bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B);
-
-Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
-
-} // namespace wolf
-
-#endif /* POLYLINE_2D_UTILS_H_ */
diff --git a/include/laser/processor/polyline_2d_utils.h b/include/laser/processor/polyline_2d_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..29e10d1f2252e797afdf492f6228d8d8f65e28ac
--- /dev/null
+++ b/include/laser/processor/polyline_2d_utils.h
@@ -0,0 +1,118 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * polyline_2d_utils.h
+ *
+ * Created on: Mar 14, 2019
+ * Author: jvallve
+ */
+
+#ifndef POLYLINE_2d_UTILS_H_
+#define POLYLINE_2d_UTILS_H_
+
+#include "core/common/wolf.h"
+#include "laser/landmark/landmark_match_polyline_2d.h"
+#include "laser/landmark/landmark_polyline_2d.h"
+#include "laser/feature/feature_match_polyline_2d.h"
+#include "laser/feature/feature_polyline_2d.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(MatchPolyline2d);
+typedef std::map<double,MatchPolyline2dPtr> MatchPolyline2dMap;
+typedef std::list<MatchPolyline2dPtr> MatchPolyline2dList;
+
+/** \brief Match between a two polylines (not specialized to landmark nor feature)
+ *
+ * Match between a two polylines (not specialized to landmark nor feature)
+ *
+ **/
+struct MatchPolyline2d
+{
+ int from_A_id_;
+ int to_A_id_;
+ int from_B_id_;
+ int to_B_id_;
+ double normalized_score_;
+ double overlap_dist_;
+ Eigen::Matrix3d T_A_B_;
+
+ void print() const
+ {
+ std::cout << std::endl << "POLYLINE2d MATCH" << std::endl;
+ std::cout << "A: " << std::endl;
+ std::cout << "\tfrom_A_id_ " << from_A_id_ << std::endl;
+ std::cout << "\tto_A_id_ " << to_A_id_ << std::endl;
+
+ std::cout << "B " << std::endl;
+ std::cout << "\tfrom_B_id_ " << from_B_id_ << std::endl;
+ std::cout << "\tto_B_id_ " << to_B_id_ << std::endl;
+
+ std::cout << "MATCH " << std::endl;
+ std::cout << "\tnormalized_score_ " << normalized_score_ << std::endl;
+ std::cout << "\toverlap_dist_ " << overlap_dist_ << std::endl;
+ std::cout << "\tT_A_B_ " << std::endl << T_A_B_ << std::endl;
+ }
+};
+
+Eigen::Matrix3d pose2T2d(const Eigen::Vector2d& t, const double& theta);
+Eigen::Matrix3d pose2T2d(const Eigen::Vector3d& pose);
+Eigen::Vector3d T2pose2d(const Eigen::Matrix3d& T);
+
+Eigen::Vector3d computeRigidTrans(const Eigen::MatrixXd& _points_incoming, const Eigen::MatrixXd& _points_last, const bool& first_defined = true, const bool& last_defined=true);
+
+/** \brief Computes the squared distance from a point to a segment defined by two points
+ **/
+double sqDistPoint2Segment(const Eigen::Vector2d& p, const Eigen::Vector2d& a, const Eigen::Vector2d& b);
+
+double sqDistPointToLine(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux,
+ const Eigen::Vector3d& _B, bool _A_defined, bool _B_defined, bool strict=false);
+
+MatchPolyline2dPtr computeBestSqDist(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& max_sq_error,
+ const double& min_overlapping_dist,
+ const int& min_N_overlapped,
+ const int& min_N_defined_overlapped,
+ bool both_exceeding_A_allowed_ = true,
+ bool both_exceeding_B_allowed_ = true);
+
+Eigen::Array<double,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXd& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
+ const Eigen::MatrixXd& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
+ const int& last_A, const int& N_overlapped );
+
+MatchPolyline2dList computeBestRigidTrans(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& max_sq_error );
+
+bool isProjectedOverSegment(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux, const Eigen::Vector3d& _B);
+
+Eigen::VectorXd computeSquaredMahalanobisDistances(const Eigen::Vector2d& _feature,
+ const Eigen::Matrix2d& _feature_cov,
+ const Eigen::Vector2d& _expected_feature,
+ const Eigen::Matrix2d& _expected_feature_cov,
+ const Eigen::MatrixXd& _mu);
+
+} // namespace wolf
+
+#endif /* POLYLINE_2d_UTILS_H_ */
diff --git a/include/laser/processor/processor_loop_closure_falko.h b/include/laser/processor/processor_loop_closure_falko.h
new file mode 100644
index 0000000000000000000000000000000000000000..755b702fa89231af99f6b43330a6393e40bb1e7a
--- /dev/null
+++ b/include/laser/processor/processor_loop_closure_falko.h
@@ -0,0 +1,677 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef PROCESSOR_LOOP_CLOSURE_FALKO_H_
+#define PROCESSOR_LOOP_CLOSURE_FALKO_H_
+
+#include <iostream>
+#include <fstream>
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/common/wolf.h"
+#include "core/processor/processor_loop_closure.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/utils/graph_search.h"
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_scene_falko.h"
+
+/**************************
+ * laseScanUtils includes *
+ **************************/
+#include "laser_scan_utils/loop_closure_falko.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosureFalko);
+
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
+struct MatchLoopClosureFalko : public MatchLoopClosure
+{
+ laserscanutils::MatchLoopClosureScenePtr match_falko_;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureFalko);
+
+/** \brief Struct class that store falkolib parameters and processorBase parameters
+ **/
+struct ParamsProcessorLoopClosureFalko : public ParamsProcessorLoopClosure
+{
+ int min_keypoints;
+ double score_th;
+ double distance_between_ref_frm;
+ int near_matches_th;
+ bool logging;
+ double th_kp_area;
+ double th_kp_perimeter;
+ double th_kp_maxd;
+ double th_kp_eig;
+ double th_scan_area;
+ double th_scan_eig;
+ std::string logging_filepath;
+
+ laserscanutils::ParameterLoopClosureFalko param;
+
+ ParamsProcessorLoopClosureFalko() = default;
+ ParamsProcessorLoopClosureFalko(std::string _unique_name, const ParamsServer &_server)
+ : ParamsProcessorLoopClosure(_unique_name, _server)
+ {
+
+ // processor params
+ min_keypoints = _server.getParam<int>(prefix + _unique_name + "/min_keypoints");
+ score_th = _server.getParam<double>(prefix + _unique_name + "/score_th");
+ distance_between_ref_frm = _server.getParam<double>(prefix + _unique_name + "/distance_between_ref_frm");
+ near_matches_th = _server.getParam<int>(prefix + _unique_name + "/near_matches_th");
+ th_kp_area = _server.getParam<double>(prefix + _unique_name + "/th_kp_area");
+ th_kp_perimeter = _server.getParam<double>(prefix + _unique_name + "/th_kp_perimeter");
+ th_kp_maxd = _server.getParam<double>(prefix + _unique_name + "/th_kp_maxd");
+ th_kp_eig = _server.getParam<double>(prefix + _unique_name + "/th_kp_eig");
+ th_scan_area = _server.getParam<double>(prefix + _unique_name + "/th_scan_area");
+ th_scan_eig = _server.getParam<double>(prefix + _unique_name + "/th_scan_eig");
+ logging = _server.getParam<bool>(prefix + _unique_name + "/logging");
+ if (logging)
+ logging_filepath = _server.getParam<std::string>(prefix + _unique_name + "/logging_filepath");
+
+ // Falko
+ param.matcher_distance_th_ = _server.getParam<double>(prefix + _unique_name + "/falko/matcher_distance_th");
+ param.use_descriptors_ = _server.getParam<bool>(prefix + _unique_name + "/falko/use_descriptors");
+ param.circularSectorNumber_ = _server.getParam<int>(prefix + _unique_name + "/falko/circularSectorNumber");
+ param.radialRingNumber_ = _server.getParam<int>(prefix + _unique_name + "/falko/radialRingNumber");
+ param.matcher_ddesc_th_ = _server.getParam<double>(prefix + _unique_name + "/falko/matcher_ddesc_th");
+ param.keypoints_number_th_ = _server.getParam<int>(prefix + _unique_name + "/falko/keypoints_number_th");
+ // Keypoints extractor params
+ param.enable_subbeam_ = _server.getParam<bool>(prefix + _unique_name + "/falko/enable_subbeam");
+ param.nms_radius_ = _server.getParam<double>(prefix + _unique_name + "/falko/nms_radius");
+ param.neigh_b_ = _server.getParam<double>(prefix + _unique_name + "/falko/neigh_b");
+ param.b_ratio_ = _server.getParam<double>(prefix + _unique_name + "/falko/b_ratio");
+ // Aht matcher params
+ param.xRes_ = _server.getParam<double>(prefix + _unique_name + "/falko/xRes");
+ param.yRes_ = _server.getParam<double>(prefix + _unique_name + "/falko/yRes");
+ param.thetaRes_ = _server.getParam<double>(prefix + _unique_name + "/falko/thetaRes");
+ param.xAbsMax_ = _server.getParam<double>(prefix + _unique_name + "/falko/xAbsMax");
+ param.yAbsMax_ = _server.getParam<double>(prefix + _unique_name + "/falko/yAbsMax");
+ param.thetaAbsMax_ = _server.getParam<double>(prefix + _unique_name + "/falko/thetaAbsMax");
+ };
+ virtual ~ParamsProcessorLoopClosureFalko() = default;
+
+ std::string print() const override
+ {
+
+ return ParamsProcessorLoopClosure::print() + "\n"
+ + "min_keypoints: " + std::to_string(min_keypoints) + "\n"
+ + "score_th: " + std::to_string(score_th) + "\n"
+ + "distance_between_ref_frm: " + std::to_string(distance_between_ref_frm) + "\n"
+ + "near_matches_th: " + std::to_string(near_matches_th) + "\n"
+ + "th_kp_area: " + std::to_string(th_kp_area) + "\n"
+ + "th_kp_perimeter: " + std::to_string(th_kp_perimeter) + "\n"
+ + "th_kp_maxd: " + std::to_string(th_kp_maxd) + "\n"
+ + "th_kp_eig: " + std::to_string(th_kp_eig) + "\n"
+ + "th_scan_area: " + std::to_string(th_scan_area) + "\n"
+ + "th_scan_eig: " + std::to_string(th_scan_eig) + "\n"
+ + "logging: " + std::to_string(logging) + "\n"
+ + "logging_filepath: " + logging_filepath + "\n"
+
+ + "falko/matcher_distance_th_: " + std::to_string(param.matcher_distance_th_) + "\n"
+ + "falko/use_descriptors_: " + std::to_string(param.use_descriptors_) + "\n"
+ + "falko/circularSectorNumber_: " + std::to_string(param.circularSectorNumber_) + "\n"
+ + "falko/radialRingNumber_: " + std::to_string(param.radialRingNumber_) + "\n"
+ + "falko/matcher_ddesc_th_: " + std::to_string(param.matcher_ddesc_th_) + "\n"
+ + "falko/keypoints_number_th_: " + std::to_string(param.keypoints_number_th_) + "\n"
+
+ + "falko/enable_subbeam_: " + std::to_string(param.enable_subbeam_) + "\n"
+ + "falko/nms_radius_: " + std::to_string(param.nms_radius_) + "\n"
+ + "falko/neigh_b_: " + std::to_string(param.neigh_b_) + "\n"
+ + "falko/b_ratio_: " + std::to_string(param.b_ratio_) + "\n"
+
+ + "falko/xRes_: " + std::to_string(param.xRes_) + "\n"
+ + "falko/yRes_: " + std::to_string(param.yRes_) + "\n"
+ + "falko/thetaRes_: " + std::to_string(param.thetaRes_) + "\n"
+ + "falko/xAbsMax_: " + std::to_string(param.xAbsMax_) + "\n"
+ + "falko/yAbsMax_: " + std::to_string(param.yAbsMax_) + "\n"
+ + "falko/thetaAbsMax_: " + std::to_string(param.thetaAbsMax_) + "\n";
+ };
+};
+
+/** \brief A class to implement a loop closure processor using loop closure detection from falko library
+ * The Laser data is provided by the sensor owning this processor.
+ *
+ * It extracts Features that contain laserscanutils::SceneFalko. The scene contain keypoints and descriptors
+ *
+ * It manages the capture and frame links in the inherited class ProcessorLoopClosure.
+ *
+ * It matches a target falko feature against a list of reference falko features.
+ *
+ * The reference features are found from a search of the previous captures
+ *
+ * The loop closures are not validated with an ICP
+ *
+ * Different types of descriptors and matchers can be used and are specified as template parameters.
+ * \tparam D Descriptor type. <bsc> or <cgh>
+ * \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor>
+ * \tparam L Loop closure Type <LoopClosureFalkoAht> or <LoopClosureFalkoNn>
+ * \param _param_falko parameter struct with falko lib parameters
+ **/
+template <typename D, typename Extr, template <typename, typename> typename L>
+class ProcessorLoopClosureFalko : public ProcessorLoopClosure
+{
+ public:
+ ProcessorLoopClosureFalko(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosure("ProcessorLoopClosureFalko", 2, _params_falko)
+ , params_falko_(_params_falko)
+ , loop_closure_(std::make_shared<L<D, Extr>>(params_falko_->param))
+ {
+ if (!params_falko_->logging)
+ return;
+
+ std::ostringstream ss, ss1, ss2, ss3, ss4, ss5, ss6, ss7, ss8, ss9, ss10, ss11, ss12;
+ ss << std::setw(2) << std::setfill('0') << params_falko_->score_th;
+ std::string score_th(ss.str());
+
+ ss1 << std::setw(4) << std::setfill('0') << params_falko_->param.matcher_distance_th_;
+ std::string matcher_distance_th(ss1.str());
+
+ ss2 << std::setw(4) << std::setfill('0') << params_falko_->param.matcher_ddesc_th_;
+ std::string matcher_ddesc_th(ss2.str());
+
+ ss3 << std::setw(4) << std::setfill('0') << params_falko_->param.enable_subbeam_;
+ std::string enable_subbeam(ss3.str());
+
+ ss4 << std::setw(4) << std::setfill('0') << params_falko_->param.nms_radius_;
+ std::string nms_radius(ss4.str());
+
+ ss5 << std::setw(4) << std::setfill('0') << params_falko_->param.neigh_b_;
+ std::string neigh_b(ss5.str());
+
+ ss6 << std::setw(4) << std::setfill('0') << params_falko_->param.b_ratio_;
+ std::string b_ratio(ss6.str());
+
+ ss7 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_area;
+ std::string th_kp_area(ss7.str());
+
+ ss8 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_perimeter;
+ std::string th_kp_perimeter(ss8.str());
+
+ ss9 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_maxd;
+ std::string th_kp_maxd(ss9.str());
+
+ ss10 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_eig;
+ std::string th_kp_eig(ss10.str());
+
+ ss11 << std::setw(3) << std::setfill('0') << params_falko_->th_scan_area;
+ std::string th_scan_area(ss11.str());
+
+ ss12 << std::setw(3) << std::setfill('0') << params_falko_->th_scan_eig;
+ std::string th_scan_eig(ss12.str());
+
+ std::string filename = params_falko_->logging_filepath + "/log" +
+ "_use_descriptors_" + std::to_string(params_falko_->param.use_descriptors_) +
+ "_score_th_" + score_th +
+ "_th_kp_area_" + th_kp_area +
+ "_th_kp_perimeter_" + th_kp_perimeter +
+ "_th_kp_maxd_" + th_kp_maxd +
+ "_th_kp_eig_" + th_kp_eig +
+ "_th_scan_area_" + th_scan_area +
+ "_th_scan_eig_" + th_scan_eig +
+ "_matcher_distance_th_" + matcher_distance_th +
+ "matcher_ddesc_th_" + matcher_ddesc_th +
+ "_enable_subbeam_" + enable_subbeam +
+ "_nms_radius_" + nms_radius +
+ "_neigh_b_" + neigh_b +
+ "_b_ratio_" + b_ratio + ".dat";
+
+ // change ~ with HOME using environment variable
+ if (filename.at(0) == '~')
+ filename = std::string(std::getenv("HOME")) + filename.substr(1);
+
+ outdata.open(filename, std::ofstream::out); // opens the file
+ if (outdata.fail())
+ WOLF_ERROR("ProcessorLoopClosureFalko: logging file '", filename, "' could not be created. \nError: ", strerror(errno));
+
+ init_outData_ = 0;
+ };
+
+ ~ProcessorLoopClosureFalko() { outdata.close(); };
+
+ std::list<MatchLoopClosurePtr> match_list_;
+
+ protected:
+ /** \brief Function not implemented
+ **/
+ bool voteFindLoopClosures(CaptureBasePtr cap) override { return true; };
+
+ /** \brief Function not implemented
+ **/
+ bool validateLoopClosure(MatchLoopClosurePtr match) override { return true; };
+
+ /** \brief It emplace factors between frames that its scenes are a match
+ * \param match structure that contains scenes and capture pointers that are a match
+ **/
+ void emplaceFactors(MatchLoopClosurePtr match) override
+ {
+ match_list_.push_back(match);
+ auto falko_match = std::static_pointer_cast<MatchLoopClosureFalko>(match);
+ auto ftr = FeatureBase::emplace<FeatureBase>(match->capture_reference, "FeatureTransformFalko",
+ falko_match->match_falko_->transform_vector,
+ Eigen::MatrixXd::Identity(3, 3));
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr, ftr, match->capture_target->getFrame(),
+ shared_from_this(), params_->apply_loss_function,
+ TOP_LOOP);
+ };
+
+ /** \brief Extracts a falko scene from a laser scan and emplaces it as a scen feature
+ * \param cap laser capture with a laser scan
+ **/
+ void emplaceFeatures(CaptureBasePtr cap) override
+ {
+ auto start_extract_scene = std::chrono::high_resolution_clock::now();
+
+ auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
+
+ assert(cap_laser && "ProcessorLoopClosureFalko::emplaceFeatures called with a capture of wrong type.");
+ // WOLF_INFO(params_falko_->print());
+ auto scene_falko = std::static_pointer_cast<laserscanutils::SceneFalko<D>>(loop_closure_->extractScene(
+ cap_laser->getScan(), std::static_pointer_cast<SensorLaser2d>(getSensor())->getScanParams()));
+
+ // Falko implementation
+ FeatureBase::emplace<FeatureSceneFalko<D>>(cap_laser, scene_falko);
+
+ WOLF_INFO("empalceFeatures: number of keypoints ", scene_falko->keypoints_list_.size())
+
+ auto duration_ms = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_extract_scene);
+
+ duration_extract_scene = duration_ms;
+ };
+
+ /** \brief It matches a target falko feature against a list of reference falko features.
+ * The reference features are found from a search of the previous captures
+ * \param cap laser capture with a laser scan and a falko feature
+ **/
+ std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
+ {
+ auto start_find_loop = std::chrono::high_resolution_clock::now();
+
+ auto current_frame = _capture->getFrame();
+ assert(_capture->getFrame() && "ProcessorLoopClosureFalko::findLoopClosures capture with no frame.");
+ assert(_capture->getFrame()->getTrajectory() &&
+ "ProcessorLoopClosureFalko::findLoopClosures capture with frame not in trajectory");
+
+ std::shared_ptr<laserscanutils::SceneFalko<D>> scene_falko_current;
+ // Find current falko scene
+ for (auto feat : _capture->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalko<D>>(feat);
+ scene_falko_current = feat_falko->getScene();
+ }
+
+ // Find reference scenes
+ std::map<laserscanutils::sceneBasePtr, CaptureBasePtr> map_scene_cap_ref;
+ std::list<laserscanutils::sceneBasePtr> list_scene_reference;
+ auto old_frame = _capture->getFrame()->getPreviousFrame();
+
+ const double& target_kp_area = scene_falko_current->area_;
+ const double& target_scan_area = scene_falko_current->area_scan_;
+ const double& target_kp_perimeter = scene_falko_current->perimeter_;
+ const double& target_kp_maxd = scene_falko_current->max_distance_;
+ const auto& target_eig_kp = scene_falko_current->eigenvalues_kp_;
+ const auto& target_eig_scan = scene_falko_current->eigenvalues_scan_;
+ GraphSearch graph_search;
+
+ int num_ref_scenes = 0;
+ int acum_score = 0;
+ while (old_frame)
+ {
+ for (auto cap : old_frame->getCaptureList())
+ {
+ for (auto feat : cap->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+
+ num_ref_scenes += 1;
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalko<D>>(feat);
+ double a_kp = feat_falko->getScene()->area_;
+ double a_min_kp = target_kp_area * params_falko_->th_kp_area;
+ double a_max_kp = target_kp_area * (1 + (1 - params_falko_->th_kp_area));
+
+ double p_kp = feat_falko->getScene()->perimeter_;
+ double p_min = target_kp_perimeter * params_falko_->th_kp_perimeter;
+ double p_max = target_kp_perimeter * (1 + (1 - params_falko_->th_kp_perimeter));
+
+ double maxd_kp = feat_falko->getScene()->max_distance_;
+ double maxd_min_kp = target_kp_maxd * params_falko_->th_kp_maxd;
+ double maxd_max_kp = target_kp_maxd * (1 + (1 - params_falko_->th_kp_maxd));
+
+ double a_scan = feat_falko->getScene()->area_scan_;
+ double a_min_scan = target_scan_area * params_falko_->th_scan_area;
+ double a_max_scan = target_scan_area * (1 + (1 - params_falko_->th_scan_area));
+
+ auto eig1_kp = feat_falko->getScene()->eigenvalues_kp_[0];
+ double eig1_min_kp = target_eig_kp[0] * params_falko_->th_kp_eig;
+ double eig1_max_kp = target_eig_kp[0] * (1 + (1 - params_falko_->th_kp_eig));
+
+ auto eig2_kp = feat_falko->getScene()->eigenvalues_kp_[1];
+ double eig2_min_kp = target_eig_kp[1] * params_falko_->th_kp_eig;
+ double eig2_max_kp = target_eig_kp[1] * (1 + (1 - params_falko_->th_kp_eig));
+
+ auto eig1_scan = feat_falko->getScene()->eigenvalues_scan_[0];
+ double eig1_min_scan = target_eig_scan[0] * params_falko_->th_scan_eig;
+ double eig1_max_scan = target_eig_scan[0] * (1 + (1 - params_falko_->th_scan_eig));
+
+ auto eig2_scan = feat_falko->getScene()->eigenvalues_scan_[1];
+ double eig2_min_scan = target_eig_scan[1] * params_falko_->th_scan_eig;
+ double eig2_max_scan = target_eig_scan[1] * (1 + (1 - params_falko_->th_scan_eig));
+
+ if (feat_falko->getScene()->keypoints_list_.size() <
+ params_falko_->min_keypoints) // min KEYPOINTS
+ continue;
+ if (((a_min_kp > a_kp) or (a_max_kp < params_falko_->th_kp_area)) and
+ a_kp != 0) // Area KEYPOINTS restriction
+ continue;
+ if (((p_min > p_kp) or (p_max < p_kp)) and
+ params_falko_->th_kp_perimeter != 0) // Perimeter KEYPOINTS restriction
+ continue;
+ if (((maxd_min_kp > maxd_kp) or (maxd_max_kp < maxd_kp)) and
+ params_falko_->th_kp_maxd != 0) // max_dist KEYPOINTS restriction
+ continue;
+ if (((a_min_scan > a_scan) or (a_max_scan < a_scan)) and
+ params_falko_->th_scan_area != 0) // Area SCAN restriction
+ continue;
+ if (((eig1_min_kp > eig1_kp) or (eig1_max_kp < eig1_kp)) and
+ params_falko_->th_kp_eig != 0) // eigenvalue 1 KEYPOINTS restriction
+ continue;
+ if (((eig2_min_kp > eig2_kp) or (eig2_max_kp < eig2_kp)) and
+ params_falko_->th_kp_eig != 0) // eigenvalue 2 KEYPOINTS restriction
+ continue;
+ if (((eig1_min_scan > eig1_scan) or (eig1_max_scan < eig1_scan)) and
+ params_falko_->th_scan_eig != 0) // eigenvalue 1 SCAN restriction
+ continue;
+ if (((eig2_min_scan > eig2_scan) or (eig2_max_scan < eig2_scan)) and
+ params_falko_->th_scan_eig != 0) // eigenvalue 2 SCAN restriction
+ continue;
+
+ auto shortest_path = graph_search.computeShortestPath(old_frame, current_frame, 10);
+
+ map_scene_cap_ref.emplace(feat_falko->getScene(), cap);
+ list_scene_reference.push_back(feat_falko->getScene());
+ }
+ }
+ old_frame = old_frame->getPreviousFrame();
+ }
+
+ WOLF_INFO("findLoopClosures: number of ref Scene ", list_scene_reference.size())
+ // find loop closure
+ std::map<double, laserscanutils::MatchLoopClosureScenePtr> match_lc_scene;
+
+ match_lc_scene = loop_closure_->findLoopClosure(list_scene_reference, scene_falko_current);
+ // fill MatchLoopClosurePtr list
+ std::map<double, MatchLoopClosurePtr> match_lc_map;
+ for (auto match_scenes : match_lc_scene)
+ {
+ if (match_scenes.first >= params_falko_->score_th)
+ {
+ WOLF_INFO("loop closure candidate score : ", match_scenes.first)
+ MatchLoopClosureFalkoPtr match_captures = std::make_shared<MatchLoopClosureFalko>();
+ match_captures->capture_reference = map_scene_cap_ref.at(match_scenes.second->scene_1);
+ match_captures->capture_target = _capture;
+ match_captures->normalized_score = match_scenes.first;
+ match_captures->match_falko_ = match_scenes.second;
+ while (match_lc_map.count(match_captures->normalized_score) != 0)
+ match_captures->normalized_score +=
+ (match_captures->normalized_score < 1.0 ? 1e-12 : -1e12);
+ acum_score += match_captures->normalized_score;
+ match_lc_map.emplace(match_captures->normalized_score, match_captures);
+
+ // TODO: calcular consistency
+ }
+ }
+
+ WOLF_INFO("findLoopClosures: lc number of candidates : ", match_lc_map.size())
+
+ duration_find_loop = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_find_loop);
+
+ if (params_falko_->logging)
+ printOutData(list_scene_reference, num_ref_scenes, match_lc_map, scene_falko_current->keypoints_list_,
+ acum_score);
+
+ if (params_falko_->near_matches_th == 1)
+ {
+ return match_lc_map;
+ }
+ // Check for robustness: more than one loop closure with nearby frames
+ std::map<double, MatchLoopClosurePtr> match_lc_map_robust;
+ // TODO: double scene_estimated_size = mean(keyoints.distance)
+ // distance_between_ref_frm_ =scene_estimated_size*factor; factor add to params
+ // near_matches_th = desapear
+ // Check angle
+ // Check in validate function: error max between transform and estimation -> consistency parameter in
+ // MatchLoopClosure if good consistency, not needed the other checks -> param for when consistency is good
+ // enough buscar papers per robustesa amb loop closures
+ for (auto match_capture_1 : match_lc_map)
+ {
+ bool consensus = true;
+ auto position_1 = match_capture_1.second->capture_reference->getFrame()->getP()->getState();
+
+ auto frame_1 = match_capture_1.second->capture_reference->getFrame();
+ // Backwards search
+ auto prev_frame_1 = frame_1->getPreviousFrame();
+ int match_prev_1_found = 0;
+ while (prev_frame_1)
+ {
+ // si surtim de la distance deixem de buscar
+ if ((position_1 - prev_frame_1->getP()->getState()).norm() >
+ params_falko_->distance_between_ref_frm)
+ {
+ break;
+ }
+ // Busquem match amb el prev_frame_1
+ for (auto match_capture_2 : match_lc_map)
+ {
+ // We look for a loop closure with the prev_frame_1
+ if (match_capture_2.second->capture_reference->getFrame() == prev_frame_1)
+ {
+ match_prev_1_found += 1;
+ }
+ }
+ prev_frame_1 = prev_frame_1->getPreviousFrame();
+ }
+
+ // Busqueda enrera
+ auto next_frame_1 = frame_1->getNextFrame();
+ int match_next_1_found = 0;
+ while (next_frame_1)
+ {
+ // si surtim de la distance deixem de buscar
+ if ((position_1 - next_frame_1->getP()->getState()).norm() >
+ params_falko_->distance_between_ref_frm)
+ {
+ break;
+ }
+ // Busquem match amb el next_frame_1
+ for (auto match_capture_2 : match_lc_map)
+ {
+ if (match_capture_2.second->capture_target->getFrame() == prev_frame_1)
+ {
+ match_next_1_found += 1;
+ }
+ }
+ next_frame_1 = next_frame_1->getNextFrame();
+ }
+
+ if (match_prev_1_found + match_next_1_found + 1 > params_falko_->near_matches_th)
+ {
+ consensus = true;
+ }
+ else
+ {
+ consensus = false;
+ }
+ if (consensus)
+ {
+ match_lc_map_robust.insert(match_capture_1);
+ }
+ }
+
+ return match_lc_map_robust;
+ }
+
+ void printOutData(std::list<laserscanutils::sceneBasePtr> &list_scene_reference, int num_ref_scenes,
+ std::map<double, MatchLoopClosurePtr> &match_lc_map, std::vector<falkolib::FALKO> &keypoints_list,
+ int acum_score)
+ {
+ if (init_outData_ == 1)
+ {
+ outdata << std::to_string(num_ok_) << ";";
+ outdata << std::to_string(num_nok_) << ";";
+ if (num_ok_ > 0)
+ {
+ outdata << std::to_string(duration_validation_ok.count() / num_ok_) << ";";
+ }
+ else
+ outdata << std::to_string(0) << ";";
+
+ if (num_nok_ > 0)
+ {
+ outdata << std::to_string(duration_validation_nok.count() / num_nok_) << ";" << std::endl;
+ }
+ else
+ outdata << std::to_string(0) << ";" << std::endl;
+ num_ok_ = 0;
+ num_nok_ = 0;
+ duration_validation_ok = std::chrono::duration_values<std::chrono::microseconds>::zero();
+ duration_validation_nok = std::chrono::duration_values<std::chrono::microseconds>::zero();
+ }
+ else
+ {
+ num_ok_ = 0;
+ num_nok_ = 0;
+ }
+ init_outData_ = 1;
+ outdata << std::to_string(list_scene_reference.size()) << ";";
+ outdata << std::to_string(num_ref_scenes) << ";";
+ outdata << std::to_string(match_lc_map.size()) << ";";
+ outdata << std::to_string(keypoints_list.size()) << ";";
+ if (match_lc_map.size() > 0)
+ {
+ outdata << std::to_string(acum_score / match_lc_map.size()) << ";";
+ }
+ else
+ {
+ outdata << std::to_string(0) << ";";
+ }
+ outdata << std::to_string(duration_find_loop.count()) << ";";
+ outdata << std::to_string(duration_extract_scene.count()) << ";";
+ }
+
+ ParamsProcessorLoopClosureFalkoPtr params_falko_;
+
+ std::shared_ptr<L<D, Extr>> loop_closure_;
+ std::ofstream outdata;
+ std::chrono::duration<long, std::micro> duration_find_loop;
+ std::chrono::duration<long, std::micro> duration_validation_ok;
+ std::chrono::duration<long, std::micro> duration_validation_nok;
+ std::chrono::duration<long, std::micro> duration_extract_scene;
+
+ public:
+ unsigned int getNStoredFrames() { return buffer_frame_.getContainer().size(); }
+ unsigned int getNStoredCaptures() { return buffer_capture_.getContainer().size(); }
+ int num_ok_;
+ int num_nok_;
+ int init_outData_;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoNnBsc
+ : public ProcessorLoopClosureFalko<laserscanutils::bsc, laserscanutils::bscExtractor,
+ laserscanutils::LoopClosureFalkoNn>
+{
+ public:
+ ProcessorLoopClosureFalkoNnBsc(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoNnBsc, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoNnBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and AHT matcher
+ **/
+class ProcessorLoopClosureFalkoAhtBsc : public ProcessorLoopClosureFalko<laserscanutils::bsc,
+ laserscanutils::bscExtractor,
+ laserscanutils::LoopClosureFalkoAht>
+{
+ public:
+ ProcessorLoopClosureFalkoAhtBsc(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoAhtBsc, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoAhtBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnCgh);
+
+/** \brief A class that implements the loop closure with CGH descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoNnCgh : public ProcessorLoopClosureFalko<laserscanutils::cgh,
+ laserscanutils::cghExtractor,
+ laserscanutils::LoopClosureFalkoNn>
+{
+ public:
+ ProcessorLoopClosureFalkoNnCgh(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoNnCgh, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoNnCgh() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtCgh);
+
+/** \brief A class that implements the loop closure with CGH descriptors and AHT matcher
+ **/
+class ProcessorLoopClosureFalkoAhtCgh : public ProcessorLoopClosureFalko<laserscanutils::cgh,
+ laserscanutils::cghExtractor,
+ laserscanutils::LoopClosureFalkoAht>
+{
+ public:
+ ProcessorLoopClosureFalkoAhtCgh(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoAhtCgh, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoAhtCgh() = default;
+};
+
+} // namespace wolf
+
+#endif // SRC_PROCESSOR_LOOP_CLOSURE_FALKO_H_
diff --git a/include/laser/processor/processor_loop_closure_falko_icp.h b/include/laser/processor/processor_loop_closure_falko_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3e41335bcebd0d73b655cbbcf7aee0846ab498b
--- /dev/null
+++ b/include/laser/processor/processor_loop_closure_falko_icp.h
@@ -0,0 +1,257 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef PROCESSOR_LOOP_CLOSURE_FALKO_ICP_H_
+#define PROCESSOR_LOOP_CLOSURE_FALKO_ICP_H_
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include <laser/processor/processor_loop_closure_falko.h>
+#include <laser/processor/params_icp.h>
+#include <laser/feature/feature_icp_align.h>
+
+/**************************
+ * laseScanUtils - ICP includes *
+ **************************/
+#include <laser_scan_utils/icp.h>
+
+using namespace laserscanutils;
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureFalkoIcp);
+
+struct ParamsProcessorLoopClosureFalkoIcp : public ParamsProcessorLoopClosureFalko, public ParamsIcp
+{
+ // ParameterLoopClosureFalko param;
+ double max_error_threshold;
+ double min_points_percent;
+
+ ParamsProcessorLoopClosureFalkoIcp() = default;
+ ParamsProcessorLoopClosureFalkoIcp(std::string _unique_name, const ParamsServer &_server) :
+ ParamsProcessorLoopClosureFalko(_unique_name, _server),
+ ParamsIcp(prefix + _unique_name, _server)
+ {
+ max_error_threshold = _server.getParam<double>(prefix + _unique_name + "/max_error_threshold");
+ min_points_percent = _server.getParam<double>(prefix + _unique_name + "/min_points_percent");
+ };
+
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorLoopClosureFalko::print() + "\n"
+ + ParamsIcp::print() + "\n"
+ + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
+ + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+ }
+};
+
+/** \brief A class to implement a loop closure processor using loop closure detection from falko library
+ * The Laser data is provided by the sensor owning this processor.
+ *
+ * All the falko loop Closure functionality is implemented in the inherited class
+ *
+ * In this class is only implemented the validation of the falko loop closures with an ICP
+ *
+ * Diferent types of descriptors and matchers can be used and are specified as template parameters.
+ * \tparam D Descriptor type. <bsc> or <cgh>
+ * \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor>
+ * \tparam M Matcher type <nn_matcher> or <aht_matcher>
+ * \param _param_falko parameter struct with falko lib parameters
+ **/
+
+template <typename D, typename Extr, template <typename, typename> typename L>
+class ProcessorLoopClosureFalkoIcp : public ProcessorLoopClosureFalko<D, Extr, L>
+{
+
+ public:
+ ProcessorLoopClosureFalkoIcp(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalko<D, Extr, L>(_params_falko)
+ , params_falko_icp_(_params_falko){
+
+ };
+
+ ~ProcessorLoopClosureFalkoIcp() = default;
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcp, ParamsProcessorLoopClosureFalkoIcp);
+
+ protected:
+ ParamsProcessorLoopClosureFalkoIcpPtr params_falko_icp_;
+ // ICP algorithm
+ std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
+
+ /** \brief Function that validates the falko loop closures with an ICP aligment.
+ *
+ * For the ICP alignment it uses the transform provided by the falko algorithm as an initial guess.
+ *
+ * \param match structure that contains scenes and capture pointers that are a match
+ **/
+ bool validateLoopClosure(MatchLoopClosurePtr match) override
+ {
+ auto start_validation = std::chrono::high_resolution_clock::now();
+ auto capture_own = match->capture_reference; //< Capture reference
+ auto capture_other = match->capture_target; // Capture target
+ auto frame_own = capture_own->getFrame();
+ auto frame_other = capture_other->getFrame();
+ bool match_valid = false;
+
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(match);
+
+ if ((capture_own->getType() == "CaptureLaser2d") & (capture_other->getType() == "CaptureLaser2d"))
+ {
+ CaptureLaser2dPtr capture_laser_other = std::static_pointer_cast<CaptureLaser2d>(capture_other);
+ CaptureLaser2dPtr capture_laser_own = std::static_pointer_cast<CaptureLaser2d>(capture_own);
+
+ SensorLaser2dPtr sensor_own, sensor_other;
+ sensor_own = std::static_pointer_cast<SensorLaser2d>(capture_laser_own->getSensor());
+ sensor_other = std::static_pointer_cast<SensorLaser2d>(capture_laser_other->getSensor());
+
+ WOLF_INFO("DBG Attempting to close loop with ", frame_own->id(), " and ", frame_other->id());
+ try
+ {
+ laserscanutils::icpOutput trf = icp_tools_ptr_->align(
+ capture_laser_other->getScan(), capture_laser_own->getScan(), sensor_own->getScanParams(),
+ sensor_other->getScanParams(), params_falko_icp_->icp_params,
+ match_falko->match_falko_->transform_vector);
+
+ double points_coeff_used =
+ ((double)trf.nvalid / (double)fmin(capture_laser_own->getScan().ranges_raw_.size(),
+ capture_laser_other->getScan().ranges_raw_.size()));
+ double mean_error = trf.error / trf.nvalid;
+ Eigen::Vector3d displacement_other_own;
+ displacement_other_own.head(2) =
+ Eigen::Rotation2Dd(-frame_other->getState().vector("O")(0)) *
+ (frame_own->getState().vector("P") - frame_other->getState().vector("P"));
+ displacement_other_own(2) =
+ frame_own->getState().vector("O")(0) - frame_other->getState().vector("O")(0);
+
+ Eigen::Vector3d displacement_own_other;
+ displacement_own_other.head(2) =
+ Eigen::Rotation2Dd(-frame_own->getState().vector("O")(0)) *
+ (frame_other->getState().vector("P") - frame_own->getState().vector("P"));
+ displacement_own_other(2) =
+ frame_other->getState().vector("O")(0) - frame_own->getState().vector("O")(0);
+
+ WOLF_INFO("DBG ------------------------------");
+ WOLF_INFO("DBG valid? ", trf.valid, "\n m_er ", mean_error,
+ " \n % points used :", points_coeff_used * 100);
+ // WOLF_INFO("DBG own_POSE: ", frame_own->getState().vector("PO").transpose(),
+ // "\nother_POSE: ", frame_other->getState().vector("PO").transpose(),
+ // WOLF_INFO("\ndisplacement_other_own : ", displacement_other_own.transpose(),
+ WOLF_INFO("\ndisplacement_own_other : ", displacement_own_other.transpose(),
+ "\nIcp initial guess: ", match_falko->match_falko_->transform_vector.transpose(),
+ "\nIcp final transfr: ", trf.res_transf.transpose());
+ // WOLF_INFO("Covariance \n", trf.res_covar);
+ if (trf.valid == 1 and mean_error < params_falko_icp_->max_error_threshold and
+ points_coeff_used * 100 > params_falko_icp_->min_points_percent)
+ {
+ WOLF_INFO("Match validated ", frame_own->id(), " and ", frame_other->id());
+ match_valid = true;
+ match_falko->match_falko_->transform_vector = trf.res_transf;
+ this->num_ok_ += 1;
+ this->duration_validation_ok += std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_validation);
+ }
+ else
+ {
+ WOLF_INFO("Match DISCARDED ", frame_own->id(), " and ", frame_other->id());
+ this->num_nok_ += 1;
+ this->duration_validation_nok += std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_validation);
+ }
+ }
+ catch (std::exception &e)
+ {
+ this->num_nok_ += 1;
+ WOLF_WARN(e.what())
+ }
+ }
+ return match_valid;
+ }
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpNnBsc
+ : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoNn>
+{
+ public:
+ ProcessorLoopClosureFalkoIcpNnBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnBsc, ParamsProcessorLoopClosureFalkoIcp);
+
+ ~ProcessorLoopClosureFalkoIcpNnBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpAhtBsc
+ : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoAht>
+{
+ public:
+ ProcessorLoopClosureFalkoIcpAhtBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtBsc, ParamsProcessorLoopClosureFalkoIcp);
+
+ ~ProcessorLoopClosureFalkoIcpAhtBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnCgh);
+
+/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpNnCgh
+ : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoNn>
+{
+ public:
+ ProcessorLoopClosureFalkoIcpNnCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnCgh, ParamsProcessorLoopClosureFalkoIcp);
+
+ ~ProcessorLoopClosureFalkoIcpNnCgh() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtCgh);
+
+/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpAhtCgh
+ : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoAht>
+{
+ public:
+ ProcessorLoopClosureFalkoIcpAhtCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtCgh, ParamsProcessorLoopClosureFalkoIcp);
+
+ ~ProcessorLoopClosureFalkoIcpAhtCgh() = default;
+};
+
+} // namespace wolf
+
+#endif // SRC_PROCESSOR_LOOP_CLOSURE_FALKO_ICP_H_
diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..5741d1110f42beca9cf2c473917bde618d164119
--- /dev/null
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -0,0 +1,158 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef SRC_PROCESSOR_LOOP_CLOSURE_ICP_H_
+#define SRC_PROCESSOR_LOOP_CLOSURE_ICP_H_
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "laser/internal/config.h"
+#include "core/common/wolf.h"
+#include "core/processor/processor_loop_closure.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+
+#include "laser/processor/params_icp.h"
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_icp_align.h"
+
+/**************************
+ * ICP includes *
+ **************************/
+#include "laser_scan_utils/laser_scan_utils.h"
+#include "laser_scan_utils/icp.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureIcp);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcp);
+
+struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure, public ParamsIcp
+{
+ int recent_frames_ignored; // recent frames not considered candidates
+ int frames_ignored_after_loop; // wait frames after closing a loop
+ int max_attempts; // max number of ICP attempts for stopping creating candidates
+ int max_candidates; // max number of candidats (ICP success) for stopping creating candidates
+
+ std::string candidate_generation; // order of selection of frames: "deterministic" or "random"
+
+ double max_error_threshold; // maximum error for accepting ICP
+ double min_points_percent; // minimum correspondence % for accepting ICP
+
+ ParamsProcessorLoopClosureIcp() = default;
+ ParamsProcessorLoopClosureIcp(std::string _unique_name, const ParamsServer& _server) :
+ ParamsProcessorLoopClosure(_unique_name, _server),
+ ParamsIcp(prefix + _unique_name, _server)
+ {
+ recent_frames_ignored = _server.getParam<int> (prefix + _unique_name + "/recent_frames_ignored");
+ frames_ignored_after_loop = _server.getParam<int> (prefix + _unique_name + "/frames_ignored_after_loop");
+ max_attempts = _server.getParam<int> (prefix + _unique_name + "/max_attempts");
+ max_candidates = _server.getParam<int> (prefix + _unique_name + "/max_candidates");
+ candidate_generation = _server.getParam<std::string> (prefix + _unique_name + "/candidate_generation");
+ max_error_threshold = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
+ min_points_percent = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
+ }
+
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorLoopClosure::print()
+ + ParamsIcp::print() + "\n"
+ + "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n"
+ + "frames_ignored_after_loop: " + std::to_string(frames_ignored_after_loop) + "\n"
+ + "max_attempts: " + std::to_string(max_attempts) + "\n"
+ + "max_candidates: " + std::to_string(max_candidates) + "\n"
+ + "candidate_generation: " + candidate_generation + "\n"
+ + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
+ + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+ }
+};
+
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
+struct MatchLoopClosureIcp : public MatchLoopClosure
+{
+ laserscanutils::icpOutput icp_result;
+};
+
+class ProcessorLoopClosureIcp : public ProcessorLoopClosure
+{
+ protected:
+ // Useful sensor stuff
+ SensorLaser2dPtr sensor_laser_; // casted pointer to parent
+ laserscanutils::LaserScanParams laser_scan_params_;
+
+ //Closeloop parameters
+ ParamsProcessorLoopClosureIcpPtr params_loop_closure_icp_;
+
+ // ICP algorithm
+ std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
+
+ MatchLoopClosurePtr last_loop_closure_;
+
+ public:
+ ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params);
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureIcp, ParamsProcessorLoopClosureIcp);
+ void configure(SensorBasePtr _sensor) override;
+
+ ParamsProcessorLoopClosureIcpPtr getParams() const;
+
+ protected:
+
+ /** \brief Returns if findLoopClosures() has to be called for the given capture
+ *
+ * This case avoids searching if a recent loop closure
+ */
+ bool voteFindLoopClosures(CaptureBasePtr cap) override;
+
+ /** \brief detects and emplaces all features of the given capture
+ *
+ * In this case, ICP does not work with features to find a loop closure.
+ * A feature will be added when emplacing the factors.
+ *
+ */
+ void emplaceFeatures(CaptureBasePtr cap) override {/*nothing*/};
+
+ /** \brief Find captures that correspond to loop closures with the given capture
+ */
+ std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override;
+
+ MatchLoopClosurePtr matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_target) const;
+
+ /** \brief validates a loop closure
+ */
+ bool validateLoopClosure(MatchLoopClosurePtr) override {return true;};
+
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ void emplaceFactors(MatchLoopClosurePtr) override;
+
+ /** \brief returns a list containing the loop closure frame candidates list
+ */
+ FrameBasePtrList generateSearchList(CaptureBasePtr) const;
+};
+
+}
+
+#endif // SRC_PROCESSOR_LOOP_CLOSURE_ICP_H_
diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..a802d9232fb0362cef9892721f5d83c81ec9da00
--- /dev/null
+++ b/include/laser/processor/processor_odom_icp.h
@@ -0,0 +1,144 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef SRC_PROCESSOR_ODOM_ICP_H_
+#define SRC_PROCESSOR_ODOM_ICP_H_
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker.h"
+#include "core/processor/motion_provider.h"
+#include "laser/processor/params_icp.h"
+#include "laser/capture/capture_laser_2d.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "laser/feature/feature_icp_align.h"
+
+/**************************
+ * ICP includes *
+ **************************/
+#include <laser_scan_utils/laser_scan_utils.h>
+#include <laser_scan_utils/icp.h>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomIcp);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp);
+
+struct ParamsProcessorOdomIcp : public ParamsProcessorTracker, public ParamsMotionProvider, public ParamsIcp
+{
+ double vfk_min_dist;
+ double vfk_min_angle;
+ double vfk_min_time;
+ double vfk_min_error;
+ int vfk_max_points;
+
+ // initial guess source
+ std::string initial_guess;
+
+ ParamsProcessorOdomIcp() = default;
+ ParamsProcessorOdomIcp(std::string _unique_name, const ParamsServer &_server):
+ ParamsProcessorTracker(_unique_name, _server),
+ ParamsMotionProvider(_unique_name, _server),
+ ParamsIcp(prefix + _unique_name, _server)
+ {
+ // keyframe voting
+ vfk_min_dist = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_dist");
+ vfk_min_angle = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_angle");
+ vfk_min_time = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_time");
+ vfk_min_error = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_error");
+ vfk_max_points = _server.getParam<int> (prefix + _unique_name + "/keyframe_vote/max_points");
+
+ initial_guess = _server.getParam<std::string>(prefix + _unique_name + "/initial_guess");
+ }
+
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorTracker::print() + "\n"
+ + ParamsMotionProvider::print() + "\n"
+ + ParamsIcp::print() + "\n"
+ + "initial_guess: " + initial_guess + "\n"
+ + "keyframe_vote/min_dist: " + std::to_string(vfk_min_dist) + "\n"
+ + "keyframe_vote/min_angle: " + std::to_string(vfk_min_angle) + "\n"
+ + "keyframe_vote/min_time: " + std::to_string(vfk_min_time) + "\n"
+ + "keyframe_vote/min_error: " + std::to_string(vfk_min_error) + "\n"
+ + "keyframe_vote/max_points: " + std::to_string(vfk_max_points) + "\n";
+ }
+};
+
+class ProcessorOdomIcp : public ProcessorTracker, public MotionProvider
+{
+ protected:
+ // Useful sensor stuff
+ SensorLaser2dPtr sensor_laser_; // casted pointer to parent
+ laserscanutils::LaserScanParams laser_scan_params_;
+
+ // ICP algorithm
+ std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
+
+ // temporary and carry-on transformations
+ laserscanutils::icpOutput trf_origin_last_;
+ laserscanutils::icpOutput trf_origin_incoming_;
+ laserscanutils::icpOutput trf_last_incoming_;
+ Eigen::Vector3d odom_origin_;
+ Eigen::Vector3d odom_last_;
+ Eigen::Vector3d odom_incoming_;
+
+ public:
+ ParamsProcessorOdomIcpPtr params_odom_icp_;
+
+ public:
+ ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params);
+ WOLF_PROCESSOR_CREATE(ProcessorOdomIcp, ParamsProcessorOdomIcp);
+ ~ProcessorOdomIcp() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ VectorComposite getState(const StateStructure& _structure = "") const override;
+ TimeStamp getTimeStamp( ) const override;
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
+ void setProblem(ProblemPtr _problem_ptr) override;
+
+ protected:
+ void preProcess() override;
+
+ unsigned int processKnown() override;
+ unsigned int processNew(const int& _max_features) override;
+
+ bool voteForKeyFrame() const override;
+
+ void advanceDerived() override;
+ void resetDerived() override;
+
+
+ void establishFactors() override;
+ FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
+
+ bool voteForKeyFrameDistance() const;
+ bool voteForKeyFrameAngle() const;
+ bool voteForKeyFrameTime() const;
+ bool voteForKeyFrameMatchQuality() const;
+};
+
+}
+
+#endif // SRC_PROCESSOR_ODOM_ICP_H_
diff --git a/include/laser/processor/processor_polyline.h b/include/laser/processor/processor_polyline.h
deleted file mode 100644
index 954b0e5e6767333d8e6851cd5ebdf28872abf5da..0000000000000000000000000000000000000000
--- a/include/laser/processor/processor_polyline.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * processor_polyline.h
- *
- * Created on: Sep 14, 2017
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_POLYLINE_H_
-#define SRC_PROCESSOR_POLYLINE_H_
-
-// Wolf includes
-#include "laser/sensor/sensor_laser_2D.h"
-#include "laser/capture/capture_laser_2D.h"
-#include "laser/feature/feature_polyline_2D.h"
-#include "core/landmark/landmark_match.h"
-#include "laser/landmark/landmark_polyline_2D.h"
-#include "laser/factor/factor_point_2D.h"
-#include "laser/factor/factor_point_to_line_2D.h"
-#include "core/state_block/state_block.h"
-#include "core/processor/processor_base.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
-WOLF_PTR_TYPEDEFS(ProcessorPolyline);
-
-struct ProcessorParamsPolyline : public ProcessorParamsBase
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar match_position_error_th;
- Scalar class_position_error_th;
- unsigned int new_features_th;
- Scalar loop_time_th;
- std::vector<PolylineRectangularClass> polyline_classes;
- Scalar position_error_th_;
- Scalar min_features_ratio_th_;
-};
-
-class ProcessorPolyline : public ProcessorBase
-{
- protected:
- CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
- CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
- laserscanutils::LineFinderIterative line_finder_;
-
- ProcessorParamsPolylinePtr params_;
-
- FeatureBasePtrList new_features_incoming_, new_features_last_, known_features_incoming_, known_features_last_;
- LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
- LandmarkMatchMap matches_landmark_from_incoming_;
- LandmarkMatchMap matches_landmark_from_last_;
-
- std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
-
- Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Matrix2s R_current_prev_;
- Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Vector2s t_current_prev_;
- Eigen::Vector2s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorPolyline(ProcessorParamsPolylinePtr _params);
-
- virtual ~ProcessorPolyline();
-
- virtual void configure(SensorBasePtr _sensor) { };
-
- // Algorithm
- virtual void process(CaptureBasePtr _capture_ptr);
-
- void processNew();
-
- virtual bool voteForKeyFrame();
-
- // Implementation
- void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
- unsigned int findLandmarks(const LandmarkBasePtrList& _landmark_list_in, FeatureBasePtrList& _feature_list_out, LandmarkMatchMap& _feature_landmark_correspondences);
-
- virtual void createNewLandmarks();
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- virtual void establishFactors();
- void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id);
- void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id);
- bool rejectOutlier(FactorBasePtr& fac_ptr);
- void classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list);
- LandmarkMatchPolyline2DPtr tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr);
- LandmarkMatchPolyline2DPtr tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
-
- // Gets/Sets
- const FeatureBasePtrList& getLastNewPolylines() const;
- const FeatureBasePtrList& getLastKnownPolylines() const;
- virtual void setKeyFrame(CaptureBasePtr _capture_ptr);
- virtual CaptureBasePtr getLast();
-
- // Maths
- void computeTransformations(const TimeStamp& _ts);
- void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_);
-
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline void ProcessorPolyline::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id)
-{
- assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
-
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-inline void ProcessorPolyline::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id)
-{
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-inline const FeatureBasePtrList& ProcessorPolyline::getLastNewPolylines() const
-{
- return new_features_last_;
-}
-
-inline const FeatureBasePtrList& ProcessorPolyline::getLastKnownPolylines() const
-{
- if (last_ptr_->getFrame()->isKey())
- return last_ptr_->getFeatureList();
- else
- return known_features_last_;
-}
-
-inline CaptureBasePtr ProcessorPolyline::getLast()
-{
- return last_ptr_;
-}
-
-} /* namespace wolf */
-
-#endif /* SRC_PROCESSOR_POLYLINE_H_ */
diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
deleted file mode 100644
index add094f3d876d2a5bffe8c5288fde811672dc97d..0000000000000000000000000000000000000000
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ /dev/null
@@ -1,209 +0,0 @@
-/*
- * processor_tracker_feature_polyline_2D.h
- *
- * Created on: Mar 11, 2019
- * Author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_
-#define PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_
-
-#include "laser/processor/polyline_2D_utils.h"
-#include "core/processor/processor_tracker_feature.h"
-#include "laser/landmark/landmark_polyline_2D.h"
-#include "laser/landmark/landmark_match_polyline_2D.h"
-#include "laser/sensor/sensor_laser_2D.h"
-#include "laser/capture/capture_laser_2D.h"
-#include "laser/feature/feature_polyline_2D.h"
-#include "laser/feature/feature_match_polyline_2D.h"
-#include "laser/factor/factor_point_2D.h"
-#include "laser/factor/factor_point_to_line_2D.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline2D);
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2D);
-typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DPtrList;
-typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DPtrList;
-typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
-typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
-
-struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar match_alignment_position_sq_norm_th;
- Scalar match_landmark_pose_sq_norm_th;
- Scalar match_feature_pose_sq_norm_th;
- Scalar class_position_error_th;
- unsigned int new_features_th;
- Scalar loop_time_th;
- std::vector<PolylineRectangularClass> polyline_classes;
-};
-
-class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
-{
- protected:
- ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_;
- laserscanutils::LineFinderIterative line_finder_;
-
- FeatureBasePtrList all_features_incoming_, all_features_last_;
- LandmarkMatchPolyline2DMap landmark_match_map_;
- LandmarkPolyline2DPtrList modified_lmks_;
- std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
-
- Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
- Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
- Eigen::Matrix2s R_sensor_world_incoming_, R_world_sensor_incoming_;
- Eigen::Vector2s t_sensor_world_incoming_, t_world_sensor_incoming_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Matrix2s R_last_incoming_, R_incoming_last_;
- Eigen::Vector2s t_last_incoming_, t_incoming_last_;
- bool extrinsics_transformation_computed_;
-
- public:
- ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params);
-
- virtual ~ProcessorTrackerFeaturePolyline2D();
-
- virtual void configure(SensorBasePtr _sensor){};
-
- protected:
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences);
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _origin_feature input feature in origin capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature);
-
- /** \brief Vote for KeyFrame generation
- *
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create KeyFrames!
- */
- virtual bool voteForKeyFrame();
-
- /**\brief Process new Features
- *
- */
- virtual unsigned int processNew(const unsigned int& _max_features);
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_new_features,
- FeatureBasePtrList& _features_last_out);
-
- /** \brief Create a new factor and link it to the wolf tree
- * \param _feature_ptr pointer to the parent Feature
- * \param _feature_other_ptr pointer to the other feature constrained.
- *
- * Implement this method in derived classes.
- *
- * This function creates a factor of the appropriate type for the derived processor.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr){ return nullptr; };
-
- /** \brief Establish factors between features in Captures \b last and \b origin
- */
- virtual void establishFactors();
-
- void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id);
-
- void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id);
-
- /** \brief create a landmark from a feature
- *
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr);
-
- /** \brief advance pointers
- *
- */
- virtual void advanceDerived() override;
-
- /** \brief reset pointers and match lists at KF creation
- *
- */
- virtual void resetDerived() override;
-
- /** \brief Pre-process
- *
- */
- virtual void preProcess() override;
-
- /** \brief Post-process
- *
- */
- virtual void postProcess() override;
-
- void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
- const FeaturePolyline2DPtr& _ftr_last,
- const FeaturePolyline2DPtr& _ftr_incoming,
- const Eigen::Matrix3s& _T_last_incoming_prior) const;
-
- void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const;
-
- bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const;
-
- void computeTransformations();
-
- LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
- FeatureMatchPolyline2DPtr ftr_match_incoming_last,
- LandmarkMatchPolyline2DPtr lmk_match_last) const;
-
- public:
-
- /// @brief Factory method
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr);
-
- const FeatureBasePtrList& getLastNewFeatures() const
- {
- return all_features_last_;
- }
-};
-
-} /* namespace wolf */
-
-#endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_ */
diff --git a/include/laser/processor/processor_tracker_feature_polyline_2d.h b/include/laser/processor/processor_tracker_feature_polyline_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..c35a574f9979cd99e37e358d32e62607010f7295
--- /dev/null
+++ b/include/laser/processor/processor_tracker_feature_polyline_2d.h
@@ -0,0 +1,289 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_tracker_feature_polyline_2d.h
+ *
+ * Created on: Mar 11, 2019
+ * Author: jvallve
+ */
+
+#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_2d_H_
+#define PROCESSOR_TRACKER_FEATURE_POLYLINE_2d_H_
+
+#include "laser/processor/polyline_2d_utils.h"
+#include "laser/landmark/landmark_polyline_2d.h"
+#include "laser/landmark/landmark_match_polyline_2d.h"
+#include "laser/sensor/sensor_laser_2d.h"
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_polyline_2d.h"
+#include "laser/feature/feature_match_polyline_2d.h"
+#include "laser/factor/factor_point_2d.h"
+#include "laser/factor/factor_point_to_line_2d.h"
+
+#include "core/processor/processor_tracker_feature.h"
+#include "core/utils/params_server.h"
+
+//laser_scan_utils
+#include "laser_scan_utils/laser_scan.h"
+#include "laser_scan_utils/line_finder_iterative.h"
+#include "laser_scan_utils/polyline.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeaturePolyline2d);
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2d);
+typedef std::list<FeatureMatchPolyline2dPtr> FeatureMatchPolyline2dPtrList;
+typedef std::list<LandmarkMatchPolyline2dPtr> LandmarkMatchPolyline2dPtrList;
+typedef std::map<double,FeatureMatchPolyline2dPtr> FeatureMatchPolyline2dScalarMap;
+typedef std::map<double,LandmarkMatchPolyline2dPtr> LandmarkMatchPolyline2dScalarMap;
+
+struct ParamsProcessorTrackerFeaturePolyline2d : public ParamsProcessorTrackerFeature
+{
+ ParamsProcessorTrackerFeaturePolyline2d() = default;
+ ParamsProcessorTrackerFeaturePolyline2d(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorTrackerFeature(_unique_name, _server)
+ {
+ // TODO write parser!
+ }
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorTrackerFeature::print();
+ // TODO write printer!
+ }
+
+ laserscanutils::LineFinderIterativeParams line_finder_params;
+ double match_feature_position_sq_norm_max;
+ double match_feature_orientation_sq_norm_max;
+ int match_feature_overlap_n_min;
+ int match_feature_overlap_n_defined_min;
+ double match_feature_overlap_dist_min;
+
+ double match_landmark_position_sq_norm_max;
+ double match_landmark_orientation_sq_norm_max;
+ int match_landmark_overlap_n_min;
+ int match_landmark_overlap_n_defined_min;
+ double match_landmark_overlap_dist_min;
+
+ bool use_probabilistic_match;
+
+ unsigned int new_features_th;
+ double loop_time_th;
+
+ std::vector<PolylineRectangularClass> polyline_classes;
+ double class_position_error_th;
+};
+
+class ProcessorTrackerFeaturePolyline2d : public ProcessorTrackerFeature
+{
+ protected:
+ ParamsProcessorTrackerFeaturePolyline2dPtr params_tracker_feature_polyline_;
+ laserscanutils::LineFinderIterative line_finder_;
+
+ FeatureBasePtrList untracked_features_incoming_, untracked_features_last_;
+ LandmarkMatchPolyline2dMap landmark_match_map_;
+ LandmarkPolyline2dPtrList modified_lmks_;
+ std::list<LandmarkPolyline2dPtrList> merge_candidates_list_;
+
+ Eigen::Matrix2d R_sensor_world_last_, R_world_sensor_last_;
+ Eigen::Vector2d t_sensor_world_last_, t_world_sensor_last_;
+ Eigen::Matrix2d R_sensor_world_incoming_, R_world_sensor_incoming_;
+ Eigen::Vector2d t_sensor_world_incoming_, t_world_sensor_incoming_;
+ Eigen::Matrix2d R_robot_sensor_;
+ Eigen::Vector2d t_robot_sensor_;
+ Eigen::Matrix2d R_last_incoming_, R_incoming_last_;
+ Eigen::Vector2d t_last_incoming_, t_incoming_last_;
+ bool extrinsics_transformation_computed_;
+
+ public:
+ ProcessorTrackerFeaturePolyline2d(ParamsProcessorTrackerFeaturePolyline2dPtr _params);
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeaturePolyline2d, ParamsProcessorTrackerFeaturePolyline2d);
+
+ ~ProcessorTrackerFeaturePolyline2d() override;
+
+ void configure(SensorBasePtr _sensor) override {};
+
+ protected:
+
+ /** \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
+ *
+ * \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 _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
+ */
+ 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 Process new 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 _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.
+ *
+ * The function is called in 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.
+ *
+ * Implement this method in derived classes.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override { return nullptr; };
+
+ /** \brief Establish factors between features in Captures \b last and \b origin
+ */
+ void establishFactors() override;
+
+ /** \brief Emplaces a new point to line factor
+ * \param _polyline_feature pointer to the feature
+ * \param _polyline_landmark pointer to the landmark
+ * \param _ftr_point_id index of the feature point
+ * \param _lmk_point_id intex of the landmark point
+ * \param _lmk_prev_point_id index of the landmark previous point
+ */
+ void emplaceFactorPointToLine(FeaturePolyline2dPtr _polyline_feature,
+ LandmarkPolyline2dPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id,
+ const int& _lmk_prev_point_id);
+
+ /** \brief Emplaces a new point to point factor
+ * \param _polyline_feature pointer to the feature
+ * \param _polyline_landmark pointer to the landmark
+ * \param _ftr_point_id index of the feature point
+ * \param _lmk_point_id intex of the landmark point
+ */
+ void emplaceFactorPoint(FeaturePolyline2dPtr _polyline_feature,
+ LandmarkPolyline2dPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id);
+
+ /** \brief emplace a landmark corresponding to a new feature
+ *
+ */
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+
+ /** \brief Modify a landmark and a landmark match according to a matched feature
+ * \param lmk_match landmark match
+ * \param pl_ftr feature
+ *
+ * Grow and/or define the extremes the landmark, according to the feature.
+ *
+ * \return if any modification was performed
+ */
+ bool modifyLandmarkAndMatch(LandmarkMatchPolyline2dPtr&);
+
+ /** \brief advance pointers
+ *
+ */
+ void advanceDerived() override;
+
+ /** \brief reset pointers and match lists at KF creation
+ *
+ */
+ void resetDerived() override;
+
+ /** \brief Pre-process
+ *
+ */
+ void preProcess() override;
+
+ /** \brief Post-process
+ *
+ */
+ void postProcess() override;
+
+ void tryMatchWithFeature(FeatureMatchPolyline2dScalarMap& ftr_matches,
+ const FeaturePolyline2dPtr& _ftr_last,
+ const FeaturePolyline2dPtr& _ftr_incoming,
+ const Eigen::Matrix3d& _T_last_incoming_prior) const;
+
+ void tryMatchWithLandmark(LandmarkMatchPolyline2dScalarMap& lmk_matches,
+ const LandmarkPolyline2dPtr& _lmk_ptr,
+ const FeaturePolyline2dPtr& _feat_ptr,
+ const Eigen::Matrix3d& _T_feature_landmark_prior) const;
+
+ bool updateLandmarkMatch(LandmarkMatchPolyline2dPtr&) const;
+
+ bool tryUpdateMatchTransformation(LandmarkMatchPolyline2dPtr&) const;
+
+ bool tryCompletePartialMatch(LandmarkMatchPolyline2dPtr& lmk_inc_match) const;
+
+ void computeTransformations();
+
+ /** \brief Concatenate feature_incoming-feature_last match with feature_last-landmark match
+ * \param pl_incoming polyline feature
+ * \param ftr_match_incoming_last match feature_incoming-feature_last
+ * \param lmk_match_last match feature_last-landmark
+ *
+ * \return the concatenated match: feature_incoming-landmark
+ */
+ LandmarkMatchPolyline2dPtr concatenateFeatureLandmarkMatches(FeatureMatchPolyline2dPtr ftr_match_incoming_last,
+ LandmarkMatchPolyline2dPtr lmk_match_last) const;
+
+ public:
+
+ const FeatureBasePtrList& getLastNewFeatures() const
+ {
+ return untracked_features_last_;
+ }
+};
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_2d_H_ */
diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h
deleted file mode 100644
index 89d3ce7687247615237b4bfcd0f1055d047c3ce7..0000000000000000000000000000000000000000
--- a/include/laser/sensor/sensor_laser_2D.h
+++ /dev/null
@@ -1,86 +0,0 @@
-// sensor_laser_2D.h
-
-#ifndef SENSOR_LASER_2D_H_
-#define SENSOR_LASER_2D_H_
-
-//wolf
-#include "core/sensor/sensor_base.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-
-// std
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsLaser2D);
-
-struct IntrinsicsLaser2D : public IntrinsicsBase
-{
- virtual ~IntrinsicsLaser2D() = default;
-
- laserscanutils::LaserScanParams scan_params;
-};
-
-WOLF_PTR_TYPEDEFS(SensorLaser2D);
-
-class SensorLaser2D : public SensorBase
-{
- protected:
- laserscanutils::LaserScanParams scan_params_;
-
- public:
- /** \brief Constructor with extrinsics
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
-
- /** \brief Constructor with extrinsics and scan parameters
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev);
-
- /** \brief Constructor with extrinsics and scan parameters
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _params Scan parameters
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
-
- virtual ~SensorLaser2D();
-
- void setDefaultScanParams();
-
- /** \brief Set scanner intrinsic parameters
- *
- * \param _params struct with scanner intrinsic parameters. See laser_scan_utils library API for reference.
- *
- **/
- void setScanParams(const laserscanutils::LaserScanParams & _params);
-
- /** \brief Get scanner intrinsic parameters
- *
- * Get scanner intrinsic parameters
- *
- **/
- const laserscanutils::LaserScanParams & getScanParams() const;
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBasePtr _intrinsics);
- static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml);
-
-};
-
-} // namespace wolf
-
-#endif /*SENSOR_LASER_2D_H_*/
diff --git a/include/laser/sensor/sensor_laser_2d.h b/include/laser/sensor/sensor_laser_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..5568961c21fe2049e5472759cb32ba28323b49cd
--- /dev/null
+++ b/include/laser/sensor/sensor_laser_2d.h
@@ -0,0 +1,135 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+// sensor_laser_2d.h
+
+#ifndef SENSOR_LASER_2d_H_
+#define SENSOR_LASER_2d_H_
+
+//wolf
+#include "core/sensor/sensor_base.h"
+
+//laser_scan_utils
+#include "laser_scan_utils/laser_scan.h"
+
+// std
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser2d);
+
+struct ParamsSensorLaser2d : public ParamsSensorBase
+{
+ ~ParamsSensorLaser2d() override = default;
+
+ laserscanutils::LaserScanParams scan_params;
+
+ ParamsSensorLaser2d() = default;
+ ParamsSensorLaser2d(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsSensorBase(_unique_name, _server)
+ {
+ scan_params.angle_min_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/angle_min");
+ scan_params.angle_max_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/angle_max");
+ scan_params.angle_step_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/angle_step");
+ scan_params.scan_time_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/scan_time");
+ scan_params.range_min_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/range_min");
+ scan_params.range_max_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/range_max");
+ scan_params.range_std_dev_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/range_std_dev");
+ scan_params.angle_std_dev_ = _server.getParam<double>(prefix + _unique_name + "/LaserScanParams/angle_std_dev");
+ }
+ std::string print() const override
+ {
+ return "\n" + ParamsSensorBase::print() + "\n"
+ + "LaserScanParams/angle_min: " + std::to_string(scan_params.angle_min_) + "\n"
+ + "LaserScanParams/angle_max: " + std::to_string(scan_params.angle_max_) + "\n"
+ + "LaserScanParams/angle_step: " + std::to_string(scan_params.angle_step_) + "\n"
+ + "LaserScanParams/scan_time: " + std::to_string(scan_params.scan_time_) + "\n"
+ + "LaserScanParams/range_min: " + std::to_string(scan_params.range_min_) + "\n"
+ + "LaserScanParams/range_max: " + std::to_string(scan_params.range_max_) + "\n"
+ + "LaserScanParams/range_std_dev: " + std::to_string(scan_params.range_std_dev_) + "\n"
+ + "LaserScanParams/angle_std_dev: " + std::to_string(scan_params.angle_std_dev_) + "\n";
+ }
+
+};
+
+WOLF_PTR_TYPEDEFS(SensorLaser2d);
+
+class SensorLaser2d : public SensorBase
+{
+ protected:
+ laserscanutils::LaserScanParams scan_params_;
+
+ public:
+ /** \brief Constructor with extrinsics
+ *
+ * \param _p_ptr StateBlock pointer to the sensor position
+ * \param _o_ptr StateBlock pointer to the sensor orientation
+ *
+ **/
+ SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
+
+ /** \brief Constructor with extrinsics and scan parameters
+ *
+ * \param _p_ptr StateBlock pointer to the sensor position
+ * \param _o_ptr StateBlock pointer to the sensor orientation
+ *
+ **/
+ SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev);
+
+ /** \brief Constructor with extrinsics and scan parameters
+ *
+ * \param _p_ptr StateBlock pointer to the sensor position
+ * \param _o_ptr StateBlock pointer to the sensor orientation
+ * \param _params Scan parameters
+ *
+ **/
+ SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
+ SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const ParamsSensorLaser2d& _params);
+ SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, ParamsSensorLaser2dPtr _params);
+ SensorLaser2d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser2dPtr _params);
+ WOLF_SENSOR_CREATE(SensorLaser2d, ParamsSensorLaser2d, 3);
+
+ ~SensorLaser2d() override;
+
+ void setDefaultScanParams();
+
+ /** \brief Set scanner intrinsic parameters
+ *
+ * \param _params struct with scanner intrinsic parameters. See laser_scan_utils library API for reference.
+ *
+ **/
+ void setScanParams(const laserscanutils::LaserScanParams & _params);
+
+ /** \brief Get scanner intrinsic parameters
+ *
+ * Get scanner intrinsic parameters
+ *
+ **/
+ const laserscanutils::LaserScanParams & getScanParams() const;
+
+ public:
+// static ParamsSensorBasePtr createParams(const std::string& _filename_dot_yaml);
+
+};
+
+} // namespace wolf
+
+#endif /*SENSOR_LASER_2d_H_*/
diff --git a/include/laser/state_block/local_parametrization_polyline_extreme.h b/include/laser/state_block/local_parametrization_polyline_extreme.h
index ce025e7404b0cde7f67a3521255b3f6597302b36..f1857ce7b8877f5db0d1ac146803b76c6b180b01 100644
--- a/include/laser/state_block/local_parametrization_polyline_extreme.h
+++ b/include/laser/state_block/local_parametrization_polyline_extreme.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
/*
* \file local_parametrization_polyline_extreme.h
*
@@ -22,15 +43,20 @@ class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
StateBlockPtr reference_point_;
public:
LocalParametrizationPolylineExtreme(StateBlockPtr _reference_point);
- virtual ~LocalParametrizationPolylineExtreme();
-
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta);
+ ~LocalParametrizationPolylineExtreme() override;
+
+ bool plus(Eigen::Map<const Eigen::VectorXd>& _point,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _point_plus_delta_theta) const override;
+ bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
+ bool minus(Eigen::Map<const Eigen::VectorXd>& _point1,
+ Eigen::Map<const Eigen::VectorXd>& _point2,
+ Eigen::Map<Eigen::VectorXd>& _delta_theta) override;
+ bool isValid(const Eigen::VectorXd& _state, double tolerance) override
+ {
+ return _state.size() == global_size_;
+ }
+
};
} // namespace wolf
diff --git a/internal/config.h.in b/internal/config.h.in
index 7c0fe4756e443695c5450cfb46054e8b7cd107a7..7014ca320e5c5fea1f59a6fe5d9d1ea0987bf95e 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -24,13 +24,13 @@
// which will be added to the include path for compilation,
// and installed with the public wolf headers.
-#ifndef WOLF_INTERNAL_CONFIG_H_
-#define WOLF_INTERNAL_CONFIG_H_
+#ifndef WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
+#define WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
#cmakedefine _WOLF_DEBUG
#cmakedefine _WOLF_TRACE
-#define _WOLF_ROOT_DIR "${_WOLF_ROOT_DIR}"
+#define _WOLF_${UPPER_NAME}_ROOT_DIR "${_WOLF_ROOT_DIR}"
#endif /* WOLF_INTERNAL_CONFIG_H_ */
diff --git a/license_header_2022.txt b/license_header_2022.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0c987025f9dba3e7af993051b9bdf4b5ff400e0d
--- /dev/null
+++ b/license_header_2022.txt
@@ -0,0 +1,17 @@
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
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/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index 78c8303e47195a85b6e6f3c3b84fed68a7959f02..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,714 +0,0 @@
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
-IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
- set(_WOLF_DEBUG true)
-ENDIF()
-
-option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-
-option(BUILD_EXAMPLES "Build examples" OFF)
-set(BUILD_TESTS true)
-
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-#find dependencies.
-
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
-
-FIND_PACKAGE(Threads REQUIRED)
-
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
- FIND_PACKAGE(GLUT REQUIRED)
- FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
-IF(laser_scan_utils_FOUND)
- MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
-ENDIF(laser_scan_utils_FOUND)
-
-FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
-IF(raw_gps_utils_FOUND)
- MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision Utils
-FIND_PACKAGE(vision_utils QUIET)
-IF (vision_utils_FOUND)
- MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
- SET(PRINT_INFO_VU false)
- FIND_PACKAGE(OpenCV QUIET)
-ENDIF(vision_utils_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
-# YAML with yaml-cpp
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
-IF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-ELSEIF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library NOT FOUND!")
-ENDIF(YAMLCPP_FOUND)
-
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
- MESSAGE("glog Library FOUND: glog related sources will be built.")
- MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
- MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
- MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
-FIND_PATH(
- Suitesparse_INCLUDE_DIRS
- NAMES SuiteSparse_config.h
- PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND TRUE)
- MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND FALSE)
- MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
-
-# Define the directory where will be the configured config.h
-SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/internal)
-
-# Create the specified output directory if it does not exist.
-IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
- message(STATUS "Creating config output directory: ${WOLF_CONFIG_DIR}")
- file(MAKE_DIRECTORY "${WOLF_CONFIG_DIR}")
-ENDIF()
-IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
- message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
- "${WOLF_CONFIG_DIR} exists, but is not a directory.")
-ENDIF()
-# Configure config.h
-configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-
-#TEMPORAL INCLUDE UNTIL WE FIGURE OUT WHAT TO DO WITH FILES in src/temp
-
-include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-include_directories(../dummyInclude)
-
-# Include config.h directory at first.
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-INCLUDE_DIRECTORIES(.)
-
-# include spdlog (logging library)
-FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
-IF (SPDLOG_INCLUDE_DIR)
- INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
- MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
-ELSE (SPDLOG_INCLUDE_DIR)
- MESSAGE(FATAL_ERROR "Could not find spdlog")
-ENDIF (SPDLOG_INCLUDE_DIR)
-
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-IF(laser_scan_utils_FOUND)
- INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-ENDIF(laser_scan_utils_FOUND)
-
-IF(raw_gps_utils_FOUND)
- INCLUDE_DIRECTORIES(${raw_gps_utils_INCLUDE_DIRS})
-ENDIF(raw_gps_utils_FOUND)
-
-IF(vision_utils_FOUND)
- INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(vision_utils_FOUND)
-
-# cereal
-IF(cereal_FOUND)
- INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-
-IF(Suitesparse_FOUND)
- INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(YAMLCPP_FOUND)
- INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
-IF(GLOG_FOUND)
-INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
-
-#headers
-SET(HDRS_BASE
-capture/capture_motion.h
- eigen_assert.h
- eigen_predicates.h
-landmark/landmark_match.h
- make_unique.h
- pinhole_tools.h
-processor/processor_capture_holder.h
-processor/processor_tracker_landmark.h
- )
-SET(HDRS
-capture/capture_motion.h
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_AHP.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- temp/diff_drive_tools.h
- temp/diff_drive_tools.hpp
-feature/feature_corner_2D.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- IMU_tools.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- local_parametrization_polyline_extreme.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
- test/processor_IMU_UnitTester.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark_dummy.h
-sensor/sensor_camera.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
- SET(HDRS_CAPTURE
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-capture/capture_velocity.h
-capture/capture_wheel_joint_position.h
- )
- SET(HDRS_CONSTRAINT
-factor/factor_autodiff_trifocal.h
-factor/factor_autodiff_distance_3D.h
-factor/factor_AHP.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_diff_drive.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- )
- SET(HDRS_FEATURE
-feature/feature_corner_2D.h
-feature/feature_diff_drive.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- )
- SET(HDRS_LANDMARK
-landmark/landmark_match.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- )
- SET(HDRS_PROCESSOR
-processor/processor_capture_holder.h
-processor/processor_diff_drive.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark.h
-processor/processor_tracker_landmark_dummy.h
- )
- SET(HDRS_SENSOR
-sensor/sensor_camera.h
-sensor/sensor_diff_drive.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
-# [Add generic derived header before this line]
-
-SET(HDRS_DTASSC
- data_association/matrix.h
- data_association/association_solver.h
- data_association/association_node.h
- data_association/association_tree.h
- data_association/association_nnls.h
- )
-
-SET(HDRS_CORE
-core/capture_base.h
-core/capture_buffer.h
-core/capture_pose.h
-core/capture_void.h
-core/factor_analytic.h
-core/factor_autodiff.h
-core/factor_base.h
-core/factory.h
-core/feature_base.h
-core/feature_match.h
-core/feature_pose.h
-core/frame_base.h
-core/hardware_base.h
-core/landmark_base.h
-core/local_parametrization_angle.h
-core/local_parametrization_base.h
-core/local_parametrization_homogeneous.h
-core/local_parametrization_quaternion.h
-core/logging.h
-core/map_base.h
-core/motion_buffer.h
-core/node_base.h
-core/problem.h
-core/processor_base.h
-core/factory.h
-core/processor_loopclosure_base.h
-core/processor_motion.h
-core/processor_tracker_feature.h
-core/processor_tracker.h
-core/rotations.h
-core/sensor_base.h
-core/factory.h
-core/singleton.h
-core/state_angle.h
-core/state_block.h
-core/state_homogeneous_3D.h
-core/state_quaternion.h
-core/three_D_tools.h
-core/time_stamp.h
-core/track_matrix.h
-core/trajectory_base.h
-core/wolf.h
- )
-SET(SRCS_CORE
-core/capture_base.cpp
-core/capture_pose.cpp
-core/capture_void.cpp
-core/factor_analytic.cpp
-core/factor_base.cpp
-core/feature_base.cpp
-core/feature_pose.cpp
-core/frame_base.cpp
-core/hardware_base.cpp
-core/landmark_base.cpp
-core/local_parametrization_base.cpp
-core/local_parametrization_homogeneous.cpp
-core/local_parametrization_quaternion.cpp
-core/map_base.cpp
-core/motion_buffer.cpp
-core/node_base.cpp
-core/problem.cpp
-core/processor_base.cpp
-core/processor_loopclosure_base.cpp
-core/processor_motion.cpp
-core/processor_tracker.cpp
-core/processor_tracker_feature.cpp
-core/sensor_base.cpp
-core/state_block.cpp
-core/time_stamp.cpp
-core/track_matrix.cpp
-core/trajectory_base.cpp
- )
-#sources
-SET(SRCS_BASE
-capture/capture_motion.cpp
-processor/processor_capture_holder.cpp
-processor/processor_tracker_landmark.cpp
- )
-
-SET(SRCS
- local_parametrization_polyline_extreme.cpp
- test/processor_IMU_UnitTester.cpp
- )
- SET(SRCS_CAPTURE
-capture/capture_GPS_fix.cpp
-capture/capture_IMU.cpp
-capture/capture_odom_2D.cpp
-capture/capture_odom_3D.cpp
-capture/capture_velocity.cpp
-capture/capture_wheel_joint_position.cpp
- )
- SET(SRCS_FEATURE
-feature/feature_corner_2D.cpp
-feature/feature_diff_drive.cpp
-feature/feature_GPS_fix.cpp
-feature/feature_GPS_pseudorange.cpp
-feature/feature_IMU.cpp
-feature/feature_odom_2D.cpp
-feature/feature_polyline_2D.cpp
- )
- SET(SRCS_LANDMARK
-landmark/landmark_corner_2D.cpp
-landmark/landmark_container.cpp
-landmark/landmark_line_2D.cpp
-landmark/landmark_polyline_2D.cpp
- )
- SET(SRCS_PROCESSOR
-processor/processor_frame_nearest_neighbor_filter.cpp
-processor/processor_diff_drive.cpp
-processor/processor_IMU.cpp
-processor/processor_odom_2D.cpp
-processor/processor_odom_3D.cpp
-processor/processor_tracker_feature_dummy.cpp
-processor/processor_tracker_landmark_dummy.cpp
- )
- SET(SRCS_SENSOR
-sensor/sensor_camera.cpp
-sensor/sensor_diff_drive.cpp
-sensor/sensor_GPS.cpp
-sensor/sensor_GPS_fix.cpp
-sensor/sensor_IMU.cpp
-sensor/sensor_odom_2D.cpp
-sensor/sensor_odom_3D.cpp
- )
-SET(SRCS_DTASSC
- data_association/association_solver.cpp
- data_association/association_node.cpp
- data_association/association_tree.cpp
- data_association/association_nnls.cpp
- )
-
-# Add the solver sub-directory
-add_subdirectory(solver)
-
-#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
- ceres_wrapper/sparse_utils.h
- #ceres_wrapper/solver_manager.h
- ceres_wrapper/ceres_manager.h
- #ceres_wrapper/qr_manager.h
- ceres_wrapper/cost_function_wrapper.h
- ceres_wrapper/create_numeric_diff_cost_function.h
- ceres_wrapper/local_parametrization_wrapper.h
- )
- SET(SRCS_WRAPPER
- #ceres_wrapper/solver_manager.cpp
- ceres_wrapper/ceres_manager.cpp
- #ceres_wrapper/qr_manager.cpp
- ceres_wrapper/local_parametrization_wrapper.cpp
- )
-ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_laser_2D.h
-sensor/sensor_laser_2D.h
-processor/processor_tracker_feature_corner.h
-processor/processor_tracker_landmark_corner.h
-processor/processor_tracker_landmark_polyline.h
- )
- SET(SRCS ${SRCS}
-capture/capture_laser_2D.cpp
-sensor/sensor_laser_2D.cpp
-processor/processor_tracker_feature_corner.cpp
-processor/processor_tracker_landmark_corner.cpp
-processor/processor_tracker_landmark_polyline.cpp
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_GPS.h
-processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
-capture/capture_GPS.cpp
-processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_image.h
-feature/feature_point_image.h
-landmark/landmark_AHP.h
-processor/processor_params_image.h
-processor/processor_tracker_feature_image.h
-processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
-landmark/landmark_point_3D.h
- )
- SET(SRCS ${SRCS}
-capture/capture_image.cpp
-feature/feature_point_image.cpp
-landmark/landmark_AHP.cpp
-processor/processor_tracker_feature_image.cpp
-processor/processor_tracker_landmark_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
-landmark/landmark_point_3D.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.cpp
- )
-ENDIF(vision_utils_FOUND)
-
-# Add the capture sub-directory
-# ADD_SUBDIRECTORY(captures)
-
-# Add the factors sub-directory
-# ADD_SUBDIRECTORY(factors)
-
-# Add the features sub-directory
-# ADD_SUBDIRECTORY(features)
-
-# Add the landmarks sub-directory
-# ADD_SUBDIRECTORY(landmarks)
-
-# Add the processors sub-directory
-# ADD_SUBDIRECTORY(processors)
-
-# Add the sensors sub-directory
-# ADD_SUBDIRECTORY(sensors)
-
-# Add the hello_wolf sub-directory
-ADD_SUBDIRECTORY(hello_wolf)
-
-IF (cereal_FOUND)
- ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
-IF (Suitesparse_FOUND)
- ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS ${HDRS}
- yaml/yaml_conversion.h
- )
-
- # sources
- SET(SRCS ${SRCS}
- yaml/processor_odom_3D_yaml.cpp
- yaml/processor_IMU_yaml.cpp
- yaml/sensor_camera_yaml.cpp
- yaml/sensor_odom_3D_yaml.cpp
- yaml/sensor_IMU_yaml.cpp
- )
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/processor_image_yaml.cpp
- yaml/processor_tracker_feature_trifocal_yaml.cpp
- )
- ENDIF(vision_utils_FOUND)
-ENDIF(YAMLCPP_FOUND)
-
-# create the shared library
-add_library(${PROJECT_NAME}_core SHARED
- ${SRCS_CORE}
- ${SRCS_BASE}
- )
-ADD_LIBRARY(${PROJECT_NAME}
- SHARED
- ${SRCS_BASE}
- ${SRCS}
- ${SRCS_CAPTURE}
- ${SRCS_CONSTRAINT}
- ${SRCS_FEATURE}
- ${SRCS_LANDMARK}
- ${SRCS_PROCESSOR}
- ${SRCS_SENSOR}
- #${SRCS_DTASSC}
- ${SRCS_SOLVER}
- ${SRCS_WRAPPER}
- )
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
-
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core)
-#Link the created libraries
-#=============================================================
-IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${raw_gps_utils_LIBRARY})
-ENDIF (raw_gps_utils_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
- IF (vision_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${vision_utils_LIBRARY})
- ENDIF (vision_utils_FOUND)
-ENDIF (OPENCV_FOUND)
-
-IF (YAMLCPP_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
-ENDIF (YAMLCPP_FOUND)
-
-IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
-
-#install library
-install(TARGETS ${PROJECT_NAME}_core DESTINATION lib/iri-algorithms EXPORT ${PROJECT_NAME}_core-targets)
-install(EXPORT ${PROJECT_NAME}_core-targets DESTINATION lib/iri-algorithms)
-#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
-
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
-
-#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS_CORE}
- DESTINATION include/iri-algorithms/wolf/core)
-#INSTALL(FILES ${HDRS_DTASSC}
-# DESTINATION include/iri-algorithms/wolf/data_association)
-INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/capture)
-INSTALL(FILES ${HDRS_CONSTRAINT}
- DESTINATION include/iri-algorithms/wolf/factor)
-INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/processor)
-INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/serialization)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
- DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
-
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-DESTINATION include/iri-algorithms/wolf/internal)
-
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
-export(PACKAGE ${PROJECT_NAME})
-
-#############
-## Testing ##
-#############
-IF (GLOG_FOUND)
- IF(BUILD_TESTS)
- MESSAGE("Building tests.")
- add_subdirectory(test)
- ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
-
-IF(BUILD_EXAMPLES)
- #Build examples
- MESSAGE("Building examples.")
- ADD_SUBDIRECTORY(examples)
-ENDIF(BUILD_EXAMPLES)
-
diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp
deleted file mode 100644
index 6f49fdd2ef93d1a034638d51ac56fb91dc2396d2..0000000000000000000000000000000000000000
--- a/src/capture/capture_laser_2D.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "laser/capture/capture_laser_2D.h"
-
-namespace wolf {
-
-CaptureLaser2D::CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges) :
- CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2D>(getSensor())), scan_(_ranges)
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/capture/capture_laser_2d.cpp b/src/capture/capture_laser_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..407b9a845abfd2d280be5495baf35753e4383803
--- /dev/null
+++ b/src/capture/capture_laser_2d.cpp
@@ -0,0 +1,32 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/capture/capture_laser_2d.h"
+
+namespace wolf {
+
+CaptureLaser2d::CaptureLaser2d(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges) :
+ CaptureBase("CaptureLaser2d", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2d>(getSensor())), scan_(_ranges)
+{
+ //
+}
+
+} // namespace wolf
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
deleted file mode 100644
index 836caa3d1c337a37eb8680871139251a371353a2..0000000000000000000000000000000000000000
--- a/src/examples/CMakeLists.txt
+++ /dev/null
@@ -1,223 +0,0 @@
-# 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})
-
- 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)
diff --git a/src/examples/Test_ORB.png b/src/examples/Test_ORB.png
deleted file mode 100644
index 016141f5309c1ed34a61d71cfa63b130ea90aa8f..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_ORB.png and /dev/null differ
diff --git a/src/examples/Test_gazebo_x-10cm_y-20cm.jpg b/src/examples/Test_gazebo_x-10cm_y-20cm.jpg
deleted file mode 100644
index 5ad1c83e82ec99b1493914d216bf9fc47c1120ed..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x-10cm_y-20cm.jpg and /dev/null differ
diff --git a/src/examples/Test_gazebo_x-10cm_y00cm.jpg b/src/examples/Test_gazebo_x-10cm_y00cm.jpg
deleted file mode 100644
index 81d004c6643f33130babc65cf439e846b2a112d5..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x-10cm_y00cm.jpg and /dev/null differ
diff --git a/src/examples/Test_gazebo_x-20cm_y-10cm.jpg b/src/examples/Test_gazebo_x-20cm_y-10cm.jpg
deleted file mode 100644
index 9b6f60ad8ffb895a94cc6873a061e0810f2d1dbc..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x-20cm_y-10cm.jpg and /dev/null differ
diff --git a/src/examples/Test_gazebo_x-20cm_y-20cm.jpg b/src/examples/Test_gazebo_x-20cm_y-20cm.jpg
deleted file mode 100644
index 3c15fc66bdd164d15ff55b8647dccd035bcdb3f1..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x-20cm_y-20cm.jpg and /dev/null differ
diff --git a/src/examples/Test_gazebo_x-20cm_y00cm.jpg b/src/examples/Test_gazebo_x-20cm_y00cm.jpg
deleted file mode 100644
index b94fdeef33fba7191105f013ace71c5717a1eefb..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x-20cm_y00cm.jpg and /dev/null differ
diff --git a/src/examples/Test_gazebo_x00cm_y-10cm.jpg b/src/examples/Test_gazebo_x00cm_y-10cm.jpg
deleted file mode 100644
index 64637cf8350e0bceeeb74ba6be05b9a70353c879..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x00cm_y-10cm.jpg and /dev/null differ
diff --git a/src/examples/Test_gazebo_x00cm_y-20cm.jpg b/src/examples/Test_gazebo_x00cm_y-20cm.jpg
deleted file mode 100644
index 954c20701c8db1abcd49becfad0448f94e475619..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x00cm_y-20cm.jpg and /dev/null differ
diff --git a/src/examples/Test_gazebo_x00cm_y00cm.jpg b/src/examples/Test_gazebo_x00cm_y00cm.jpg
deleted file mode 100644
index 6935015a05b35aa74fee175c52b9b3f88af0c9f0..0000000000000000000000000000000000000000
Binary files a/src/examples/Test_gazebo_x00cm_y00cm.jpg and /dev/null differ
diff --git a/src/examples/camera_params.yaml b/src/examples/camera_params.yaml
deleted file mode 100644
index de0c31d0b05a024724840598a490527a62002ca2..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/camera_params_1280x960_ideal.yaml b/src/examples/camera_params_1280x960_ideal.yaml
deleted file mode 100644
index 814d3cb3f74d51925e62389366ef0a24d429ee44..0000000000000000000000000000000000000000
--- a/src/examples/camera_params_1280x960_ideal.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 1280
-image_height: 960
-camera_name: canonical
-camera_matrix:
- rows: 3
- cols: 3
- data: [640, 0, 640, 0, 640, 480, 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: [640, 0, 640, 0, 0, 640, 480, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/src/examples/camera_params_canonical.yaml b/src/examples/camera_params_canonical.yaml
deleted file mode 100644
index 370b662c9b958d0c4ab528df8ab793567141f685..0000000000000000000000000000000000000000
--- a/src/examples/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, 1, 0, 0, 1, 1, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/src/examples/camera_params_ueye.yaml b/src/examples/camera_params_ueye.yaml
deleted file mode 100644
index 8703fc4575d1422190e318fb7e0f8a816d37e5cb..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/camera_params_ueye_radial_dist.yaml b/src/examples/camera_params_ueye_radial_dist.yaml
deleted file mode 100644
index 52d1fbb4dd01de04e6c759f0850015509235f4b4..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/camera_params_ueye_sim.yaml b/src/examples/camera_params_ueye_sim.yaml
deleted file mode 100644
index 97f8f676b6b36629bb76a8bdc8c3fe06250b7b5b..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/camera_params_vga_ideal.yaml b/src/examples/camera_params_vga_ideal.yaml
deleted file mode 100644
index c0c21118a4d69765f3d86471a7ff4003afc64dc4..0000000000000000000000000000000000000000
--- a/src/examples/camera_params_vga_ideal.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: canonical
-camera_matrix:
- rows: 3
- cols: 3
- data: [320, 0, 320, 0, 320, 240, 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: [320, 0, 320, 0, 0, 320, 240, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/src/examples/input_M3500b_toro.graph b/src/examples/input_M3500b_toro.graph
deleted file mode 100644
index 21d4f410448f092466196e433705f6a96463e0a1..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/map_polyline_example.yaml b/src/examples/map_polyline_example.yaml
deleted file mode 100644
index d1cde10c538cfa7e6f5b6a4b117309e36426528d..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/processor_image_feature.yaml b/src/examples/processor_image_feature.yaml
deleted file mode 100644
index a48c9e4277b483a0f67e4f856f72c0076a2cc2be..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/processor_image_vision_utils.yaml b/src/examples/processor_image_vision_utils.yaml
deleted file mode 100644
index 028f69ba6321802009765d346f2e735c6a018990..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
deleted file mode 100644
index 070f92f9a45e1ff8be6592fbcdca374a26b92b8d..0000000000000000000000000000000000000000
--- a/src/examples/processor_imu.yaml
+++ /dev/null
@@ -1,10 +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.001 # 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/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
deleted file mode 100644
index e2c9c2afceb37a38316c92b93bd9a419cf275a4c..0000000000000000000000000000000000000000
--- a/src/examples/processor_imu_no_vote.yaml
+++ /dev/null
@@ -1,10 +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.001 # Time tolerance for joining KFs
-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/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
deleted file mode 100644
index 76d903cef79b11e701cf4e577942154cc56030c7..0000000000000000000000000000000000000000
--- a/src/examples/processor_imu_t1.yaml
+++ /dev/null
@@ -1,10 +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.001 # 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/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
deleted file mode 100644
index 653e1e847e26d95c467170c96fdcc0cf6b145c1a..0000000000000000000000000000000000000000
--- a/src/examples/processor_imu_t6.yaml
+++ /dev/null
@@ -1,10 +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.001 # 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/src/examples/processor_odom_3D.yaml b/src/examples/processor_odom_3D.yaml
deleted file mode 100644
index aff35c9d2732c6489c1506021874f5325f303678..0000000000000000000000000000000000000000
--- a/src/examples/processor_odom_3D.yaml
+++ /dev/null
@@ -1,7 +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
-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/src/examples/sensor_imu.yaml b/src/examples/sensor_imu.yaml
deleted file mode 100644
index 66b81a5288877362753612f7aa4b9222da009e8d..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/sensor_odom_3D.yaml b/src/examples/sensor_odom_3D.yaml
deleted file mode 100644
index 9ea77803548cfd9d033f54e40b6d92a72710afb8..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/sensor_odom_3D_HQ.yaml b/src/examples/sensor_odom_3D_HQ.yaml
deleted file mode 100644
index 08945ef842e46f28642f1be63ca95850b618a35e..0000000000000000000000000000000000000000
--- a/src/examples/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/src/examples/solver/test_SPQR.cpp b/src/examples/solver/test_SPQR.cpp
deleted file mode 100644
index 04592dbd047fed1d4b1d577c2df53cd5bbe57766..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_SPQR.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * test_SPQR.cpp
- *
- * Created on: Jun 18, 2015
- * Author: jvallve
- */
-
-#include <iostream>
-#include <eigen3/Eigen/SPQRSupport>
-#include <eigen3/Eigen/CholmodSupport>
-#include "SuiteSparseQR.hpp"
-
-using namespace Eigen;
-
-int main (int argc, char **argv)
-{
- ///////////////////////////////////////////////////////////////////////
- // Eigen Support SPQR
- SPQR < SparseMatrix<double> > solver;
- //solver.setSPQROrdering(0); // no ordering -> segmentation fault
-
- SparseMatrix<double> matA(4,3);
- matA.coeffRef(0,0) = 0.1;
- matA.coeffRef(1,0) = 0.4;
- matA.coeffRef(1,1) = 0.2;
- matA.coeffRef(2,1) = 0.4;
- matA.coeffRef(2,2) = 0.2;
- matA.coeffRef(3,2) = 0.1;
-
- std::cout << "matA: " << std::endl << matA << std::endl;
-
- VectorXd b_ = VectorXd::Ones(4);
- VectorXd x_(3);
-
- std::cout << "b_: " << std::endl << b_ << std::endl;
-
- solver.compute(matA);
- if (solver.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- std::cout << "R: " << std::endl << solver.matrixR() << std::endl;
- x_ = solver.solve(b_);
- std::cout << "solved x_" << std::endl << x_ << std::endl;
- std::cout << "ordering: " << solver.colsPermutation().indices().transpose() << std::endl;
-
- ///////////////////////////////////////////////////////////////////////
- // Directly in suitesparse
- cholmod_common Common, *cc ;
- cholmod_sparse A ;
- cholmod_dense *X, *B, *Residual ;
- double rnorm, one [2] = {1,0}, minusone [2] = {-1,0} ;
- int mtype ;
-
- // start CHOLMOD
- cc = &Common ;
- cholmod_l_start (cc) ;
-
- // load A
- A = viewAsCholmod(matA);
- //A = (cholmod_sparse *) cholmod_l_read_matrix (stdin, 1, &mtype, cc) ;
- std::cout << "A.xtype " << A.xtype << std::endl;
- std::cout << "A.nrow " << A.nrow << std::endl;
- std::cout << "A.ncol " << A.ncol << std::endl;
-
- // B = ones (size (A,1),1)
- B = cholmod_l_ones (A.nrow, 1, A.xtype, cc) ;
-
- std::cout << "2" << std::endl;
- // X = A\B
- //X = SuiteSparseQR <double> (0, SPQR_DEFAULT_TOL, &A, B, cc) ;
- X = SuiteSparseQR <double> (&A, B, cc);
-
- std::cout << "3" << std::endl;
- // rnorm = norm (B-A*X)
- Residual = cholmod_l_copy_dense (B, cc) ;
- std::cout << "4" << std::endl;
- cholmod_l_sdmult (&A, 0, minusone, one, X, Residual, cc) ;
- std::cout << "5" << std::endl;
- rnorm = cholmod_l_norm_dense (Residual, 2, cc) ;
- printf ("2-norm of residual: %8.1e\n", rnorm) ;
- printf ("rank %ld\n", cc->SPQR_istat [4]) ;
-
- // free everything and finish CHOLMOD
- cholmod_l_free_dense (&Residual, cc) ;
- //cholmod_l_free_sparse (A, cc) ;
- cholmod_l_free_dense (&X, cc) ;
- cholmod_l_free_dense (&B, cc) ;
- cholmod_l_finish (cc) ;
- return (0) ;
-}
diff --git a/src/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
deleted file mode 100644
index 48831f6511f2d29689ced193e599dc4ae6292a5f..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_ccolamd.cpp
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * test_ccolamd.cpp
- *
- * Created on: Jun 11, 2015
- * Author: jvallve
- */
-
-// Wolf includes
-#include "core/common/wolf.h"
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
-
-using namespace Eigen;
-using namespace wolf;
-
-//main
-int main(int argc, char *argv[])
-{
- if (argc != 2 || atoi(argv[1]) < 1)
- {
- std::cout << "Please call me with: [./test_ccolamd SIZE], where:" << std::endl;
- std::cout << " - SIZE: integer size of the problem" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- SizeEigen size = atoi(argv[1]);
-
- SparseMatrix<double, ColMajor, SizeEigen> A(size, size), Aordered(size, size);
- CholmodSupernodalLLT < SparseMatrix<double, ColMajor, SizeEigen> > solver;
- PermutationMatrix<Dynamic, Dynamic, SizeEigen> perm(size);
- CCOLAMDOrdering<SizeEigen> ordering;
- Matrix<SizeEigen, Dynamic, 1> ordering_factors = Matrix<SizeEigen, Dynamic, 1>::Ones(size);
- VectorXd b(size), bordered(size), xordered(size), x(size);
- clock_t t1, t2, t3;
- double time1, time2, time3;
-
- // BUILD THE PROBLEM ----------------------------
- //Fill A & b
- A.insert(0, 0) = 5;
- b(0) = 1;
- for (int i = 1; i < size; i++)
- {
- A.insert(i, i) = 5;
- A.insert(i, i - 1) = 1;
- A.insert(i - 1, i) = 1;
- b(i) = i + 1;
- }
- A.insert(size - 1, 0) = 2;
- A.insert(0, size - 1) = 2;
-
- std::cout << "Solving Ax = b:" << std::endl << "A = " << std::endl << A << std::endl << std::endl;
- std::cout << "b = " << std::endl << b.transpose() << std::endl << std::endl;
-
- // SOLVING WITHOUT REORDERING ------------------------------------
- // solve Ax = b
- t1 = clock();
- solver.compute(A);
- if (solver.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x = solver.solve(b);
- time1 = ((double) clock() - t1) / CLOCKS_PER_SEC;
- std::cout << "solved in " << time1 << "seconds" << std::endl;
- std::cout << "x = " << x.transpose() << std::endl;
-
- // SOLVING AFTER REORDERING ------------------------------------
- // ordering factors
- ordering_factors(size-1) = 2;
- ordering_factors(0) = 2;
-
- // ordering
- t2 = clock();
- A.makeCompressed();
-
- std::cout << "Reordering using CCOLAMD:" << std::endl;
- std::cout << "ordering_factors = " << std::endl << ordering_factors.transpose() << std::endl << std::endl;
- ordering(A, perm, ordering_factors.data());
- std::cout << "perm = " << std::endl << perm.indices().transpose() << std::endl << std::endl;
-
- bordered = perm * b;
- Aordered = A.twistedBy(perm);
- std::cout << "reordered A = " << std::endl << Aordered * MatrixXd::Identity(size, size) << std::endl << std::endl;
- std::cout << "reordered b = " << std::endl << bordered.transpose() << std::endl << std::endl;
-
- // solve Ax = b
- solver.compute(Aordered);
- if (solver.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- xordered = solver.solve(bordered);
- time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
- std::cout << "solved in " << time2 << "seconds" << std::endl;
- std::cout << "x = " << (perm.inverse() * xordered).transpose() << std::endl;
- std::cout << "x = " << x.transpose() << " (solution without reordering)" << std::endl;
-
- // SOLVING AND REORDERING ------------------------------------
- t3 = clock();
- SparseLU<SparseMatrix<double, ColMajor, SizeEigen>, CCOLAMDOrdering<SizeEigen> > solver2;
- solver2.compute(A);
- if (solver2.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x = solver2.solve(b);
- time3 = ((double) clock() - t3) / CLOCKS_PER_SEC;
- std::cout << "solved in " << time3 << "seconds" << std::endl;
- std::cout << "x = " << x.transpose() << std::endl;
-}
-
diff --git a/src/examples/solver/test_ccolamd_blocks.cpp b/src/examples/solver/test_ccolamd_blocks.cpp
deleted file mode 100644
index 83cc7af8407282484ba5aa26dc66eec59a30bb11..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_ccolamd_blocks.cpp
+++ /dev/null
@@ -1,180 +0,0 @@
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
-{
- for (unsigned int i = row; i < row + Nrows; i++)
- for (unsigned int j = col; j < row + Ncols; j++)
- original.coeffRef(i,j) = 0.0;
-
- original.makeCompressed();
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
-{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size)
-{
- ArrayXXi idx(dim, size);
- idx.row(0) = dim * perm.indices().transpose();
- for (int i = 1; i<dim; i++)
- idx.row(i) = idx.row(i-1) + 1;
- Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1);
- perm_blocks.indices() = idx_blocks;
-}
-
-//main
-int main(int argc, char *argv[])
-{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
- {
- std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl;
- std::cout << " - SIZE: integer size of the problem" << std::endl;
- std::cout << " - DIM: integer dimension of the nodes" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
-
- SparseMatrix<double> H(size * dim, size * dim), H_ordered(size * dim, size * dim), H_block_ordered(size * dim, size * dim);
- SparseMatrix<int> FactorMatrix(size,size);
- CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
- PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim);
- CCOLAMDOrdering<int> ordering;
- VectorXi block_ordering_factors = VectorXi::Ones(size);
- VectorXi ordering_factors = VectorXi::Ones(size*dim);
- VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim), x_ordered(size * dim), x(size * dim);
- clock_t t1, t2, t3;
- double time1, time2, time3;
-
- MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
-
- // BUILD THE PROBLEM ----------------------------
- //Fill H & b
- for (int i = 0; i < size; i++)
- {
- addSparseBlock(5*omega, H, i*dim, i*dim);
- FactorMatrix.insert(i,i) = 1;
- if (i > 0)
- {
- addSparseBlock(omega, H, i*dim, (i-1)*dim);
- addSparseBlock(omega, H, (i-1)*dim, i*dim);
- FactorMatrix.insert(i,i-1) = 1;
- FactorMatrix.insert(i-1,i) = 1;
- }
- b.segment(i*dim, dim) = VectorXd::Constant(dim, i+1);
- }
- addSparseBlock(2*omega, H, 0, (size - 1)*dim);
- addSparseBlock(2*omega, H, (size-1)*dim, 0);
- FactorMatrix.insert(0,size-1) = 1;
- FactorMatrix.insert(size-1,0) = 1;
-
- std::cout << "Solving factor graph:" << std::endl;
- std::cout << "Factors: " << std::endl << FactorMatrix * MatrixXi::Identity(size,size) << std::endl << std::endl;
-
- // SOLVING WITHOUT REORDERING ------------------------------------
- // solve Hx = b
- t1 = clock();
- solver.compute(H);
- if (solver.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x = solver.solve(b);
- time1 = ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // SOLVING AFTER REORDERING ------------------------------------
- // ordering factors
- ordering_factors.segment(dim * (size-1), dim) = VectorXi::Constant(dim,2);
- ordering_factors.segment(0, dim) = VectorXi::Constant(dim,2);
-
- // variable ordering
- t2 = clock();
- H.makeCompressed();
-
- std::cout << "Reordering using CCOLAMD:" << std::endl;
- std::cout << "ordering_factors = " << std::endl << ordering_factors.transpose() << std::endl << std::endl;
- ordering(H, perm, ordering_factors.data());
-
- b_ordered = perm * b;
- H_ordered = H.twistedBy(perm);
-
- // solve Hx = b
- solver2.compute(H_ordered);
- if (solver2.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_ordered = solver2.solve(b_ordered);
- x_ordered = perm.inverse() * x_ordered;
- time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
-
- // SOLVING AFTER BLOCK REORDERING ------------------------------------
- // ordering factors
- block_ordering_factors(size-1) = 2;
- block_ordering_factors(0) = 2;
-
- // block ordering
- t3 = clock();
- FactorMatrix.makeCompressed();
-
- std::cout << "Reordering using Block CCOLAMD:" << std::endl;
- std::cout << "block_ordering_factors = " << std::endl << block_ordering_factors.transpose() << std::endl << std::endl;
- ordering(FactorMatrix, perm_blocks, block_ordering_factors.data());
-
- // variable ordering
- permutation_2_block_permutation(perm_blocks, perm , dim, size);
- b_block_ordered = perm * b;
- H_block_ordered = H.twistedBy(perm);
-
- // solve Hx = b
- solver3.compute(H_block_ordered);
- if (solver3.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_block_ordered = solver3.solve(b_block_ordered);
- x_block_ordered = perm.inverse() * x_block_ordered;
- time3 = ((double) clock() - t3) / CLOCKS_PER_SEC;
-
- // RESULTS ------------------------------------
- std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl;
- std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
- //std::cout << "x = " << x.transpose() << std::endl;
- //std::cout << "x = " << x_ordered.transpose() << std::endl;
- //std::cout << "x = " << x_block_ordered.transpose() << std::endl;
-}
-
diff --git a/src/examples/solver/test_iQR.cpp b/src/examples/solver/test_iQR.cpp
deleted file mode 100644
index b027c874d6a2e6c4910d5dc45114055d87aa2d54..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_iQR.cpp
+++ /dev/null
@@ -1,350 +0,0 @@
-/*
- * test_iQR.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
-
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
-#include <eigen3/Eigen/SPQRSupport>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-class block_pruning
-{
- public:
- int col, row, Nrows, Ncols;
- block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
- col(_col),
- row(_row),
- Nrows(_Nrows),
- Ncols(_Ncols)
- {
- //
- }
- bool operator()(int i, int j, double) const
- {
- return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
- }
-};
-
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
-{
- // prune all non-zero elements that not satisfy the 'keep' operand
- // elements that are not in the block rows or are not in the block columns should be kept
- //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
-
- block_pruning bp(row, col, Nrows, Ncols);
- original.prune(bp);
-
-// for (unsigned int i = row; i < row + Nrows; i++)
-// for (unsigned int j = col; j < row + Ncols; j++)
-// original.coeffRef(i,j) = 0.0;
-//
-// original.prune(0);
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
-{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- if (ins(r,c) != 0)
- original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables, const int dim)
-{
- ArrayXXi idx(dim, perm_nodes.indices().rows());
- idx.row(0) = dim * perm_nodes.indices().transpose();
-
- for (int i = 1; i<dim; i++)
- idx.row(i) = idx.row(i-1) + 1;
- Map<ArrayXi> idx_blocks(idx.data(), dim*perm_nodes.indices().rows(), 1);
- perm_variables.indices() = idx_blocks;
-}
-
-void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
-{
- int old_size = perm.indices().size();
- int dim = new_size - old_size;
- VectorXi new_indices(new_size);
- new_indices.head(old_size)= perm.indices();
- new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
- perm.resize(new_size);
- perm.indices() = new_indices;
-}
-
-//main
-int main(int argc, char *argv[])
-{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
- {
- std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl;
- std::cout << " - SIZE: integer size of the problem" << std::endl;
- std::cout << " - DIM: integer dimension of the nodes" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
-
- // Problem variables
- SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_ordered, solver_unordered, solver_ordered_partial;
-
- MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
- SparseMatrix<double> A(0,0),
- A_ordered(0,0),
- R(0,0);
- VectorXd b,
- x,
- x_ordered,
- x_ordered_partial;
- int n_measurements = 0;
- int n_nodes = 0;
-
- // ordering variables
- SparseMatrix<int> A_nodes_ordered(0,0);
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix(0);
-
- CCOLAMDOrdering<int> ordering, partial_ordering;
- VectorXi nodes_ordering_factors;
-
- // results variables
- clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
- double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0;
-
- std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
-
- // GENERATING MEASUREMENTS
- std::vector<std::vector<int>> measurements;
- for (int i = 0; i < size; i++)
- {
- std::vector<int> meas(0);
- if (i == 0) //prior
- {
- meas.push_back(0);
- measurements.push_back(meas);
- meas.clear();
- }
- else //odometry
- {
- meas.push_back(i-1);
- meas.push_back(i);
- measurements.push_back(meas);
- meas.clear();
- }
- if (i > size / 2) // loop closures
- {
- meas.push_back(0);
- meas.push_back(i);
- measurements.push_back(meas);
- meas.clear();
- }
- }
-
- // INCREMENTAL LOOP
- for (unsigned int i = 0; i < measurements.size(); i++)
- {
- std::cout << "========================= MEASUREMENT " << i << ":" << std::endl;
- std::vector<int> measurement = measurements.at(i);
-
- // AUGMENT THE PROBLEM ----------------------------
- n_measurements++;
- while (n_nodes < measurement.back()+1)
- {
- n_nodes++;
- // Resize accumulated permutations
- augment_permutation(acc_permutation_nodes_matrix, n_nodes);
-
- // Resize state
- x.conservativeResize(n_nodes*dim);
- x_ordered.conservativeResize(n_nodes*dim);
- x_ordered_partial.conservativeResize(n_nodes*dim);
- }
- A.conservativeResize(n_measurements*dim,n_nodes*dim);
- A_ordered.conservativeResize(n_measurements*dim,n_nodes*dim);
- R.conservativeResize(n_nodes*dim,n_nodes*dim);
- b.conservativeResize(n_measurements*dim);
- A_nodes_ordered.conservativeResize(n_measurements,n_nodes);
-
- // ADD MEASUREMENTS
- int min_ordered_node = n_nodes;
- for (unsigned int j = 0; j < measurement.size(); j++)
- {
- int ordered_node = acc_permutation_nodes_matrix.indices()(measurement.at(j));
-
- addSparseBlock(2*omega, A, A.rows()-dim, measurement.at(j) * dim);
- addSparseBlock(2*omega, A_ordered, A_ordered.rows()-dim, ordered_node * dim);
-
- A_nodes_ordered.coeffRef(A_nodes_ordered.rows()-1, ordered_node) = 1;
-
- b.segment(b.size() - dim, dim) = VectorXd::LinSpaced(dim, b.size()-dim, b.size()-1);
- // store minimum ordered node
- if (min_ordered_node > ordered_node)
- min_ordered_node = ordered_node;
- }
-
-// std::cout << "Solving Ax = b" << std::endl;
-// std::cout << "A_nodes_ordered: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl;
-// std::cout << "A: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A << std::endl << std::endl;
-// std::cout << "A_ordered: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered << std::endl << std::endl;
-// std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl;
-
- // BLOCK REORDERING ------------------------------------
- t_ordering = clock();
- int ordered_nodes = n_nodes - min_ordered_node;
- int unordered_nodes = n_nodes - ordered_nodes;
- if (n_nodes > 1 && ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
- {
- // SUBPROBLEM ORDERING (from first node variable to last one)
- std::cout << "ordering partial problem: " << min_ordered_node << " to "<< n_nodes - 1 << std::endl;
- SparseMatrix<int> sub_A_nodes_ordered = A_nodes_ordered.rightCols(ordered_nodes);
-
- // partial ordering factors
- VectorXi nodes_partial_ordering_factors = sub_A_nodes_ordered.bottomRows(1).transpose();
-
- // computing nodes partial ordering
- A_nodes_ordered.makeCompressed();
- PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes_matrix(ordered_nodes);
- partial_ordering(sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_factors.data());
-
- // node ordering to variable ordering
- PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_matrix(A_ordered.cols());
- permutation_2_block_permutation(partial_permutation_nodes_matrix, partial_permutation_matrix , dim);
-
- // apply partial orderings
- A_nodes_ordered.rightCols(ordered_nodes) = (A_nodes_ordered.rightCols(ordered_nodes) * partial_permutation_nodes_matrix.transpose()).sparseView();
- A_ordered.rightCols(ordered_nodes * dim) = (A_ordered.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
- R.rightCols(ordered_nodes * dim) = (R.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
-
- // ACCUMULATING PERMUTATIONS
- PermutationMatrix<Dynamic, Dynamic, int> permutation_nodes_matrix(VectorXi::LinSpaced(n_nodes, 0, n_nodes - 1)); // identity permutation
- permutation_nodes_matrix.indices().tail(ordered_nodes) = partial_permutation_nodes_matrix.indices() + VectorXi::Constant(ordered_nodes, n_nodes - ordered_nodes);
- acc_permutation_nodes_matrix = permutation_nodes_matrix * acc_permutation_nodes_matrix;
- }
- time_ordering += ((double) clock() - t_ordering) / CLOCKS_PER_SEC;
- // std::cout << "incrementally ordered A Block structure: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl;
- //std::cout << "ordered A: " << std::endl << MatrixXd::Identity(A_ordered.rows(), A_ordered.rows()) * A_ordered << std::endl << std::endl;
- //std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl;
-
- // SOLVING
- // solving ordered subproblem
- t_solving_ordered_partial = clock();
- A_nodes_ordered.makeCompressed();
- A_ordered.makeCompressed();
-
- // finding measurements block
- SparseMatrix<int> measurements_to_initial = A_nodes_ordered.col(min_ordered_node);
-// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
-// std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
- int initial_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
-
- SparseMatrix<double> A_ordered_partial = A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim);
- solver_ordered_partial.compute(A_ordered_partial);
- if (solver_ordered_partial.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- std::cout << "R new" << std::endl << MatrixXd::Identity(ordered_nodes * dim, ordered_nodes * dim) * solver_ordered_partial.matrixR() << std::endl;
- x_ordered_partial.tail(ordered_nodes * dim) = solver_ordered_partial.solve(b.tail(ordered_nodes * dim));
- std::cout << "x_ordered_partial.tail(ordered_nodes * dim)" << std::endl << x_ordered_partial.tail(ordered_nodes * dim).transpose() << std::endl;
- // store new part of R (equivalent to R.bottomRightCorner(ordered_nodes * dim, ordered_nodes * dim) = solver3.matrixR();)
- eraseSparseBlock(R, unordered_nodes * dim, unordered_nodes * dim, ordered_nodes * dim, ordered_nodes * dim);
- addSparseBlock(solver_ordered_partial.matrixR(), R, unordered_nodes * dim, unordered_nodes * dim);
- std::cout << "R" << std::endl << MatrixXd::Identity(R.rows(), R.rows()) * R << std::endl;
- R.makeCompressed();
-
- // solving not ordered subproblem
- if (unordered_nodes > 0)
- {
- std::cout << "--------------------- solving unordered part" << std::endl;
- SparseMatrix<double> R1 = R.topLeftCorner(unordered_nodes * dim, unordered_nodes * dim);
- std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
- SparseMatrix<double> R2 = R.topRightCorner(unordered_nodes * dim, ordered_nodes * dim);
- std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
- solver_ordered_partial.compute(R1);
- if (solver_ordered_partial.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_ordered_partial.head(unordered_nodes * dim) = solver_ordered_partial.solve(b.head(unordered_nodes * dim) - R2 * x_ordered_partial.tail(ordered_nodes * dim));
- }
- // undo ordering
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(A_ordered.cols());
- permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix , dim);
- x_ordered_partial = acc_permutation_matrix.inverse() * x_ordered_partial;
- time_solving_ordered_partial += ((double) clock() - t_solving_ordered_partial) / CLOCKS_PER_SEC;
-
- // SOLVING
- // full ordered problem
- t_solving_ordered_full = clock();
- A_nodes_ordered.makeCompressed();
- A_ordered.makeCompressed();
- solver_ordered.compute(A_ordered);
- if (solver_ordered.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_ordered = solver_ordered.solve(b);
- std::cout << "solver_ordered.matrixR()" << std::endl << MatrixXd::Identity(A_ordered.cols(), A_ordered.cols()) * solver_ordered.matrixR() << std::endl;
- // undo ordering
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix2(A_ordered.cols());
- permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix2 , dim);
- x_ordered = acc_permutation_matrix.inverse() * x_ordered;
- time_solving_ordered += ((double) clock() - t_solving_ordered_full) / CLOCKS_PER_SEC;
-
- // WITHOUT ORDERING
- t_solving_unordered = clock();
- A.makeCompressed();
- solver_unordered.compute(A);
- if (solver_unordered.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- //std::cout << "no ordering? " << solver_unordered.colsPermutation().indices().transpose() << std::endl;
- x = solver_unordered.solve(b);
- std::cout << "solver_unordered.matrixR()" << std::endl << MatrixXd::Identity(A.cols(), A.cols()) * solver_unordered.matrixR() << std::endl;
- time_solving_unordered += ((double) clock() - t_solving_unordered) / CLOCKS_PER_SEC;
-
- // RESULTS ------------------------------------
- std::cout << "========================= RESULTS " << i << ":" << std::endl;
- std::cout << "NO REORDERING: solved in " << time_solving_unordered*1e3 << " ms | " << solver_unordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time_solving_ordered*1e3 << " ms | " << solver_ordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl;
- std::cout << "BLOCK REORDERING 2: solved in " << time_solving_ordered_partial*1e3 << " ms | " << R.nonZeros() << " nonzeros in R"<< std::endl;
-
- std::cout << "x = " << x.transpose() << std::endl;
- std::cout << "x_ordered = " << x_ordered.transpose() << std::endl;
- std::cout << "x_ordered_partial = " << x_ordered_partial.transpose() << std::endl;
- if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
- std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
- else
- std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
- }
-}
-
diff --git a/src/examples/solver/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp
deleted file mode 100644
index 2fdc1f9f22ca75801b6dc7155dea9bb515d9ccd0..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_iQR_wolf.cpp
+++ /dev/null
@@ -1,560 +0,0 @@
-/*
- * test_iQR_wolf.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <string>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
-#include <Eigen/SPQRSupport>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-class block_pruning
-{
- public:
- int col, row, Nrows, Ncols;
- block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
- col(_col),
- row(_row),
- Nrows(_Nrows),
- Ncols(_Ncols)
- {
- //
- }
- bool operator()(int i, int j, double) const
- {
- return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
- }
-};
-
-void eraseSparseBlock(SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
-{
- // prune all non-zero elements that not satisfy the 'keep' operand
- // elements that are not in the block rows or are not in the block columns should be kept
- //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
-
- block_pruning bp(row, col, Nrows, Ncols);
- original.prune(bp);
-
-// for (unsigned int i = row; i < row + Nrows; i++)
-// for (unsigned int j = col; j < row + Ncols; j++)
-// original.coeffRef(i,j) = 0.0;
-//
-// original.prune(0);
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col)
-{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- if (ins(r,c) != 0)
- original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-struct node
-{
- public:
- int id;
- int dim;
- int location;
- int order;
-
- node(const int _id, const int _dim, const int _location, const int _order) :
- id(_id),
- dim(_dim),
- location(_location),
- order(_order)
- {
-
- }
-};
-
-struct measurement
-{
- public:
- std::vector<MatrixXd> jacobians;
- std::vector<int> nodes_idx;
- VectorXd error;
- int dim;
- bool odometry_type;
- int location;
-
- measurement(const MatrixXd & _jacobian1, const int _idx1, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
- jacobians({_jacobian1}),
- nodes_idx({_idx1}),
- error(_error),
- dim(_meas_dim),
- odometry_type(_odometry_type),
- location(0)
- {
- //jacobians.push_back(_jacobian1);
- }
-
- measurement(const MatrixXd & _jacobian1, const int _idx1, const MatrixXd & _jacobian2, const int _idx2, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
- jacobians({_jacobian1, _jacobian2}),
- nodes_idx({_idx1, _idx2}),
- error(_error),
- dim(_meas_dim),
- odometry_type(_odometry_type),
- location(0)
- {
-
- }
-};
-
-class SolverQR
-{
- protected:
- std::string name_;
- SparseQR < SparseMatrix<double, ColMajor>, NaturalOrdering<int>> solver_;
- SparseMatrix<double, ColMajor> A_, R_;
- VectorXd b_, x_incr_;
- int n_measurements;
- int n_nodes_;
- std::vector<node> nodes_;
- std::vector<measurement> measurements_;
-
- // ordering
- SparseMatrix<int, ColMajor> A_nodes_;
- PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_;
-
- CCOLAMDOrdering<int> orderer_;
- VectorXi node_ordering_restrictions_;
- int first_ordered_node_;
-
- // time
- clock_t t_ordering_, t_solving_, t_managing_;
- double time_ordering_, time_solving_, time_managing_;
-
- public:
- SolverQR(const std::string &_name) :
- name_(_name),
- A_(0,0),
- R_(0,0),
-// b_(0),
-// x_(0),
- n_measurements(0),
- n_nodes_(0),
- A_nodes_(0,0),
- acc_node_permutation_(0),
-// nodes_(0),
-// measurements_(0),
- first_ordered_node_(0),
- t_ordering_(0),
- t_solving_(0),
- t_managing_(0),
- time_ordering_(0),
- time_solving_(0),
- time_managing_(0)
- {
- //
- }
-
- virtual ~SolverQR()
- {
-
- }
-
- void add_state_unit(const int node_dim, const int node_idx)
- {
- t_managing_ = clock();
-
- n_nodes_++;
- nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
-
- // Resize accumulated permutations
- augment_permutation(acc_node_permutation_, n_nodes_);
-
- // Resize state
- x_incr_.conservativeResize(x_incr_.size() + node_dim);
-
- // Resize problem
- A_.conservativeResize(A_.rows(), A_.cols() + node_dim);
- R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim);
- //A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary
-
- time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
- }
-
- void addFactor(const measurement& _meas)
- {
- t_managing_ = clock();
-
- assert(_meas.jacobians.size() == _meas.nodes_idx.size());
- assert(_meas.error.size() == _meas.dim);
-
- n_measurements++;
- measurements_.push_back(_meas);
- measurements_.back().location = A_.rows();
-
- // Resize problem
- A_.conservativeResize(A_.rows() + _meas.dim, A_.cols());
- b_.conservativeResize(b_.size() + _meas.dim);
- A_nodes_.conservativeResize(n_measurements,n_nodes_);
-
- // ADD MEASUREMENTS
- first_ordered_node_ = n_nodes_;
- for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++)
- {
- assert(acc_node_permutation_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order);
-
- int ordered_node = nodes_.at(_meas.nodes_idx.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j));
-
- addSparseBlock(_meas.jacobians.at(j), A_, A_.rows()-_meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location);
-
- A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
-
- assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim);
- assert(_meas.jacobians.at(j).rows() == _meas.dim);
-
- // store minimum ordered node
- if (first_ordered_node_ > ordered_node)
- first_ordered_node_ = ordered_node;
- }
-
- // error
- b_.tail(_meas.dim) = _meas.error;
-
- time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
- }
-
- void ordering(const int & _first_ordered_node)
- {
- t_ordering_ = clock();
-
- // full problem ordering
- if (_first_ordered_node == 0)
- {
- // ordering ordering factors
- node_ordering_restrictions_.resize(n_nodes_);
- node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
-
- // computing nodes partial ordering_
- A_nodes_.makeCompressed();
- PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
- orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
-
- // node ordering to variable ordering
- PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
- nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
-
- // apply partial_ordering orderings
- A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
- A_ = (A_ * incr_permutation.transpose()).sparseView();
-
- // ACCUMULATING PERMUTATIONS
- accumulatePermutation(incr_permutation_nodes);
- }
-
- // partial ordering
- else
- {
- int ordered_nodes = n_nodes_ - _first_ordered_node;
- int unordered_nodes = n_nodes_ - ordered_nodes;
- if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
- {
- // SUBPROBLEM ORDERING (from first node variable to last one)
- //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
- SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
-
- // _partial_ordering ordering_ factors
- node_ordering_restrictions_.resize(ordered_nodes);
- node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
-
- // computing nodes partial ordering_
- sub_A_nodes_.makeCompressed();
- PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
- orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
-
- // node ordering to variable ordering
- PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols());
- nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
-
- // apply partial_ordering orderings
- int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location;
- A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
- A_.rightCols(ordered_variables) = (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
- R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
-
- // ACCUMULATING PERMUTATIONS
- accumulatePermutation(partial_permutation_nodes);
- }
- }
- time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC;
- }
-
- bool solve(const int mode)
- {
- bool batch = (mode !=2 || first_ordered_node_ == 0);
- bool order = (mode !=0 && n_nodes_ > 1);
-
- // BATCH
- if (batch)
- {
- // REORDER
- if (order)
- ordering(0);
-
- //print_problem();
-
- // SOLVE
- t_solving_ = clock();
- A_.makeCompressed();
- solver_.compute(A_);
- if (solver_.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_ = solver_.solve(b_);
- R_ = solver_.matrixR();
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
- time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
- }
- // INCREMENTAL
- else
- {
- // REORDER SUBPROBLEM
- ordering(first_ordered_node_);
- //print_problem();
-
- // SOLVE ORDERED SUBPROBLEM
- t_solving_= clock();
- A_nodes_.makeCompressed();
- A_.makeCompressed();
-
- // finding measurements block
- SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
- // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
- int first_ordered_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
- int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
- int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
- int unordered_variables = nodes_.at(first_ordered_node_).location;
-
- SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
- solver_.compute(A_partial);
- if (solver_.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
- x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
-
- // store new part of R
- eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- R_.makeCompressed();
-
- // solving not ordered subproblem
- if (unordered_variables > 0)
- {
- //std::cout << "--------------------- solving unordered part" << std::endl;
- SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
- //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
- SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
- //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
- solver_.compute(R1);
- if (solver_.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
- }
-
- }
- // UNDO ORDERING FOR RESULT
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
- nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
- x_incr_ = acc_permutation.inverse() * x_incr_;
-
- time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
-
- return 1;
- }
-
- void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
- {
- ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
-
- int last_idx = 0;
- for (unsigned int i = 0; i<locations.size(); i++)
- {
- perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i)+nodes_.at(i).dim-1);
- last_idx += nodes_.at(i).dim;
- }
- }
-
- ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes)
- {
- ArrayXi indices = _perm_nodes.indices().array();
-
- for (unsigned int i = 0; i<indices.size(); i++)
- indices = (indices > indices(i)).select(indices + nodes_.at(i).dim-1, indices);
-
- return indices;
- }
-
- void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
- {
- int old_size = perm.indices().size();
- int dim = new_size - old_size;
- VectorXi new_indices(new_size);
- new_indices.head(old_size)= perm.indices();
- new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
- perm.resize(new_size);
- perm.indices() = new_indices;
- }
-
- void accumulatePermutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
- {
- printName();
- //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
- //std::cout << "incr perm " << perm.indices().transpose() << std::endl;
-
- // acumulate permutation
- if (perm.size() == acc_node_permutation_.size()) //full permutation
- acc_node_permutation_ = perm * acc_node_permutation_;
- else //partial permutation
- {
- PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation
- incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size());
- //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
- acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
- }
- //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
-
- // update nodes orders and locations
- ArrayXi locations = perm_nodes_2_locations(acc_node_permutation_);
- for (unsigned int i = 0; i < nodes_.size(); i++)
- {
- nodes_.at(i).order = acc_node_permutation_.indices()(i);
- nodes_.at(i).location = locations(i);
- }
- }
-
- void printName()
- {
- std::cout << name_;
- }
-
- void printResults()
- {
- printName();
- std::cout << " solved in " << time_solving_*1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"<< std::endl;
- std::cout << "x = " << x_incr_.transpose() << std::endl;
- }
-
- void printProblem()
- {
- printName();
- std::cout << std::endl << "A_nodes_: " << std::endl << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
- std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
- std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
- }
-};
-
-//main
-int main(int argc, char *argv[])
-{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
- {
- std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl;
- std::cout << " - SIZE: integer size of the problem" << std::endl;
- std::cout << " - DIM: integer dimension of the nodes" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
-
- // Problems
- SolverQR solver_ordered("FULL ORDERED");
- SolverQR solver_unordered("UNORDERED");
- SolverQR solver_ordered_partial("PARTIALLY ORDERED");
-
- MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
-
- // results variables
- clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
- double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0;
-
- std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
-
- // GENERATING MEASUREMENTS
- std::vector<measurement> measurements;
- for (int i = 0; i < size; i++)
- {
- std::vector<int> meas(0);
- if (i == 0) //prior
- measurements.push_back(measurement(omega, 0, VectorXd::LinSpaced(dim, 0, dim-1), dim));
-
- else //odometry
- measurements.push_back(measurement(2*omega, i-1, 2*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim, true));
-
- if (i > size / 2) //loop closures
- measurements.push_back(measurement(4*omega, 0, 4*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim));
- }
-
- // INCREMENTAL LOOP
- for (unsigned int i = 0; i < measurements.size(); i++)
- {
- std::cout << "========================= MEASUREMENT " << i << ":" << std::endl;
-
- // AUGMENT THE PROBLEM ----------------------------
- if (measurements.at(i).odometry_type || i == 0) // if odometry, augment the problem
- {
- solver_unordered.add_state_unit(dim, i);
- solver_ordered.add_state_unit(dim, i);
- solver_ordered_partial.add_state_unit(dim,i);
- }
-
- // ADD MEASUREMENTS
- solver_unordered.addFactor(measurements.at(i));
- solver_ordered.addFactor(measurements.at(i));
- solver_ordered_partial.addFactor(measurements.at(i));
-
- // PRINT PROBLEM
- solver_unordered.printProblem();
- solver_ordered.printProblem();
- solver_ordered_partial.printProblem();
-
- // SOLVING
- solver_unordered.solve(0);
- solver_ordered.solve(1);
- solver_ordered_partial.solve(2);
-
- // RESULTS ------------------------------------
- std::cout << "========================= RESULTS " << i << ":" << std::endl;
- solver_unordered.printResults();
- solver_ordered.printResults();
- solver_ordered_partial.printResults();
-
-// if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
-// std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
-// else
-// std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
- }
-}
-
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
deleted file mode 100644
index 9ad57098e8e3671bcf18cd54dc458fc6d776dab4..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ /dev/null
@@ -1,430 +0,0 @@
-/*
- * test_iQR_wolf.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <string>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-//Wolf includes
-#include "core/state_block/state_block.h"
-#include "core/factor/factor_base.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "wolf_manager.h"
-
-// wolf solver
-#include "solver/qr_solver.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"
-
-//Ceres includes
-#include "glog/logging.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//laser_scan_utils
-#include "iri-algorithms/laser_scan_utils/corner_detector.h"
-#include "iri-algorithms/laser_scan_utils/entities.h"
-
-//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 > 120) && (ii <= 170))
- {
- displacement_ = 0.2;
- rotation_ = 1.8 * M_PI / 180;
- }
- else if ((ii > 170) && (ii <= 220))
- {
- displacement_ = 0;
- rotation_ = -1.8 * M_PI / 180;
- }
- else if ((ii > 220) && (ii <= 310))
- {
- displacement_ = 0.1;
- rotation_ = 0;
- }
- else if ((ii > 310) && (ii <= 487))
- {
- displacement_ = 0.1;
- rotation_ = -1. * M_PI / 180;
- }
- else if ((ii > 487) && (ii <= 600))
- {
- displacement_ = 0.2;
- rotation_ = 0;
- }
- else if ((ii > 600) && (ii <= 700))
- {
- displacement_ = 0.1;
- rotation_ = -1. * M_PI / 180;
- }
- else if ((ii > 700) && (ii <= 780))
- {
- displacement_ = 0;
- rotation_ = -1. * M_PI / 180;
- }
- else
- {
- displacement_ = 0.3;
- rotation_ = 0.0 * M_PI / 180;
- }
-
- pose.moveForward(displacement_);
- pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
-}
-
-//main
-int main(int argc, char *argv[])
-{
- using namespace Eigen;
- using namespace wolf;
-
- // USER INPUT ============================================================================================
- if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[1]) > 1100 || atoi(argv[2]) < 0 || atoi(argv[2]) > 2)
- {
- std::cout << "Please call me with: [./test_ceres_manager NI MODE], where:" << std::endl;
- std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl;
- std::cout << " - MODE is the solver mode (0 batch (no ordering), 1 batch (ordering), 2 incremental" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- bool complex_angle = false;
- unsigned int solving_mode = (unsigned int) atoi(argv[2]); //solving mode
- 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.1;
- Scalar gps_std = 10;
- std::default_random_engine generator(1);
- std::normal_distribution<Scalar> gaussian_distribution(0.0, 1);
-
- // 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)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- SensorLaser2D laser_2_sensor(std::make_shared<StateBlock>(laser_2_pose.head(2)), std::make_shared<StateBlock>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-
- // Initial pose
- pose_odom << 2, 8, 0;
- 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);
-
- // Ceres initialization
- 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 = new CeresManager(wolf_manager_ceres->getProblem(), ceres_options);
- std::ofstream log_file, landmark_file; //output file
-
- // Own solver
- SolverQR solver_(wolf_manager_QR->getProblem());
-
- std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
- std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
- // 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) += gaussian_distribution(generator) * odom_std_factor * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
- odom_reading(1) += gaussian_distribution(generator) * odom_std_factor * 1e-6;
- odom_reading(2) += gaussian_distribution(generator) * odom_std_factor * (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) += gaussian_distribution(generator) * gps_std;
- gps_fix_reading(1) += gaussian_distribution(generator) * gps_std;
-
- //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;
- // 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->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
- //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
- // updating problem
- wolf_manager_QR->update();
- wolf_manager_ceres->update();
-
- // UPDATING SOLVER ---------------------------
- //std::cout << "UPDATING..." << std::endl;
- // update state units and factors in ceres
- solver_.update();
-
- // PRINT PROBLEM
- //solver_.printProblem();
-
- // SOLVE OPTIMIZATION ---------------------------
- //std::cout << "SOLVING..." << std::endl;
- ceres::Solver::Summary summary = ceres_manager->solve();
- solver_.solve(solving_mode);
-
- std::cout << "========================= RESULTS " << step << ":" << std::endl;
- //solver_.printResults();
- std::cout << "QR vehicle pose " << wolf_manager_QR->getVehiclePose().transpose() << std::endl;
- std::cout << "ceres vehicle pose " << wolf_manager_ceres->getVehiclePose().transpose() << std::endl;
-
- // COMPUTE COVARIANCES ---------------------------
- //std::cout << "COMPUTING COVARIANCES..." << std::endl;
- // TODO
-
- // DRAWING STUFF ---------------------------
- // 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_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->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_QR->getVehiclePose()(0), wolf_manager_QR->getVehiclePose()(1), 0.2, wolf_manager_QR->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);
- // instead of laser 2 we draw ceres solution
- //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());
- estimated_laser_2_pose.setPose(wolf_manager_ceres->getVehiclePose()(0), wolf_manager_ceres->getVehiclePose()(1), 0.2, wolf_manager_ceres->getVehiclePose()(2), 0, 0);
-
- 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
- myRender->setViewPoint(viewPoint);
- myRender->drawPoseAxis(devicePose);
- myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
- myRender->render();
-
- // 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;
-// wolf_manager->getProblem()->print();
- }
-
- // 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;
-// wolf_manager->getProblem()->print();
-
- // Draw Final result -------------------------
- std::cout << "Drawing final results..." << std::endl;
- std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
- {
- 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 -------------------------
- std::cout << "Printing results in a file..." << std::endl;
- // 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++)
- {
- 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));
- else
- state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
- i += 3;
- }
-
- // Landmarks
- std::cout << "Landmarks..." << std::endl;
- i = 0;
- Eigen::VectorXs 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());
- landmarks.segment(i, 2) = landmark;
- i += 2;
- }
-
- // Print log files
- std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : 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") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : 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_QR;
- delete wolf_manager_ceres;
- std::cout << "wolf deleted" << std::endl;
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
-
diff --git a/src/examples/solver/test_incremental_ccolamd_blocks.cpp b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
deleted file mode 100644
index 9283f8411954e0aea8783d067a419e85a09db932..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_incremental_ccolamd_blocks.cpp
+++ /dev/null
@@ -1,262 +0,0 @@
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
-{
- for (unsigned int i = row; i < row + Nrows; i++)
- for (unsigned int j = col; j < row + Ncols; j++)
- original.coeffRef(i,j) = 0.0;
-
- original.makeCompressed();
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
-{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- if (ins(r,c) != 0)
- original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size)
-{
- ArrayXXi idx(dim, size);
- idx.row(0) = dim * perm.indices().transpose();
-
- for (int i = 1; i<dim; i++)
- idx.row(i) = idx.row(i-1) + 1;
- Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1);
- perm_blocks.indices() = idx_blocks;
-}
-
-//main
-int main(int argc, char *argv[])
-{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
- {
- std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl;
- std::cout << " - SIZE: integer size of the problem" << std::endl;
- std::cout << " - DIM: integer dimension of the nodes" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
-
- // Problem variables
- //CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
- SparseLU < SparseMatrix<double>, NaturalOrdering<int> > solver, solver2, solver3;
- MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
- SparseMatrix<double> H(dim,dim),
- H_ordered(dim,dim),
- H_b_ordered(dim,dim);
- VectorXd b(dim),
- b_ordered(dim),
- b_b_ordered(dim),
- x_b_ordered(dim),
- x_ordered(dim),
- x(dim);
-
- // ordering variables
- SparseMatrix<int> factors(1,1), factors_ordered(1,1);
- ArrayXi acc_permutation(dim),
- acc_permutation_b(dim),
- acc_permutation_factors(1);
- acc_permutation = ArrayXi::LinSpaced(dim,0,dim-1);
- acc_permutation_b = acc_permutation;
- acc_permutation_factors(0) = 0;
-
- CCOLAMDOrdering<int> ordering;
- VectorXi factor_ordering_factors(1);
- VectorXi ordering_factors(1);
-
- // results variables
- clock_t t1, t2, t3;
- double time1=0, time2=0, time3=0;
-
- // INITIAL STATE
- addSparseBlock(5*omega, H, 0, 0);
- factors.insert(0,0) = 1;
- b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim-1);
-
- std::cout << "STARTING INCREMENTAL TEST" << std::endl << std::endl;
-
- // INCREMENTAL LOOP
- for (int i = 1; i < size; i++)
- {
- std::cout << "========================= STEP " << i << ":" << std::endl;
- // AUGMENT THE PROBLEM ----------------------------
- H.conservativeResize((i+1)*dim,(i+1)*dim);
- H_ordered.conservativeResize((i+1)*dim,(i+1)*dim);
- H_b_ordered.conservativeResize((i+1)*dim,(i+1)*dim);
- b.conservativeResize((i+1)*dim);
- b_ordered.conservativeResize((i+1)*dim);
- b_b_ordered.conservativeResize((i+1)*dim);
- x.conservativeResize((i+1)*dim);
- x_ordered.conservativeResize((i+1)*dim);
- x_b_ordered.conservativeResize((i+1)*dim);
- factors.conservativeResize(i+1, i+1);
-
- // Odometry
- addSparseBlock(5*omega, H, i*dim, i*dim);
- addSparseBlock(omega, H, i*dim, (i-1)*dim);
- addSparseBlock(omega, H, (i-1)*dim, i*dim);
- factors.insert(i,i) = 1;
- factors.insert(i,i-1) = 1;
- factors.insert(i-1,i) = 1;
-
- // Loop Closure
- if (i == size-1)
- {
- addSparseBlock(2*omega, H, 0, i*dim);
- addSparseBlock(2*omega, H, i*dim, 0);
- factors.insert(0,i) = 1;
- factors.insert(i,0) = 1;
- }
-
- // r.h.v
- b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1);
-
- std::cout << "Solving factor graph:" << std::endl;
- std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl;
-// std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl;
-
- // SOLVING WITHOUT REORDERING ------------------------------------
- // solve Hx = b
- t1 = clock();
- solver.compute(H);
- if (solver.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x = solver.solve(b);
- time1 += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
- // SOLVING WITH REORDERING ------------------------------------
- // Order with previous orderings
- acc_permutation.conservativeResize(dim*(i+1));
- acc_permutation.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1);
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(dim*(i+1));
- acc_permutation_matrix.indices() = acc_permutation;
- b_ordered = acc_permutation_matrix * b;
- H_ordered = H.twistedBy(acc_permutation_matrix);
-
- // ordering factors
- ordering_factors.resize(dim*(i+1));
- ordering_factors = ((H_ordered.rightCols(3) * MatrixXd::Ones(3,1)).array() == 0).select(VectorXi::Zero(dim*(i+1)),VectorXi::Ones(dim*(i+1)));
-
- // variable ordering
- t2 = clock();
- H_ordered.makeCompressed();
-
- PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim*(i+1));
- ordering(H_ordered, permutation_matrix, ordering_factors.data());
-
- // applying ordering
- acc_permutation_matrix = permutation_matrix * acc_permutation_matrix;
- acc_permutation = acc_permutation_matrix.indices();
- b_ordered = permutation_matrix * b_ordered;
- H_ordered = H_ordered.twistedBy(permutation_matrix);
-
- // solve Hx = b
- solver2.compute(H_ordered);
- if (solver2.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_ordered = solver2.solve(b_ordered);
- x_ordered = acc_permutation_matrix.inverse() * x_ordered;
- time2 += ((double) clock() - t2) / CLOCKS_PER_SEC;
-
- // SOLVING WITH BLOCK REORDERING ------------------------------------
- // Order with previous orderings
- acc_permutation_b.conservativeResize(dim*(i+1));
- acc_permutation_b.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1);
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_b_matrix(dim*(i+1));
- acc_permutation_b_matrix.indices() = acc_permutation_b;
- b_b_ordered = acc_permutation_b_matrix * b;
- H_b_ordered = H.twistedBy(acc_permutation_b_matrix);
-
- acc_permutation_factors.conservativeResize(i+1);
- acc_permutation_factors(i) = i;
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_factors_matrix(dim*(i+1));
- acc_permutation_factors_matrix.indices() = acc_permutation_factors;
- factors_ordered = factors.twistedBy(acc_permutation_factors_matrix);
-
- // ordering factors
- factor_ordering_factors.resize(i);
- factor_ordering_factors = factors_ordered.rightCols(1);
-
- // block ordering
- t3 = clock();
- factors_ordered.makeCompressed();
-
- PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i+1);
- ordering(factors_ordered, permutation_factors_matrix, factor_ordering_factors.data());
-
- // applying ordering
- permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix , dim, i+1);
- acc_permutation_factors_matrix = permutation_factors_matrix * acc_permutation_factors_matrix;
- acc_permutation_factors = acc_permutation_factors_matrix.indices();
- acc_permutation_b_matrix = permutation_matrix * acc_permutation_b_matrix;
- acc_permutation_b = acc_permutation_b_matrix.indices();
- b_b_ordered = permutation_matrix * b_b_ordered;
- H_b_ordered = H_b_ordered.twistedBy(permutation_matrix);
-
- // solve Hx = b
- solver3.compute(H_b_ordered);
- if (solver3.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_b_ordered = solver3.solve(b_b_ordered);
- x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered;
- time3 += ((double) clock() - t3) / CLOCKS_PER_SEC;
-
- // RESULTS ------------------------------------
- std::cout << "========================= RESULTS " << i << ":" << std::endl;
- std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl;
- std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
- std::cout << "x1 = " << x.transpose() << std::endl;
- std::cout << "x2 = " << x_ordered.transpose() << std::endl;
- std::cout << "x3 = " << x_b_ordered.transpose() << std::endl;
- }
-
- // RESULTS ------------------------------------
- std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl;
- std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
-
- //std::cout << "x = " << x.transpose() << std::endl;
- //std::cout << "x = " << x_ordered.transpose() << std::endl;
- //std::cout << "x = " << x_b_ordered.transpose() << std::endl;
-}
-
diff --git a/src/examples/solver/test_permutations.cpp b/src/examples/solver/test_permutations.cpp
deleted file mode 100644
index c33c744c673024b5a66aece490d98b2de5e5543b..0000000000000000000000000000000000000000
--- a/src/examples/solver/test_permutations.cpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * test_permutations.cpp
- *
- * Created on: Jun 15, 2015
- * Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-
-using namespace Eigen;
-
-//main
-int main(int argc, char *argv[])
-{
- PermutationMatrix<Dynamic, Dynamic, int> P1(5), P2(5), P3(5), P4(5);
- P1.setIdentity();
- P2.setIdentity();
- P3.setIdentity();
-
- VectorXd a = VectorXd::LinSpaced(5,1,5);
- MatrixXd A= a.asDiagonal();
- SparseMatrix<double> B = A.sparseView();
- B.makeCompressed();
-
- std::cout << "A (dense)" << std::endl << A << std::endl << std::endl;
- std::cout << "B (sparse)" << std::endl << B << std::endl << std::endl;
-
- P1.indices()(3) = 4;
- P1.indices()(4) = 3;
-
- std::cout << "Permutation 1" << std::endl << P1.indices().transpose() << std::endl << std::endl;
-
- P2.indices()(0) = 4;
- P2.indices()(4) = 0;
-
- std::cout << "Permutation 2" << std::endl << P2.indices().transpose() << std::endl << std::endl;
-
- std::cout << "Pre-multiplying: Permutating rows" << std::endl;
- std::cout << "P1 * A" << std::endl << P1 * A << std::endl << std::endl;
- std::cout << "P1 * B" << std::endl << P1 * B << std::endl << std::endl;
- SparseMatrix<double> C = (P1 * B).sparseView();
- std::cout << "(P1 * B).bottomRows(1)" << std::endl << C.bottomRows(1) << std::endl << std::endl;
-
- std::cout << "Post-multiplying: Permutating cols" << std::endl;
- std::cout << "A * P1.transpose()" << std::endl << A * P1.transpose()<< std::endl << std::endl;
- std::cout << "B * P1.transpose()" << std::endl << B * P1.transpose()<< std::endl << std::endl;
-
- std::cout << "Pre&post-multiplying:" << std::endl;
- std::cout << "P1 * A * P1.transpose()" << std::endl << P1 * A * P1.transpose() << std::endl << std::endl;
- std::cout << "P2 * P1 * A * P1.transpose() * P2.transpose()" << std::endl << P2 * P1 * A * P1.transpose() * P2.transpose() << std::endl << std::endl;
- std::cout << "P1 * P2 * A * P2.transpose() * P1.transpose()" << std::endl << P1 * P2 * A * P2.transpose() * P1.transpose() << std::endl << std::endl;
-
- P3 = P1 * P2;
-
- std::cout << "Permutation P3 = P1 * P2" << std::endl << P3.indices().transpose() << std::endl << std::endl;
- std::cout << "P3 * A * P3.transpose()" << std::endl << P3 * A * P3.transpose() << std::endl << std::endl;
-
- std::cout << "PERMUTATING INDICES" << std::endl;
- ArrayXi acc_permutations(5);
- acc_permutations << 0,1,2,3,4;
-
- std::cout << "acc_permutations: " << acc_permutations.transpose() << std::endl;
-
- std::cout << "P1: " << P1.indices().transpose() << std::endl;
- std::cout << "P1 * acc_permutations: " << (P1 * acc_permutations.matrix()).transpose() << std::endl;
- std::cout << "P1.inverse() * acc_permutations: " << (P1.inverse() * acc_permutations.matrix()).transpose() << std::endl;
-
- std::cout << "P2: " << P2.indices().transpose() << std::endl;
- std::cout << "P2 * (P1 * acc_permutations): " << (P2 * (P1 * acc_permutations.matrix())).transpose() << std::endl;
- std::cout << "(P2 * P1).inverse() * acc_permutations): " << ((P2 * P1).inverse() * acc_permutations.matrix()).transpose() << std::endl;
- P4 = P1 * P2 * P3;
- std::cout << "Permutation P4 = P1 * P2 * P3: " << P4.indices().transpose() << std::endl;
- std::cout << "P3 * (P2 * (P1 * acc_permutations)): " << (P3 * (P2 * (P1 * acc_permutations.matrix()))).transpose() << std::endl;
- std::cout << "accumulated permutations can not be stored in vectors..." << std::endl;
-
- std::cout << std::endl << "PARTIALL PERMUTATIONS" << std::endl;
- PermutationMatrix<Dynamic, Dynamic, int> P5(2);
- P5.indices()(0) = 1;
- P5.indices()(1) = 0;
- std::cout << "P5 (equivalent to P1 but partiall): " << std::endl << P5.indices().transpose() << std::endl;
- std::cout << "P2: " << P2.indices().transpose() << std::endl << std::endl;
- std::cout << "A * P2.transpose(): " << std::endl << A * P2.transpose() << std::endl << std::endl;
- std::cout << "(A * P2.transpose()).rightCols(2) * P5.transpose(): " << std::endl << (A * P2.transpose()).rightCols(2) * P5.transpose() << std::endl << std::endl;
- PermutationMatrix<Dynamic, Dynamic, int> P6 = P2;
- P6.indices().tail(2) = P5 * P6.indices().tail(2);
- std::cout << "P6 = P2, P6.indices().tail(2) = P5 * P6.indices().tail(2): " << P6.indices().transpose() << std::endl << std::endl;
- std::cout << "A * P6.transpose(): " << std::endl << A * P6.transpose() << std::endl << std::endl;
- std::cout << "(P1 * P2): " << std::endl << (P1 * P2).indices().transpose() << std::endl << std::endl;
- std::cout << "A * (P1 * P2).transpose(): " << std::endl << A * (P1 * P2).transpose() << std::endl << std::endl;
- std::cout << "Partiall permutations can not be accumulated, for doing that they should be full size" << std::endl;
-
- std::cout << std::endl << "PERMUTATION OF MAPPED VECTORS" << std::endl;
- std::cout << "a = " << a.transpose() << std::endl << std::endl;
- Map<VectorXd> mapped_a(a.data(), 1);
- std::cout << "mapped_a1 = " << mapped_a << std::endl << std::endl;
- a = P2 * a;
- std::cout << "a = P2 * a: " << std::endl << a.transpose() << std::endl << std::endl;
- std::cout << "mapped_a = " << mapped_a.transpose() << std::endl << std::endl;
- std::cout << "maps are affected of the reorderings in mapped vectors" << std::endl;
-
-// Map<>
-}
diff --git a/src/examples/test_analytic_odom_factor.cpp b/src/examples/test_analytic_odom_factor.cpp
deleted file mode 100644
index 32a2cbc64fcac5b0d37052347919f3e308042585..0000000000000000000000000000000000000000
--- a/src/examples/test_analytic_odom_factor.cpp
+++ /dev/null
@@ -1,314 +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"
-
-// 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)
-{
- 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_;
- ceres::Solver::Summary summary_autodiff, summary_analytic;
-
- // loading variables
- std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_autodiff;
- 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);
-
- // 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);
-
- // 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_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
- wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
- // store
- index_2_frame_ptr_autodiff[vertex_index] = vertex_frame_ptr_autodiff;
- index_2_frame_ptr_analytic[vertex_index] = vertex_frame_ptr_analytic;
-
- //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_analytic->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_autodiff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
- CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor);
- assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!");
- FrameBasePtr frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old];
- assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() && "edge to vertex not added!");
- 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);
- 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;
-
- // add capture, feature and factor to problem
- FeatureBasePtr feature_ptr_analytic = new FeatureBase("POSE", edge_vector, edge_information.inverse());
- CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor);
- assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!");
- FrameBasePtr frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old];
- assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() && "edge to vertex not added!");
- 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);
- 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;
- //std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << factor_ptr_analytic->getMeasurementCovariance() << std::endl;
- }
- }
- else
- assert("unknown line");
- }
- printf("\nGraph loaded!\n");
- }
- else
- 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);
- first_frame_autodiff->addCapture(initial_covariance_autodiff);
- first_frame_analytic->addCapture(initial_covariance_analytic);
- initial_covariance_autodiff->emplaceFeatureAndFactor();
- initial_covariance_analytic->emplaceFeatureAndFactor();
-
- // SOLVING PROBLEMS
- std::cout << "solving..." << std::endl;
- std::cout << "ANALYTIC -----------------------------------" << std::endl;
- summary_analytic = ceres_manager_analytic->solve();
- std::cout << summary_analytic.FullReport() << std::endl;
- std::cout << "AUTODIFF -----------------------------------" << std::endl;
- summary_autodiff = ceres_manager_autodiff->solve();
- std::cout << summary_autodiff.FullReport() << std::endl;
-
- // COMPUTE COVARIANCES
- std::cout << "computing covariances..." << std::endl;
- std::cout << "ANALYTIC -----------------------------------" << std::endl;
- clock_t t1 = clock();
- ceres_manager_analytic->computeCovariances(ALL);//ALL_MARGINALS
- std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
- std::cout << "AUTODIFF -----------------------------------" << std::endl;
- t1 = clock();
- ceres_manager_autodiff->computeCovariances(ALL);//ALL_MARGINALS
- std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
-
- delete wolf_problem_autodiff; //not necessary to delete anything more, wolf will do it!
- std::cout << "wolf_problem_ deleted!" << std::endl;
- delete ceres_manager_autodiff;
- delete ceres_manager_analytic;
- std::cout << "ceres_manager deleted!" << std::endl;
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/src/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp
deleted file mode 100644
index 6fa1e01edddf6861f52c6c50222be29521ce68a2..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
deleted file mode 100644
index 21c2a8b9495dc92fa43dfae9dd0c87238971125d..0000000000000000000000000000000000000000
--- a/src/examples/test_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 2D");
-
- 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/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
deleted file mode 100644
index 1e85039d7d5a25c6320c74d0d1914c372a9220f6..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_eigen_template.cpp b/src/examples/test_eigen_template.cpp
deleted file mode 100644
index 47e5aa4191e1d680b4cbc5a8702d7c55f74ed52e..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp
deleted file mode 100644
index 10c7610ca7c523cd56689896aeaf4549e155a58b..0000000000000000000000000000000000000000
--- a/src/examples/test_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 3D");
-
- /* 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/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
deleted file mode 100644
index 9d7d73379708fe897e8ec0b7bb00e39fda7f8559..0000000000000000000000000000000000000000
--- a/src/examples/test_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_FRAME, 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_FRAME, 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_FRAME, 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/src/examples/test_factor_odom_3D.cpp b/src/examples/test_factor_odom_3D.cpp
deleted file mode 100644
index be7a81c4f454a8c3c6a4bc621b9de8cb3ef39a0d..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
deleted file mode 100644
index 4414520dc1fc0edf2260ad4cf416ee3ddd2ea56d..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_fcn_ptr.cpp b/src/examples/test_fcn_ptr.cpp
deleted file mode 100644
index 8ca82790e934cb0c9236ad83cfe7a92d1cc6d284..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_image.cpp b/src/examples/test_image.cpp
deleted file mode 100644
index 842a6a14b6e682da3481a7f3bd51d80e819fb857..0000000000000000000000000000000000000000
--- a/src/examples/test_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 3D");
-
- //=====================================================
- // 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/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
deleted file mode 100644
index c05fbbf9c4e676c5611c7073b94b9f55385a9267..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
deleted file mode 100644
index 039615445807ab6ef9e1dadab7e273135bc650ef..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
deleted file mode 100644
index e0e4e4778e91090221d70121bd89907ad3756823..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
deleted file mode 100644
index 817b08a14a4c71caf9f9e4807c71cc14cc43d78e..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
deleted file mode 100644
index ea1079d75c7f11cb3de61fe55df12ce3bd39c441..0000000000000000000000000000000000000000
--- a/src/examples/test_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 2D");
-
- 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/src/examples/test_list_remove.cpp b/src/examples/test_list_remove.cpp
deleted file mode 100644
index b0795d0a687e92b774c50de49c2645a488676ced..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
deleted file mode 100644
index c4ba44c55669a60a9d188a0217f537be30e31fce..0000000000000000000000000000000000000000
--- a/src/examples/test_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 2D");
- 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/src/examples/test_matrix_prod.cpp b/src/examples/test_matrix_prod.cpp
deleted file mode 100644
index b03068283ac33b65a430534a3deb8cf0856195ed..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
deleted file mode 100644
index f2d5bbade62637c592b5f2dd0cf2341d825dded8..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_polylines.cpp b/src/examples/test_polylines.cpp
deleted file mode 100644
index b972424c7c86018386d2ee0b5fded686b7239c75..0000000000000000000000000000000000000000
--- a/src/examples/test_polylines.cpp
+++ /dev/null
@@ -1,305 +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 "capture_void.h"
-#include "feature_odom_2D.h"
-#include "constraint_base.h"
-#include "ceres_wrapper/ceres_manager.h"
-
-// EIGEN
-//#include <Eigen/CholmodSupport>
-#include <Eigen/StdVector> // Eigen in std vector
-
-namespace wolf{
-
-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_FRAME, 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_FRAME, 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->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->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->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->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
- ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, frame_to_ptr);
- feature_ptr->addConstraint(constraint_ptr);
- frame_to_ptr->addConstrainedBy(constraint_ptr);
-
- // SOLVE
- // solution
- bl_summary = bl_ceres_manager->solve(1);
- std::cout << bl_summary << std::endl;
-
- // covariance
- bl_ceres_manager->computeCovariances(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/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
deleted file mode 100644
index ec77aedeee3242daffe10d89c1444fa9ea1bf1f9..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
deleted file mode 100644
index 988244d1598d3a8a9a1bdebf0144266b6744044b..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
deleted file mode 100644
index 8103a7ff39f3e1dc55788230bcbded86b56927cc..0000000000000000000000000000000000000000
--- a/src/examples/test_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 3D");
- 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/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
deleted file mode 100644
index a5c2476d700e0651bc027787e2b58d6b9e339e78..0000000000000000000000000000000000000000
--- a/src/examples/test_processor_tracker_feature.cpp
+++ /dev/null
@@ -1,87 +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"
-
-void print_prc(wolf::ProcessorTrackerFeaturePtr _prc)
-{
- using namespace wolf;
-
- auto o = _prc->getOrigin();
- auto l = _prc->getLast();
- auto i = _prc->getIncoming();
-
- std::cout << "o: C" << ( int)( o ? o->id() : -99 )
- << " || l: C" << ( int)( l ? l->id() : -99 )
- << " || i: C" << ( int)( i ? i->id() : -99 )
- << std::endl;
- for (auto ftr : _prc->getLast()->getFeatureList())
- {
- SizeStd trk_id = ftr->trackId();
- std::cout << "Track " << trk_id << " ---------------------" << std::endl;
- for (auto ftr_pair : _prc->track(trk_id))
- {
- auto f = ftr_pair.second;
- auto C = f->getCapture();
- std::cout << "f" << f->id() << " C" << (int)(C ? C->id() : -99) << std::endl;
- }
- }
-}
-
-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 2D");
- 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->voting_active = true;
- params_trk->max_new_features = 7;
- params_trk->min_features_for_keyframe = 4;
- 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++)
- {
- std::cout << "\n===== Capture TS = " << t << " =====" << std::endl;
- processor_ptr_->process(make_shared<CaptureVoid>(t, sensor_ptr_));
-
- print_prc(processor_ptr_);
-
- t += dt;
- }
-
- wolf_problem_ptr_->print(2);
-
- return 0;
-}
-
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
deleted file mode 100644
index 05eb1a5194b43dadb453120af570ebbc342fb7e3..0000000000000000000000000000000000000000
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ /dev/null
@@ -1,92 +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 2D");
- 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);
-
- 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);
-
- 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/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
deleted file mode 100644
index f3c79633d5b27a3ec3fc84bb52124eb080380993..0000000000000000000000000000000000000000
--- a/src/examples/test_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 3D");
-
- // 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_FRAME, (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/src/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp
deleted file mode 100644
index 6d6f943ab4b3ee11ea03965f8a872fc3454a9f7b..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_sh_ptr.cpp b/src/examples/test_sh_ptr.cpp
deleted file mode 100644
index 3a10903c85ee72967dbe8ca23dd1584dbda494b8..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
deleted file mode 100644
index 290e0915cb85612c6c310eb898b7da42badd7104..0000000000000000000000000000000000000000
--- a/src/examples/test_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 3D");
- // One anchor frame to define the lmk, and a copy to make a factor
- FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(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_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_4 = problem->emplaceFrame(KEY_FRAME,(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/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
deleted file mode 100644
index b27e1c0070ef55f91a4ddcb592dc30f0e6c0ac77..0000000000000000000000000000000000000000
--- a/src/examples/test_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_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
- FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
- FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
- problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
- FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
- FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl;
- frm5->setKey();
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl;
- frm2->setKey();
-
- 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->setKey();
-
- printFrames(problem_ptr);
-
- std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl;
- frm6->setKey();
-
- printFrames(problem_ptr);
-
- FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY_FRAME, 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_FRAME, 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/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
deleted file mode 100644
index fa577c15b88f05a9f9f94a7f63eb926f493cf2c7..0000000000000000000000000000000000000000
--- a/src/examples/test_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_FRAME, 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_FRAME, 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/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
deleted file mode 100644
index 6528a2b3935e488c7534c44f031a7cff22a112ed..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_tracker_ORB.cpp b/src/examples/test_tracker_ORB.cpp
deleted file mode 100644
index 89ea1800adf53cd76e7e858ab8c3c1fe1e0fb9ed..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_virtual_hierarchy.cpp b/src/examples/test_virtual_hierarchy.cpp
deleted file mode 100644
index fb1b248ce8bae0371579391351daec340fa2c8c5..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp
deleted file mode 100644
index 8d0897956275c596d47782117311784b8ea2828e..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
deleted file mode 100644
index ce7db15c8ee5a986d8d4ed469efd9ee57dbf3351..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
deleted file mode 100644
index 9463399ff7843eca655e702d768a7b62fe383528..0000000000000000000000000000000000000000
--- a/src/examples/test_wolf_imported_graph.cpp
+++ /dev/null
@@ -1,415 +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"
-
-// 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)
-{
- 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);
- Scalar xi, yi, thi, si, ci, xj, yj;
-
- // 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);
-
- // 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_FRAME, 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_FRAME, 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();
-
- // 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);
- 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;
- //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;
-
- 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;
- }
- }
- 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("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);
- 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);
- Lambda = Lambda + DeltaLambda;
-
- // 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()));
- 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;
-
- 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;
-
- // 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;
-
- // 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;
-
- //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 << "IG = " << IG << std::endl;
-
- if (IG < 2)
- (*c_it)->setStatus(FAC_INACTIVE);
- }
- 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 << "solving..." << std::endl;
- summary_full = ceres_manager_full->solve();
- std::cout << summary_full.FullReport() << 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/src/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp
deleted file mode 100644
index ee3b9b5763037355fcf24ec813a79199067eb227..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
deleted file mode 100644
index de917e471e6535c626561ab398e8bf00d3637725..0000000000000000000000000000000000000000
--- a/src/examples/test_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_FRAME, 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_FRAME, 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/src/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp
deleted file mode 100644
index 4ea048471753a28281c01dc50f3fcda0316f0bd7..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_wolf_tree.cpp b/src/examples/test_wolf_tree.cpp
deleted file mode 100644
index 9a0075a93c089db05f9bb044ec9c5f90f237228f..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
deleted file mode 100644
index 5768f4b50ce0963ba4974baa10d27c4ffc1e2382..0000000000000000000000000000000000000000
--- a/src/examples/test_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/src/examples/test_yaml_conversions.cpp b/src/examples/test_yaml_conversions.cpp
deleted file mode 100644
index b8e1f41f9c0f8b6ae50e65c4fec943dbb1305a7d..0000000000000000000000000000000000000000
--- a/src/examples/test_yaml_conversions.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-/**
- * \file test_yaml_conversions.cpp
- *
- * Created on: May 15, 2016
- * \author: jsola
- */
-
-#include "core/yaml/yaml_conversion.h"
-
-#include <yaml-cpp/yaml.h>
-
-#include <eigen3/Eigen/Dense>
-
-#include <iostream>
-//#include <fstream>
-
-int main()
-{
-
- using namespace Eigen;
-
- std::cout << "\nTrying different yaml specs for matrix..." << std::endl;
-
- YAML::Node mat_sized_23, mat_sized_33, mat_sized_bad, mat_23, mat_33, mat_bad;
-
- mat_sized_23 = YAML::Load("[[2, 3] ,[1, 2, 3, 4, 5, 6] ]"); // insensitive to spacing
- mat_sized_33 = YAML::Load("[[3, 3] ,[1, 2, 3, 4, 5, 6, 7, 8, 9]]"); // insensitive to spacing
-
- mat_23 = YAML::Load("[1, 2, 3, 4, 5, 6]"); // insensitive to spacing
- mat_33 = YAML::Load("[1, 2, 3, 4, 5, 6, 7, 8, 9]"); // insensitive to spacing
-
- MatrixXd Mx = mat_sized_23.as<MatrixXd>();
- std::cout << "Dyn-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << Mx << 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;
-
- 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;
-
- 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;
-
- M2D = mat_23.as<Matrix<double, 2, Dynamic>>();
- std::cout << "2-Dyn [1, 2, 3, 4, 5, 6] = \n" << M2D << std::endl;
-
- MD3 = mat_23.as<Matrix<double, Dynamic, 3>>();
- std::cout << "Dyn-3 [1, 2, 3, 4, 5, 6] = \n" << MD3 << std::endl;
-
- M3 = mat_33.as<Matrix3d>();
- std::cout << "3-3 [1, 2, 3, 4, 5, 6, 7, 8, 9] = \n" << M3 << std::endl;
-
- return 0;
-}
diff --git a/src/feature/feature_icp_align.cpp b/src/feature/feature_icp_align.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82a38d2d93d0ef8ef38084fd54b7604c1c592a8b
--- /dev/null
+++ b/src/feature/feature_icp_align.cpp
@@ -0,0 +1,54 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/feature/feature_icp_align.h"
+
+namespace wolf {
+
+FeatureICPAlign::FeatureICPAlign(const laserscanutils::icpOutput& _icp_align_res):
+ FeatureBase("FeatureICPAlign", _icp_align_res.res_transf, _icp_align_res.res_covar),
+ icp_align_res_(_icp_align_res)
+{
+
+}
+
+FeatureICPAlign::~FeatureICPAlign()
+{
+ //
+}
+
+void FeatureICPAlign::findFactors()
+{
+
+}
+
+const laserscanutils::icpOutput& FeatureICPAlign::getQualityAlign() const
+{
+ return icp_align_res_;
+}
+
+Eigen::Vector3s FeatureICPAlign::getTransf()
+{
+ return this->icp_align_res_.res_transf;
+}
+
+
+} // namespace wolf
diff --git a/src/feature/feature_match_polyline_2d.cpp b/src/feature/feature_match_polyline_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c748f8ea9816d94e27a81b207a43df74e62a4fe
--- /dev/null
+++ b/src/feature/feature_match_polyline_2d.cpp
@@ -0,0 +1,180 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/feature/feature_match_polyline_2d.h"
+#include "laser/feature/feature_polyline_2d.h"
+#include "laser/processor/polyline_2d_utils.h"
+
+namespace wolf {
+
+bool FeatureMatchPolyline2d::fromDefined() const
+{
+ assert(pl_last_ != nullptr);
+
+ return (feature_last_from_id_ != 0 || pl_last_->isFirstDefined()) &&
+ (feature_incoming_from_id_ != 0 || pl_incoming_->isFirstDefined());
+}
+
+bool FeatureMatchPolyline2d::toDefined() const
+{
+ assert(pl_incoming_ != nullptr);
+ assert(pl_last_ != nullptr);
+
+ return (feature_last_to_id_ != pl_last_->getNPoints()-1 || pl_last_->isLastDefined()) &&
+ (feature_incoming_to_id_ != pl_incoming_->getNPoints()-1 || pl_incoming_->isLastDefined());
+}
+
+unsigned int FeatureMatchPolyline2d::getNPoints() const
+{
+ return feature_incoming_to_id_ - feature_incoming_from_id_ + 1;
+}
+
+unsigned int FeatureMatchPolyline2d::getNDefinedPoints() const
+{
+ return getNPoints() - (fromDefined() ? 0:1) - (toDefined() ? 0:1);
+}
+
+bool FeatureMatchPolyline2d::check(bool check_partial_match) const
+{
+ // feature last polyline type
+ if (pl_last_ == nullptr)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature last polyline 2d nullptr");
+ return false;
+ }
+ if (pl_last_->isRemoving())
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d removed feature last");
+ return false;
+ }
+ // feature polyline type
+ if (pl_incoming_ == nullptr)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature incoming polyline 2d nullptr");
+ return false;
+ }
+ if (pl_incoming_->isRemoving())
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d removed feature incoming");
+ return false;
+ }
+ // Check idxs
+ if (feature_last_from_id_ > feature_last_to_id_)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature_last_from_id_ > feature_last_to_id_");
+ return false;
+ }
+ if (feature_last_from_id_ < 0)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature_last_from_id_ < 0");
+ return false;
+ }
+ if (feature_last_to_id_ > pl_last_->getNPoints()-1)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature_last_to_id_ < pl_last_->getNPoints()-1");
+ return false;
+ }
+ if (feature_incoming_from_id_ > feature_incoming_to_id_)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature_incoming_from_id_ > feature_incoming_to_id_");
+ return false;
+ }
+ if (feature_incoming_from_id_ < 0)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature_incoming_from_id_ < 0");
+ return false;
+ }
+ if (feature_incoming_to_id_ > pl_incoming_->getNPoints()-1)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature_incoming_to_id_ < pl_incoming_->getNPoints()-1");
+ return false;
+ }
+ // partial match
+ if (check_partial_match)
+ {
+ if (feature_incoming_from_id_ != 0 && feature_last_from_id_ != 0)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d partial match in front");
+ return false;
+ }
+ if (feature_incoming_to_id_ != pl_incoming_->getNPoints()-1 && feature_last_to_id_ != pl_last_->getNPoints()-1)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d partial match in back");
+ return false;
+ }
+ }
+ // correspondences
+ if (feature_last_to_id_ - feature_last_from_id_ != feature_incoming_to_id_ - feature_incoming_from_id_)
+ {
+ WOLF_ERROR("FeatureMatchPolyline2d feature_last_to_id_ - feature_last_from_id_ != feature_incoming_to_id_ - feature_incoming_from_id_");
+ return false;
+ }
+
+ return true;
+}
+
+Eigen::Array<double,Eigen::Dynamic,1> FeatureMatchPolyline2d::computeSqDistArray() const
+{
+ assert(pl_incoming_ != nullptr);
+ assert(pl_last_ != nullptr);
+
+ return computeSqDist(T_incoming_last_ * pl_last_->getPoints(), //_points_A
+ feature_last_from_id_, //from_A,
+ feature_last_to_id_, //to_A,
+ feature_last_from_id_ != 0 || pl_last_->isFirstDefined(), //from_A_defined,
+ feature_last_to_id_ != pl_last_->getNPoints()-1 || pl_last_->isLastDefined(), //to_A_defined,
+ pl_incoming_->getPoints(), //_points_B
+ feature_incoming_from_id_, //from_B,
+ feature_incoming_to_id_, //to_B,
+ feature_incoming_from_id_ != 0 || pl_incoming_->isFirstDefined(),//from_B_defined,
+ feature_incoming_to_id_ != pl_incoming_->getNPoints()-1 || pl_incoming_->isLastDefined(), //to_B_defined,
+ pl_last_->getNPoints()-1, //last_A,
+ getNPoints()); //N_overlapped
+}
+
+void FeatureMatchPolyline2d::print() const
+{
+ assert(pl_incoming_ != nullptr);
+ assert(pl_last_ != nullptr);
+
+ std::cout << std::endl << "POLYLINE FEATURE MATCH" << std::endl;
+ std::cout << "INCOMING: " << pl_incoming_->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_incoming_->getNPoints() << std::endl;
+ std::cout << "\tfirst defined " << pl_incoming_->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_incoming_->isLastDefined() << std::endl;
+ std::cout << "\tpoints \n" << pl_incoming_->getPoints() << std::endl;
+
+ std::cout << "LAST " << pl_last_->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_last_->getNPoints() << std::endl;
+ std::cout << "\tfirst defined " << pl_last_->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_last_->isLastDefined() << std::endl << std::endl;
+ std::cout << "\tpoints \n" << pl_last_->getPoints() << std::endl;
+ std::cout << "\tpoints (local) \n" << T_incoming_last_ * pl_last_->getPoints() << std::endl;
+
+ std::cout << "MATCH " << std::endl;
+ std::cout << "\tfrom feature incoming point " << feature_incoming_from_id_ << std::endl;
+ std::cout << "\tto feature incoming point " << feature_incoming_to_id_ << std::endl;
+ std::cout << "\tfrom feature last point " << feature_last_from_id_ << std::endl;
+ std::cout << "\tto feature last point " << feature_last_to_id_ << std::endl;
+ std::cout << "\tT_incoming_last_ " << std::endl << T_incoming_last_ << std::endl;
+}
+
+} // namespace wolf
diff --git a/src/feature/feature_polyline_2D.cpp b/src/feature/feature_polyline_2D.cpp
deleted file mode 100644
index 9b281eb920ba1738b1f4808f7d53076a16f1795f..0000000000000000000000000000000000000000
--- a/src/feature/feature_polyline_2D.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-/**
- * \file feature_polyline.cpp
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#include "laser/feature/feature_polyline_2D.h"
-
-namespace wolf
-{
-
-} /* namespace wolf */
diff --git a/src/feature/feature_polyline_2d.cpp b/src/feature/feature_polyline_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2afe6b82bcbfdb7594666e5e1d19bdc44e82a92e
--- /dev/null
+++ b/src/feature/feature_polyline_2d.cpp
@@ -0,0 +1,34 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file feature_polyline.cpp
+ *
+ * Created on: May 26, 2016
+ * \author: jvallve
+ */
+
+#include "laser/feature/feature_polyline_2d.h"
+
+namespace wolf
+{
+
+} /* namespace wolf */
diff --git a/src/feature/feature_scene_falko.cpp b/src/feature/feature_scene_falko.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39e6b054fd48a379dfc6abc1ceadb7bd0f9e3de4
--- /dev/null
+++ b/src/feature/feature_scene_falko.cpp
@@ -0,0 +1,27 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/feature/feature_scene_falko.h"
+
+namespace wolf
+{
+
+} // namespace wolf
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
deleted file mode 100644
index 318255a731e3bc0cf24fb7758c4c2e8e937bfedf..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_match_polyline_2D.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
- * landmark_match_polyline_2D.cpp
- *
- * Created on: Apr 2, 2019
- * Author: jvallve
- */
-
-#include "laser/landmark/landmark_match_polyline_2D.h"
-#include "laser/landmark/landmark_polyline_2D.h"
-
-namespace wolf {
-
-bool LandmarkMatchPolyline2D::check() const
-{
- LandmarkPolyline2DPtr pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_);
- if (pl_lmk == nullptr)
- {
- WOLF_ERROR("LandmarkMatchPolyline2D landmark is not polyline 2D");
- return false;
- }
- if (!pl_lmk->isClosed())
- {
- if (landmark_from_id_ > landmark_to_id_)
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk");
- return false;
- }
- if (landmark_from_id_ < pl_lmk->getFirstId())
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk");
- return false;
- }
- if (landmark_to_id_ > pl_lmk->getLastId())
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk");
- return false;
- }
- }
- int lmk_id = landmark_from_id_;
- int ftr_id = feature_from_id_;
- while (1)
- {
- if (lmk_id == landmark_to_id_)
- {
- if (ftr_id != feature_to_id_)
- {
- WOLF_ERROR("LandmarkMatchPolyline2D landmark_to_id_ does not correspond to feature_to_id_");
- return false;
- }
- break;
- }
- else if (lmk_id == pl_lmk->getLastId() && !pl_lmk->isClosed())
- {
- WOLF_ERROR("LandmarkMatchPolyline2D lmk_id == lmk_last_id but lmk_id != landmark_to_id_ in a not closed lmk");
- return false;
- }
- else
- {
- lmk_id = pl_lmk->getNextValidId(lmk_id);
- ftr_id++;
- }
- }
- return true;
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_match_polyline_2d.cpp b/src/landmark/landmark_match_polyline_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cfb48ee32589cb60d305085440493577b3cbfa27
--- /dev/null
+++ b/src/landmark/landmark_match_polyline_2d.cpp
@@ -0,0 +1,244 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * landmark_match_polyline_2d.cpp
+ *
+ * Created on: Apr 2, 2019
+ * Author: jvallve
+ */
+
+#include "laser/landmark/landmark_match_polyline_2d.h"
+#include "laser/landmark/landmark_polyline_2d.h"
+#include "laser/feature/feature_polyline_2d.h"
+#include "laser/processor/polyline_2d_utils.h"
+
+namespace wolf {
+
+
+
+bool LandmarkMatchPolyline2d::fromDefined() const
+{
+ assert(pl_landmark_ != nullptr);
+
+ return (landmark_from_id_ != pl_landmark_->getFirstId() || pl_landmark_->isFirstDefined()) &&
+ (feature_from_id_ != 0 || pl_landmark_->isFirstDefined());
+}
+
+bool LandmarkMatchPolyline2d::toDefined() const
+{
+ assert(pl_feature_ != nullptr);
+ assert(pl_landmark_ != nullptr);
+
+ return (landmark_to_id_ != pl_landmark_->getLastId() || pl_landmark_->isLastDefined()) &&
+ (feature_to_id_ != pl_feature_->getNPoints()-1 || pl_feature_->isLastDefined());
+}
+
+unsigned int LandmarkMatchPolyline2d::getNPoints() const
+{
+ return feature_to_id_ - feature_from_id_ + 1;
+}
+
+unsigned int LandmarkMatchPolyline2d::getNDefinedPoints() const
+{
+ return getNPoints() - (fromDefined() ? 0:1) - (toDefined() ? 0:1);
+}
+
+bool LandmarkMatchPolyline2d::isDeprecated() const
+{
+ assert(pl_landmark_ != nullptr);
+
+ return landmark_version_ != pl_landmark_->getVersion() || pl_landmark_->getMergedInLandmark() != nullptr;
+}
+
+bool LandmarkMatchPolyline2d::isPartialFront() const
+{
+ assert(pl_landmark_ != nullptr);
+
+ // exceeding points if: feature point is not the extreme && either landmark point is not the extreme or landmark is closed
+ return (feature_from_id_ != 0 && (landmark_from_id_ != pl_landmark_->getFirstId() || pl_landmark_->isClosed()));
+}
+
+bool LandmarkMatchPolyline2d::isPartialBack() const
+{
+ assert(pl_feature_ != nullptr);
+ assert(pl_landmark_ != nullptr);
+
+ // exceeding points if: feature point is not the extreme && either landmark point is not the extreme or landmark is closed
+ return (feature_to_id_ != pl_feature_->getNPoints()-1 && (landmark_to_id_ != pl_landmark_->getLastId() || pl_landmark_->isClosed()));
+}
+
+
+bool LandmarkMatchPolyline2d::check(bool check_partial_match) const
+{
+ // landmark polyline type
+ if (pl_landmark_ == nullptr)
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d landmark polyline 2d nullptr");
+ return false;
+ }
+ if (isDeprecated())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d landmark polyline 2d was merged or changed the version");
+ return false;
+ }
+ if (pl_landmark_->isRemoving())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d removed landmark");
+ return false;
+ }
+ // feature polyline type
+ if (pl_feature_ == nullptr)
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d feature polyline 2d nullptr");
+ return false;
+ }
+ if (pl_feature_->isRemoving())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d removed feature");
+ return false;
+ }
+ // Check idxs
+ if (!pl_landmark_->isClosed() && landmark_from_id_ > landmark_to_id_)
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d lmk_from > lmk_to with a not closed lmk");
+ return false;
+ }
+ if (!pl_landmark_->isClosed() && landmark_from_id_ < pl_landmark_->getFirstId())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d lmk_from < lmk_first with a not closed lmk");
+ return false;
+ }
+ if (!pl_landmark_->isClosed() && landmark_to_id_ > pl_landmark_->getLastId())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d lmk_to < lmk_last with a not closed lmk");
+ return false;
+ }
+ // partial match
+ if (check_partial_match)
+ {
+ if (isPartialFront())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d partial match in front");
+ return false;
+ }
+ if (isPartialBack())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d partial match in back");
+ return false;
+ }
+ }
+ // N points
+ int landmark_to_idx = pl_landmark_->id2idx(landmark_to_id_);
+ int landmark_from_idx = pl_landmark_->id2idx(landmark_from_id_);
+ int N_landmark = (landmark_to_idx >= landmark_from_idx ?
+ landmark_to_idx - landmark_from_idx + 1 :
+ pl_landmark_->getNPoints() - (landmark_from_idx - landmark_to_idx) + 1);
+ int N_feature = feature_to_id_ - feature_from_id_ + 1;
+ if (N_landmark != N_feature)
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d different amount of matching points in landmark and feature",
+ "landmark_from_idx: ", pl_landmark_->id2idx(landmark_from_id_),
+ "landmark_to_idx: ", pl_landmark_->id2idx(landmark_to_id_),
+ "feature_from_id_: ", feature_from_id_,
+ "feature_to_id_: ", feature_to_id_);
+ return false;
+ }
+ // correspondences
+ int lmk_id = landmark_from_id_;
+ int ftr_id = feature_from_id_;
+ while (1)
+ {
+ if (lmk_id == landmark_to_id_)
+ {
+ if (ftr_id != feature_to_id_)
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d landmark_to_id_ does not correspond to feature_to_id_");
+ return false;
+ }
+ break;
+ }
+ else if (lmk_id == pl_landmark_->getLastId() && !pl_landmark_->isClosed())
+ {
+ WOLF_ERROR("LandmarkMatchPolyline2d lmk_id == lmk_last_id but lmk_id != landmark_to_id_ in a not closed lmk");
+ return false;
+ }
+ else
+ {
+ lmk_id = pl_landmark_->getNextValidId(lmk_id);
+ ftr_id++;
+ }
+ }
+
+ return true;
+}
+
+Eigen::Array<double,Eigen::Dynamic,1> LandmarkMatchPolyline2d::computeSqDistArray() const
+{
+ assert(pl_feature_ != nullptr);
+ assert(pl_landmark_ != nullptr);
+
+ return computeSqDist(T_feature_landmark_ * pl_landmark_->computePointsGlobal(landmark_from_id_,landmark_to_id_),//_points_A
+ 0, //from_A,
+ getNPoints()-1, //to_A,
+ landmark_from_id_ != pl_landmark_->getFirstId() || pl_landmark_->isFirstDefined(), //from_A_defined,
+ landmark_to_id_ != pl_landmark_->getLastId() || pl_landmark_->isLastDefined(), //to_A_defined,
+ pl_feature_->getPoints(), //_points_B
+ feature_from_id_, //from_B,
+ feature_to_id_, //to_B,
+ feature_from_id_ != 0 || pl_feature_->isFirstDefined(), //from_B_defined,
+ feature_to_id_ != pl_feature_->getNPoints()-1 || pl_feature_->isLastDefined(), //to_B_defined,
+ getNPoints()-1, //last_A,
+ getNPoints()); //N_overlapped
+}
+
+void LandmarkMatchPolyline2d::print() const
+{
+ assert(pl_feature_ != nullptr);
+ assert(pl_landmark_ != nullptr);
+
+ std::cout << std::endl << "POLYLINE LANDMARK MATCH" << std::endl;
+ std::cout << "FEATURE: " << pl_feature_->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_feature_->getNPoints() << std::endl;
+ std::cout << "\tfirst defined " << pl_feature_->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_feature_->isLastDefined() << std::endl;
+ std::cout << "\tpoints \n" << pl_feature_->getPoints() << std::endl;
+
+ std::cout << "LANDMARK " << pl_landmark_->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_landmark_->getNPoints() << " ( ";
+ std::vector<int> lmk_ids = pl_landmark_->getValidIds();
+ for (auto id : lmk_ids)
+ std::cout << id << " ";
+ std::cout << ")" << std::endl;
+ std::cout << "\tfirst defined " << pl_landmark_->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_landmark_->isLastDefined() << std::endl << std::endl;
+ std::cout << "\tpoints \n" << pl_landmark_->computePointsGlobal() << std::endl;
+ std::cout << "\tpoints (local) \n" << T_feature_landmark_ * pl_landmark_->computePointsGlobal() << std::endl;
+
+ std::cout << "MATCH " << std::endl;
+ std::cout << "\tfrom feature point " << feature_from_id_ << std::endl;
+ std::cout << "\tto feature point " << feature_to_id_ << std::endl;
+ std::cout << "\tfrom landmark point " << landmark_from_id_ << std::endl;
+ std::cout << "\tto landmark point " << landmark_to_id_ << std::endl;
+ std::cout << "\tT_feature_landmark_ " << std::endl << T_feature_landmark_ << std::endl;
+}
+
+} // namespace wolf
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2d.cpp
similarity index 66%
rename from src/landmark/landmark_polyline_2D.cpp
rename to src/landmark/landmark_polyline_2d.cpp
index ab06878f1eab0a8501f56cba1b639ced9cdb337f..bdd6566a85dcf025d6053ce62cfc7b47ab2e98a4 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2d.cpp
@@ -1,15 +1,36 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
/**
- * \file landmark_polyline_2D.cpp
+ * \file landmark_polyline_2d.cpp
*
* Created on: May 26, 2016
* \author: jvallve
*/
-#include "laser/feature/feature_polyline_2D.h"
-#include "laser/landmark/landmark_polyline_2D.h"
+#include "laser/feature/feature_polyline_2d.h"
+#include "laser/landmark/landmark_polyline_2d.h"
#include "laser/state_block/local_parametrization_polyline_extreme.h"
-#include "laser/factor/factor_point_2D.h"
-#include "laser/factor/factor_point_to_line_2D.h"
+#include "laser/factor/factor_point_2d.h"
+#include "laser/factor/factor_point_to_line_2d.h"
#include "core/state_block/state_block.h"
#include "core/common/factory.h"
#include "core/yaml/yaml_conversion.h"
@@ -18,15 +39,20 @@ namespace wolf
{
-LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id) :
- LandmarkPolyline2D(_p_ptr, _o_ptr, _points, _first_defined, _last_defined, _first_id, PolylineRectangularClass())
+LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id) :
+ LandmarkPolyline2d(_p_ptr, _o_ptr, _points, _first_defined, _last_defined, _first_id, PolylineRectangularClass())
{
}
-LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
- LandmarkBase("POLYLINE 2D", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
+//<<<<<<< HEAD
+LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
+ LandmarkBase("LandmarkPolyline2d", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
+//=======
+//LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
+// LandmarkBase("POLYLINE 2d", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
+//>>>>>>> 8-replace-scalar-to-double
{
- //std::cout << "LandmarkPolyline2D::LandmarkPolyline2D" << std::endl;
+ //std::cout << "LandmarkPolyline2d::LandmarkPolyline2d" << std::endl;
assert(_points.cols() >= 2 && "2 points at least needed.");
for (auto i = 0; i < _points.cols(); i++)
@@ -37,54 +63,51 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
if (!last_defined_)
lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.rbegin())->second));
- first_id_ = getFirstId();
- last_id_ = getLastId();
-
- //std::cout << "LandmarkPolyline2D " << id() << " created. First: " << first_id_ << " last: "<< last_id_ << std::endl;
+ //std::cout << "LandmarkPolyline2d " << id() << " created. First: " << getFirstId() << " last: "<< getLastId() << std::endl;
}
-LandmarkPolyline2D::~LandmarkPolyline2D()
+LandmarkPolyline2d::~LandmarkPolyline2d()
{
removeStateBlocks();
}
-void LandmarkPolyline2D::setFirst(const Eigen::VectorXs& _point, bool _defined)
+void LandmarkPolyline2d::setFirst(const Eigen::VectorXd& _point, bool _defined)
{
- //std::cout << "LandmarkPolyline2D::setFirst. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "LandmarkPolyline2D::setFirstExtreme: bad point size");
+ //std::cout << "LandmarkPolyline2d::setFirst. Defined " << _defined << std::endl;
+ assert(_point.size() >= 2 && "LandmarkPolyline2d::setFirstExtreme: bad point size");
assert(!(!_defined && first_defined_) && "setting a defined extreme with a not defined point");
firstStateBlock()->setState(_point.head(2));
if (!first_defined_ && _defined)
defineFirst();
}
-void LandmarkPolyline2D::setLast(const Eigen::VectorXs& _point, bool _defined)
+void LandmarkPolyline2d::setLast(const Eigen::VectorXd& _point, bool _defined)
{
- //std::cout << "LandmarkPolyline2D::setLast. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "LandmarkPolyline2D::setLastExtreme: bad point size");
+ //std::cout << "LandmarkPolyline2d::setLast. Defined " << _defined << std::endl;
+ assert(_point.size() >= 2 && "LandmarkPolyline2d::setLastExtreme: bad point size");
assert(!(!_defined && last_defined_) && "setting a defined extreme with a not defined point");
lastStateBlock()->setState(_point.head(2));
if (!last_defined_ && _defined)
defineLast();
}
-const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
+const Eigen::VectorXd LandmarkPolyline2d::getPointVector(int _i) const
{
assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
return point_state_ptr_map_.at(_i)->getState();
}
-StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
+StateBlockPtr LandmarkPolyline2d::getPointStateBlock(int _i)
{
assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
return point_state_ptr_map_.at(_i);
}
-Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal() const
+Eigen::MatrixXd LandmarkPolyline2d::computePointsGlobal() const
{
- Eigen::MatrixXs points_global = Eigen::MatrixXs::Ones(3,getNPoints());
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getO()->getState()(0)).matrix();
- Eigen::Vector2s t_world_points = getP()->getState();
+ Eigen::MatrixXd points_global = Eigen::MatrixXd::Ones(3,getNPoints());
+ Eigen::Matrix2d R_world_points = Eigen::Rotation2Dd(getO()->getState()(0)).matrix();
+ Eigen::Vector2d t_world_points = getP()->getState();
for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id))
{
@@ -96,18 +119,52 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal() const
if (classification_.type != UNCLASSIFIED) // points are in landmark PO reference only when clasified
points_global.topRows(2) = (R_world_points * points_global.topRows(2)).colwise() + t_world_points;
+ assert(points_global.isApprox(computePointsGlobal(getFirstId(), getLastId()),1e-12));
+
+ return points_global;
+}
+
+Eigen::MatrixXd LandmarkPolyline2d::computePointsGlobal(const int& _from_id, const int& _to_id) const
+{
+ assert((_from_id <= _to_id || closed_) && "wrong input");
+
+ int N_points = (_from_id <= _to_id ? id2idx(_to_id) - id2idx(_from_id) + 1 : getNPoints() - (id2idx(_from_id) - id2idx(_to_id)) + 1);
+
+ Eigen::MatrixXd points_global = Eigen::MatrixXd::Ones(3,N_points);
+
+ int i = 0, valid_id = _from_id;
+
+ while (i < N_points)
+ {
+ points_global.block(0,i,2,1) = getPointVector(valid_id);
+
+ if (i == N_points-1)
+ break;
+
+ i++;
+ valid_id = getNextValidId(valid_id);
+ }
+ assert(valid_id == _to_id && i == N_points-1);
+
+ if (classification_.type != UNCLASSIFIED) // points are in landmark PO reference only when clasified
+ {
+ Eigen::Matrix2d R_world_points = Eigen::Rotation2Dd(getO()->getState()(0)).matrix();
+ points_global.topRows(2) = (R_world_points * points_global.topRows(2)).colwise() + getP()->getState();
+ }
+
return points_global;
}
-Eigen::MatrixXs LandmarkPolyline2D::computePointsCovGlobal() const
+
+Eigen::MatrixXd LandmarkPolyline2d::computePointsCovGlobal() const
{
- Eigen::MatrixXs points_cov_global = Eigen::MatrixXs::Zero(2,2*getNPoints());
+ Eigen::MatrixXd points_cov_global = Eigen::MatrixXd::Zero(2,2*getNPoints());
for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id))
{
// TODO
- points_cov_global.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
+ points_cov_global.middleCols(i*2, 2) = Eigen::MatrixXd::Identity(2,2);
if (valid_id == getLastId())
break;
@@ -116,11 +173,11 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsCovGlobal() const
return points_cov_global;
}
-void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back)
+void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _defined, const bool& _back)
{
assert(!closed_ && "adding point to a closed polyline!");
- //std::cout << "LandmarkPolyline2D::addPoint. Defined " << _defined << std::endl;
+ //std::cout << "LandmarkPolyline2d::addPoint. Defined " << _defined << std::endl;
assert(_point.size() >= 2 && "bad point size");
// define previous extreme if not defined
@@ -135,10 +192,10 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
(!_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
- point_state_ptr_map_[++last_id_] = new_sb_ptr;
+ point_state_ptr_map_[getLastId()+1]= new_sb_ptr;
if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
+ getProblem()->notifyStateBlock(new_sb_ptr,ADD);
last_defined_ = _defined;
}
@@ -149,27 +206,24 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
(!_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
- point_state_ptr_map_[--first_id_] = new_sb_ptr;
+ point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
+ getProblem()->notifyStateBlock(new_sb_ptr,ADD);
first_defined_ = _defined;
}
// new version
version_++;
-
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
}
-void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined,
+void LandmarkPolyline2d::addPoints(const Eigen::MatrixXd& _points, const unsigned int& _idx, const bool& _defined,
const bool& _back)
{
assert(!closed_ && "adding points to a closed polyline!");
- //std::cout << "LandmarkPolyline2D::addPoints from/to: " << _idx << " Defined " << _defined << std::endl;
+ //std::cout << "LandmarkPolyline2d::addPoints from/to: " << _idx << " Defined " << _defined << std::endl;
assert(_points.rows() >= 2 && "bad points size");
assert(_idx < _points.cols() && "bad index!");
@@ -187,10 +241,10 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
(i == _points.cols()-1 && !_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
- point_state_ptr_map_[++last_id_] = new_sb_ptr;
+ point_state_ptr_map_[getLastId()+1] = new_sb_ptr;
if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
+ getProblem()->notifyStateBlock(new_sb_ptr,ADD);
}
last_defined_ = _defined;
}
@@ -203,33 +257,29 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
(i == 0 && !_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
- point_state_ptr_map_[--first_id_] = new_sb_ptr;
+ point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
if (getProblem())
- getProblem()->addStateBlock(new_sb_ptr);
+ getProblem()->notifyStateBlock(new_sb_ptr,ADD);
}
first_defined_ = _defined;
}
// new version
version_++;
-
- //std::cout << "final number of points: " << point_state_ptr_vector_.size() << std::endl;
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
}
-void LandmarkPolyline2D::defineFirst()
+void LandmarkPolyline2d::defineFirst()
{
defineExtreme(false);
}
-void LandmarkPolyline2D::defineLast()
+void LandmarkPolyline2d::defineLast()
{
defineExtreme(true);
}
-void LandmarkPolyline2D::defineExtreme(const bool _back)
+void LandmarkPolyline2d::defineExtreme(const bool _back)
{
StateBlockPtr extreme_state = (_back ? lastStateBlock() : firstStateBlock());
assert((_back ? !last_defined_: !first_defined_) && "defining an already defined extreme");
@@ -247,7 +297,7 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
first_defined_ = true;
}
-bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
+bool LandmarkPolyline2d::tryClose(const double& _dist_tol)
{
//std::cout << "tryClose landmark " << id() << " with tolerance = " << _dist_tol << std::endl;
@@ -275,8 +325,8 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
std::vector<std::pair<int, int> > overlapped_ids;
// Check first defined point against the rest defined points
- int first_point_id = (first_defined_ ? first_id_ : getNextValidId(first_id_));
- int with_point_id = (last_defined_ ? last_id_ : getPrevValidId(last_id_));
+ int first_point_id = (first_defined_ ? getFirstId() : getNextValidId(getFirstId()));
+ int with_point_id = (last_defined_ ? getLastId() : getPrevValidId(getLastId()));
int last_with_id = first_point_id; // limit for avoiding more than one overlapping per defined point (full overlapping of defined points)
bool found = false;
@@ -312,10 +362,10 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
}
// overlapped points from corresponding first point match to match with last point
- while (with_point_id <= last_id_)
+ while (with_point_id <= getLastId())
{
overlapped_ids.push_back(std::pair<int,int>(first_point_id, with_point_id));
- if (with_point_id == last_id_)
+ if (with_point_id == getLastId())
break;
first_point_id = getNextValidId(first_point_id);
with_point_id = getNextValidId(with_point_id);
@@ -327,17 +377,17 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Check that the other overlapped points are close enough (0 already checked)
for (auto i = 0; i < overlapped_ids.size(); i++)
{
- Scalar sq_dist=1e6;
+ double sq_dist=1e6;
// 2 defined points: point-point
- if ( (overlapped_ids[i].first != first_id_ || first_defined_) &&
- (overlapped_ids[i].second != last_id_ || last_defined_) )
+ if ( (overlapped_ids[i].first != getFirstId() || first_defined_) &&
+ (overlapped_ids[i].second != getLastId() || last_defined_) )
sq_dist = (getPointVector(overlapped_ids[i].first)-getPointVector(overlapped_ids[i].second)).squaredNorm();
// defined with not defined: not defined point-segment
- else if (overlapped_ids[i].first != first_id_ || first_defined_)
+ else if (overlapped_ids[i].first != getFirstId() || first_defined_)
sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].second), getPointVector(overlapped_ids[i].first), getPointVector(getNextValidId(overlapped_ids[i].first)));
// not defined with defined: not defined point-segment
- else if (overlapped_ids[i].second != last_id_ || last_defined_)
+ else if (overlapped_ids[i].second != getLastId() || last_defined_)
sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].first),getPointVector(overlapped_ids[i].second), getPointVector(getPrevValidId(overlapped_ids[i].second)));
else
throw std::runtime_error("when trying to close, overlapping two not defined points in a landmark");
@@ -345,13 +395,13 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
if (sq_dist > _dist_tol*_dist_tol)
{
WOLF_TRACE("\nBad overlapping: ", sq_dist,
- "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""),
- "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : ""));
+ "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""),
+ "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : ""));
return false;
}
WOLF_TRACE("\nGood overlapping: ", sq_dist,
- "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""),
- "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : ""));
+ "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""),
+ "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : ""));
}
}
}
@@ -363,10 +413,10 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
WOLF_TRACE("No overlapping found with defined points, trying with not defined extremes");
// Check for both not defined extremes that the distance to the segment of first and last defined points is below the threshold
- Eigen::Vector2s first_not_def = getPointVector(first_id_);
- Eigen::Vector2s last_not_def = getPointVector(last_id_);
- Eigen::Vector2s first_def = getPointVector(getNextValidId(first_id_));
- Eigen::Vector2s last_def = getPointVector(getPrevValidId(last_id_));
+ Eigen::Vector2d first_not_def = getPointVector(getFirstId());
+ Eigen::Vector2d last_not_def = getPointVector(getLastId());
+ Eigen::Vector2d first_def = getPointVector(getNextValidId(getFirstId()));
+ Eigen::Vector2d last_def = getPointVector(getPrevValidId(getLastId()));
if(sqDistPoint2Segment(first_not_def, last_def, first_def) < _dist_tol*_dist_tol &&
sqDistPoint2Segment(last_not_def, last_def, first_def) < _dist_tol*_dist_tol)
@@ -379,8 +429,8 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Also that the projection of them in the segment should be overlapped
// It means that last_not_def projected onto the segment (first_def-last_def) is closer to first_def than the projection of first_not_def
- Scalar last_proj_2_first_def = (last_def-first_def).dot(last_not_def -first_def) / (last_def-first_def).norm();
- Scalar first_proj_2_first_def = (last_def-first_def).dot(first_not_def-first_def) / (last_def-first_def).norm();
+ double last_proj_2_first_def = (last_def-first_def).dot(last_not_def -first_def) / (last_def-first_def).norm();
+ double first_proj_2_first_def = (last_def-first_def).dot(first_not_def-first_def) / (last_def-first_def).norm();
if (last_proj_2_first_def < first_proj_2_first_def)
{
@@ -388,7 +438,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
WOLF_TRACE("The not defined extremes are overlapped",
"\n\tlast_proj_2_first_def: ", last_proj_2_first_def,
"\n\tfirst_proj_2_first_def: ", first_proj_2_first_def);
- overlapped_ids.push_back(std::pair<int,int>(first_id_, last_id_));
+ overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getLastId()));
}
}
}
@@ -402,7 +452,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
while (!overlapped_ids.empty())
{
// not defined points are merged into defined ones
- if ((overlapped_ids.back().first == first_id_ && !first_defined_) || (overlapped_ids.back().first == last_id_ && !last_defined_))
+ if ((overlapped_ids.back().first == getFirstId() && !first_defined_) || (overlapped_ids.back().first == getLastId() && !last_defined_))
mergePoints(overlapped_ids.back().first,overlapped_ids.back().second);
else
mergePoints(overlapped_ids.back().second,overlapped_ids.back().first);
@@ -413,15 +463,12 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
WOLF_TRACE("Landmark ",id()," was closed.");
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
-
return true;
}
return false;
}
-void LandmarkPolyline2D::setClosed()
+void LandmarkPolyline2d::setClosed()
{
assert(getNDefinedPoints() >= 2 && "closing a polyline with less than 2 defined points");
@@ -432,14 +479,14 @@ void LandmarkPolyline2D::setClosed()
version_++;
}
-void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
+void LandmarkPolyline2d::mergePoints(int _remove_id, int _remain_id)
{
assert(point_state_ptr_map_.find(_remove_id) != point_state_ptr_map_.end() && "removing an inexistent point");
assert(point_state_ptr_map_.find(_remain_id) != point_state_ptr_map_.end() && "merging an inexistent point");
assert(!(_remain_id == getLastId() && !last_defined_) && "in merging points, the remaining point must be defined");
assert(!(_remain_id == getFirstId() && !first_defined_) && "in merging points, the remaining point must be defined");
- //std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << first_id_ << " to " << getLastId() << ")" << std::endl;
+ //std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << getFirstId() << " to " << getLastId() << ")" << std::endl;
StateBlockPtr remove_state = getPointStateBlock(_remove_id);
//std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
@@ -447,23 +494,26 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
// Change factors from remove_state to remain_state
FactorBasePtrList old_factors_list = getConstrainedByList();
//std::cout << "changing " << old_factors_list.size() << " factors." << std::endl;
- FactorBasePtr new_fac_ptr = nullptr;
for (auto fac_ptr : old_factors_list)
{
- FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
- FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
+ auto fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2d>(fac_ptr);
+ auto fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2d>(fac_ptr);
if (fac_point_ptr)
{
// If landmark point constrained -> new factor
if (fac_point_ptr->getLandmarkPointId() == _remove_id)
{
- new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_ptr->getProcessor(),
- fac_point_ptr->getFeaturePointId(),
- _remain_id,
- fac_point_ptr->getApplyLossFunction(),
- fac_point_ptr->getStatus());
+ // emplace new factor
+ FactorBase::emplace<FactorPoint2d>(fac_ptr->getFeature(),
+ std::static_pointer_cast<FeaturePolyline2d>(fac_ptr->getFeature()),
+ std::static_pointer_cast<LandmarkPolyline2d>(shared_from_this()),
+ fac_point_ptr->getProcessor(),
+ fac_point_ptr->getFeaturePointId(),
+ _remain_id,
+ fac_point_ptr->getApplyLossFunction(),
+ fac_point_ptr->getStatus());
+ // remove old factor
+ fac_ptr->remove();
}
}
else if (fac_point_line_ptr)
@@ -471,26 +521,34 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
// If landmark point constrained -> new factor
if (fac_point_line_ptr->getLandmarkPointId() == _remove_id)
{
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- _remain_id,
- fac_point_line_ptr->getLandmarkPointAuxId(),
- fac_point_line_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
+ // emplace new factor
+ FactorBase::emplace<FactorPointToLine2d>(fac_ptr->getFeature(),
+ std::static_pointer_cast<FeaturePolyline2d>(fac_ptr->getFeature()),
+ std::static_pointer_cast<LandmarkPolyline2d>(shared_from_this()),
+ fac_point_line_ptr->getProcessor(),
+ fac_point_line_ptr->getFeaturePointId(),
+ _remain_id,
+ fac_point_line_ptr->getLandmarkPointAuxId(),
+ fac_point_line_ptr->getApplyLossFunction(),
+ fac_point_line_ptr->getStatus());
+ // remove old factor
+ fac_ptr->remove();
}
// If landmark point is aux point -> new factor
else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
{
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- fac_point_line_ptr->getLandmarkPointId(),
- _remain_id,
- fac_point_line_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
+ // emplace new factor
+ FactorBase::emplace<FactorPointToLine2d>(fac_ptr->getFeature(),
+ std::static_pointer_cast<FeaturePolyline2d>(fac_ptr->getFeature()),
+ std::static_pointer_cast<LandmarkPolyline2d>(shared_from_this()),
+ fac_point_line_ptr->getProcessor(),
+ fac_point_line_ptr->getFeaturePointId(),
+ fac_point_line_ptr->getLandmarkPointId(),
+ _remain_id,
+ fac_point_line_ptr->getApplyLossFunction(),
+ fac_point_line_ptr->getStatus());
+ // remove old factor
+ fac_ptr->remove();
}
}
else
@@ -498,54 +556,49 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
WOLF_ERROR("polyline factor of unknown type");
throw std::runtime_error ("polyline factor of unknown type");
}
-
- // If new factor
- if (new_fac_ptr)
- {
- //std::cout << "created new factor: " << new_fac_ptr->id() << std::endl;
- //std::cout << "deleting factor: " << fac_ptr->id() << std::endl;
-
- // add new factor
- fac_ptr->getFeature()->addFactor(new_fac_ptr);
- addConstrainedBy(new_fac_ptr);
-
- // remove factor
- fac_ptr->remove();
-
- new_fac_ptr = nullptr;
- }
}
// If removed was a not defined extreme, set defined extreme
- if (_remove_id == first_id_ && !first_defined_)
+ if (_remove_id == getFirstId() && !first_defined_)
first_defined_ = true;
- if (_remove_id == last_id_ && !last_defined_)
+ if (_remove_id == getLastId() && !last_defined_)
last_defined_ = true;
// Remove remove_state
if (getProblem() != nullptr)
- getProblem()->removeStateBlock(remove_state);
+ getProblem()->notifyStateBlock(remove_state, REMOVE);
//std::cout << "state removed " << std::endl;
// remove element from deque
point_state_ptr_map_.erase(_remove_id);
//std::cout << "state removed from point vector " << std::endl;
- // reset first_id_ & last_id_
- first_id_ = getFirstId();
- last_id_ = getLastId();
-
// new version
version_++;
-
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
}
-bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes)
+bool LandmarkPolyline2d::tryClassify(const double& _dist_tol, std::vector<PolylineRectangularClass> _classes)
{
//std::cout << "tryClassify landmark " << id() << " with " << getNDefinedPoints() << " defined points of " << getNPoints() << " - tol = " << _dist_tol << std::endl;
+ if (classification_.type != UNCLASSIFIED)
+ {
+ WOLF_WARN("trying to classify a container already classified");
+ return false;
+ }
+
+ if (closed_ && getNDefinedPoints() != 4)
+ {
+ WOLF_WARN("trying to classify a closed polyline of Npoints different than 4");
+ return false;
+ }
+
+ if (getNDefinedPoints() < 3 && getNDefinedPoints() > 4)
+ {
+ WOLF_WARN("wrong number of defined points for classification of rectangular forms");
+ return false;
+ }
+
assert(classification_.type == UNCLASSIFIED && "trying to classify a container already classified");
assert((!closed_ || getNDefinedPoints() == 4) && "trying to classify a closed polyline of Npoints different than 4");
assert(getNDefinedPoints() > 2 && getNDefinedPoints() < 5 && "wrong number of defined points for classification of rectangular forms");
@@ -557,9 +610,9 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
points_ids.pop_back();
// consider 3 first defined points A-0, B-1, C-2 (and D-3)
- Scalar dAB = (getPointVector(points_ids[0]) - getPointVector(points_ids[1])).norm();
- Scalar dBC = (getPointVector(points_ids[1]) - getPointVector(points_ids[2])).norm();
- Scalar dAC = (getPointVector(points_ids[0]) - getPointVector(points_ids[2])).norm();
+ double dAB = (getPointVector(points_ids[0]) - getPointVector(points_ids[1])).norm();
+ double dBC = (getPointVector(points_ids[1]) - getPointVector(points_ids[2])).norm();
+ double dAC = (getPointVector(points_ids[0]) - getPointVector(points_ids[2])).norm();
//std::cout << "dAB = " << dAB << std::endl;
//std::cout << "dBC = " << dBC << std::endl;
@@ -601,9 +654,9 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
{
//std::cout << "checking with 4th point... " << std::endl;
- Scalar dAD = (getPointVector(points_ids[0]) - getPointVector(points_ids[3])).norm();
- Scalar dBD = (getPointVector(points_ids[1]) - getPointVector(points_ids[3])).norm();
- Scalar dCD = (getPointVector(points_ids[2]) - getPointVector(points_ids[3])).norm();
+ double dAD = (getPointVector(points_ids[0]) - getPointVector(points_ids[3])).norm();
+ double dBD = (getPointVector(points_ids[1]) - getPointVector(points_ids[3])).norm();
+ double dCD = (getPointVector(points_ids[2]) - getPointVector(points_ids[3])).norm();
// necessary conditions (rectangular shape) not fulfilled
if (fabs(dAD-dBC) > _dist_tol ||
@@ -632,8 +685,8 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
// Add a point if not enough points
if (getNPoints() < 4)
{
- addPoint(Eigen::Vector2s::Zero(), true, true);
- points_ids.push_back(last_id_);
+ addPoint(Eigen::Vector2d::Zero(), true, true);
+ points_ids.push_back(getLastId());
//std::cout << "ADDING POINT\n";
//std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
}
@@ -647,14 +700,14 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
{
//std::cout << "4 DEFINED POINTS\n";
if (!last_defined_)
- mergePoints(first_id_, getPrevValidId(last_id_));
+ mergePoints(getFirstId(), getPrevValidId(getLastId()));
else
- mergePoints(first_id_, last_id_);
+ mergePoints(getFirstId(), getLastId());
}
else // define
{
defineFirst();
- points_ids.push_back(first_id_);
+ points_ids.push_back(getFirstId());
}
//std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
}
@@ -667,12 +720,12 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
if (getNDefinedPoints() == 4) // merge
{
//std::cout << "4 DEFINED POINTS\n";
- mergePoints(last_id_, first_id_);
+ mergePoints(getLastId(), getFirstId());
}
else // define
{
defineLast();
- points_ids.push_back(last_id_); // it is points_ids[3]
+ points_ids.push_back(getLastId()); // it is points_ids[3]
}
//std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
}
@@ -680,8 +733,6 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
assert(getNDefinedPoints() == 4 && "classified landmark must have 4 defined points");
assert(getNPoints() == 4 && "classified landmark must have 4 points");
assert(points_ids.size() == 4 && "classified landmark must have 4 points");
- assert(getFirstId() == first_id_);
- assert(getLastId() == last_id_);
}
// Close
@@ -693,8 +744,8 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
// Move origin to to the center
getP()->setState((getPointVector(points_ids[0]) + getPointVector(points_ids[2])) / 2.0);
- Eigen::Vector2s frame_x = (configuration ? getPointVector(points_ids[0])-getPointVector(points_ids[1]) : getPointVector(points_ids[2])-getPointVector(points_ids[1]));
- getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
+ Eigen::Vector2d frame_x = (configuration ? getPointVector(points_ids[0])-getPointVector(points_ids[1]) : getPointVector(points_ids[2])-getPointVector(points_ids[1]));
+ getO()->setState(Eigen::Vector1d::Constant(atan2(frame_x(1),frame_x(0))));
// Unfix origin
getP()->unfix();
@@ -710,17 +761,17 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
// Set polyline points to its respective relative positions
if (configuration)
{
- getPointStateBlock(points_ids[0])->setState(Eigen::Vector2s( classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[1])->setState(Eigen::Vector2s(-classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[2])->setState(Eigen::Vector2s(-classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[3])->setState(Eigen::Vector2s( classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[0])->setState(Eigen::Vector2d( classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[1])->setState(Eigen::Vector2d(-classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[2])->setState(Eigen::Vector2d(-classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[3])->setState(Eigen::Vector2d( classification_.L / 2, classification_.W / 2));
}
else
{
- getPointStateBlock(points_ids[0])->setState(Eigen::Vector2s(-classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[1])->setState(Eigen::Vector2s(-classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[2])->setState(Eigen::Vector2s( classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[3])->setState(Eigen::Vector2s( classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[0])->setState(Eigen::Vector2d(-classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[1])->setState(Eigen::Vector2d(-classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[2])->setState(Eigen::Vector2d( classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[3])->setState(Eigen::Vector2d( classification_.L / 2,-classification_.W / 2));
}
for (auto st_pair : point_state_ptr_map_)
@@ -731,7 +782,7 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
return false;
}
-void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, const int& _from, const int& _to, const int& _merged_from, const int& _merged_to)
+void LandmarkPolyline2d::mergeLandmark(const LandmarkPolyline2dPtr _merged_lmk, const int& _from, const int& _to, const int& _merged_from, const int& _merged_to)
{
// merge landmark into this landmark. Assumptions & ideas:
// - if merged landmark is closed, this is also closed.
@@ -754,17 +805,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
// ADD FRONT POINTS
if (_merged_from > _merged_lmk->getFirstId())
{
- assert(_from == first_id_);
+ assert(_from == getFirstId());
for (int merged_id = _merged_lmk->getPrevValidId(_merged_from); ; merged_id = _merged_lmk->getPrevValidId(merged_id))
{
//std::cout << "adding front point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl;
- //std::cout << "Until current first point: " << first_id_ << "\n";
+ //std::cout << "Until current first point: " << getFirstId() << "\n";
// Add point
addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined() ,false);
//std::cout << "added\n";
- //std::cout << "adding correspondence" << merged_id << " with " << first_id_ << "\n";
- merged_id_to_id[merged_id] = first_id_;
+ //std::cout << "adding correspondence" << merged_id << " with " << getFirstId() << "\n";
+ merged_id_to_id[merged_id] = getFirstId();
//std::cout << "added correspondence\n";
// exit loop
@@ -775,17 +826,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
// ADD BACK POINTS
if (_merged_to < _merged_lmk->getLastId())
{
- assert(_to == last_id_);
+ assert(_to == getLastId());
for (int merged_id = _merged_lmk->getNextValidId(_merged_to); ; merged_id = _merged_lmk->getNextValidId(merged_id))
{
//std::cout << "adding back point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl;
- //std::cout << "Until current last point: " << last_id_ << "\n";
+ //std::cout << "Until current last point: " << getLastId() << "\n";
// Add point
addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined() ,true);
//std::cout << "added\n";
- //std::cout << "adding correspondence" << merged_id << " with " << last_id_ << "\n";
- merged_id_to_id[merged_id] = last_id_;
+ //std::cout << "adding correspondence" << merged_id << " with " << getLastId() << "\n";
+ merged_id_to_id[merged_id] = getLastId();
//std::cout << "added correspondence\n";
// exit loop
@@ -804,9 +855,9 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
merged_id_to_id[merged_id] = id;
// Set Defined extremes if it's the case
- if (id == first_id_ && !first_defined_ && (merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined()))
+ if (id == getFirstId() && !first_defined_ && (merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined()))
defineFirst();
- if (id == last_id_ && !last_defined_ && (merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined()))
+ if (id == getLastId() && !last_defined_ && (merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined()))
defineLast();
// exit loop
@@ -821,14 +872,13 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
// ------------------------- COPY FACTORS
FactorBasePtrList old_factors_list = _merged_lmk->getConstrainedByList();
//std::cout << "\tchanging " << old_factors_list.size() << " factors." << std::endl;
- FactorBasePtr new_fac_ptr = nullptr;
for (auto fac_ptr : old_factors_list)
{
//std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOther()->id() << std::endl;
assert(fac_ptr->getLandmarkOther() == _merged_lmk);
- FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
- FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
+ auto fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2d>(fac_ptr);
+ auto fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2d>(fac_ptr);
// point 2 point
if (fac_point_ptr)
@@ -836,13 +886,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
//std::cout << "\t\tpoint-point factor to id: " << fac_point_ptr->getLandmarkPointId() << std::endl;
assert(merged_id_to_id.find(fac_point_ptr->getLandmarkPointId()) != merged_id_to_id.end());
- new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_ptr->getProcessor(),
- fac_point_ptr->getFeaturePointId(),
- merged_id_to_id[fac_point_ptr->getLandmarkPointId()],
- fac_point_ptr->getApplyLossFunction(),
- fac_point_ptr->getStatus());
+ // emplace new factor
+ FactorBase::emplace<FactorPoint2d>(fac_ptr->getFeature(),
+ std::static_pointer_cast<FeaturePolyline2d>(fac_ptr->getFeature()),
+ std::static_pointer_cast<LandmarkPolyline2d>(shared_from_this()),
+ fac_point_ptr->getProcessor(),
+ fac_point_ptr->getFeaturePointId(),
+ merged_id_to_id[fac_point_ptr->getLandmarkPointId()],
+ fac_point_ptr->getApplyLossFunction(),
+ fac_point_ptr->getStatus());
+ // remove old factor
+ fac_ptr->remove();
}
// point 2 line
@@ -852,14 +906,18 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointId()) != merged_id_to_id.end());
assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointAuxId()) != merged_id_to_id.end());
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- merged_id_to_id[fac_point_line_ptr->getLandmarkPointId()],
- merged_id_to_id[fac_point_line_ptr->getLandmarkPointAuxId()],
- fac_point_line_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
+ // emplace new factor
+ FactorBase::emplace<FactorPointToLine2d>(fac_ptr->getFeature(),
+ std::static_pointer_cast<FeaturePolyline2d>(fac_ptr->getFeature()),
+ std::static_pointer_cast<LandmarkPolyline2d>(shared_from_this()),
+ fac_point_line_ptr->getProcessor(),
+ fac_point_line_ptr->getFeaturePointId(),
+ merged_id_to_id[fac_point_line_ptr->getLandmarkPointId()],
+ merged_id_to_id[fac_point_line_ptr->getLandmarkPointAuxId()],
+ fac_point_line_ptr->getApplyLossFunction(),
+ fac_point_line_ptr->getStatus());
+ // remove old factor
+ fac_ptr->remove();
}
else
@@ -867,71 +925,59 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
WOLF_ERROR("polyline factor of unknown type");
throw std::runtime_error ("polyline factor of unknown type");
}
- // Establish new factor
- assert(new_fac_ptr != nullptr);
- //std::cout << "\t\tcreated new factor: " << new_fac_ptr->id() << std::endl;
- //std::cout << "\t\tdeleting factor: " << fac_ptr->id() << std::endl;
-
- // add new factor
- fac_ptr->getFeature()->addFactor(new_fac_ptr);
- addConstrainedBy(new_fac_ptr);
-
- // remove factor
- fac_ptr->remove();
- //std::cout << "\t\tdeleted\n";
}
// ------------------------- REMOVE _merged_lmk
- _merged_lmk->setMergedInLandmark(std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()));
+ _merged_lmk->setMergedInLandmark(std::static_pointer_cast<LandmarkPolyline2d>(shared_from_this()));
_merged_lmk->remove();
//std::cout << "\tLandmark deleted\n";
}
-void LandmarkPolyline2D::registerNewStateBlocks()
+void LandmarkPolyline2d::registerNewStateBlocks() const
{
- LandmarkBase::registerNewStateBlocks();
+ LandmarkBase::registerNewStateBlocks(getProblem());
if (getProblem())
for (auto state_it : point_state_ptr_map_)
- getProblem()->addStateBlock(state_it.second);
+ getProblem()->notifyStateBlock(state_it.second, ADD);
}
-void LandmarkPolyline2D::removeStateBlocks()
+void LandmarkPolyline2d::removeStateBlocks()
{
auto sbp_pair = point_state_ptr_map_.begin();
while (sbp_pair != point_state_ptr_map_.end())
{
if (getProblem())
- getProblem()->removeStateBlock(sbp_pair->second);
+ getProblem()->notifyStateBlock(sbp_pair->second, REMOVE);
sbp_pair = point_state_ptr_map_.erase(sbp_pair);
}
- LandmarkBase::removeStateBlocks();
+ LandmarkBase::removeStateBlocks(getProblem());
}
// 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>();
- Eigen::VectorXs pos = _lmk_node["position"].as<Eigen::VectorXs>();
+ Eigen::VectorXd pos = _lmk_node["position"].as<Eigen::VectorXd>();
bool pos_fixed = true;//_lmk_node["position fixed"].as<bool>();
- Eigen::VectorXs ori = _lmk_node["orientation"].as<Eigen::VectorXs>();
+ Eigen::VectorXd ori = _lmk_node["orientation"].as<Eigen::VectorXd>();
bool ori_fixed = true;//_lmk_node["orientation fixed"].as<bool>();
int first_id = _lmk_node["first_id"].as<int>();
bool first_defined = _lmk_node["first_defined"].as<bool>();
bool last_defined = _lmk_node["last_defined"].as<bool>();
unsigned int npoints = _lmk_node["points"].size();
int classif_type = _lmk_node["classification_type"].as<int>();
- Scalar classif_L = _lmk_node["classification_L"].as<Scalar>();
- Scalar classif_W = _lmk_node["classification_W"].as<Scalar>();
+ double classif_L = _lmk_node["classification_L"].as<double>();
+ double classif_W = _lmk_node["classification_W"].as<double>();
// load points
- 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
- LandmarkPolyline2DPtr lmk_ptr = std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(pos, pos_fixed),
+ LandmarkPolyline2dPtr lmk_ptr = std::make_shared<LandmarkPolyline2d>(std::make_shared<StateBlock>(pos, pos_fixed),
std::make_shared<StateBlock>(ori, ori_fixed),
points,
first_defined,
@@ -949,13 +995,13 @@ LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
return lmk_ptr;
}
-YAML::Node LandmarkPolyline2D::saveToYaml() const
+YAML::Node LandmarkPolyline2d::saveToYaml() const
{
// First base things
YAML::Node node = LandmarkBase::saveToYaml();
// Then add specific things
- node["first_id"] = first_id_;
+ node["first_id"] = getFirstId();
node["first_defined"] = first_defined_;
node["last_defined"] = last_defined_;
node["classification_type"] = static_cast<int>(classification_.type);
@@ -968,15 +1014,19 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const
return node;
}
-void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol)
+void LandmarkPolyline2d::tryMergeLandmarks(LandmarkPolyline2dPtrList& _lmk_list,
+ const double& max_sq_error,
+ const double& min_overlapping_dist,
+ const int& min_N_overlapped,
+ const int& min_N_defined_overlapped)
{
- std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n";
+ //std::cout << "LandmarkPolyline2d::tryMergeLandmarks\n";
assert(_lmk_list.size() >= 2);
// 2 by 2
- LandmarkPolyline2DPtr remaining_lmk = _lmk_list.front();
+ LandmarkPolyline2dPtr remaining_lmk = _lmk_list.front();
_lmk_list.pop_front();
- LandmarkPolyline2DPtr lmk_1, lmk_2, merged_lmk;
+ LandmarkPolyline2dPtr lmk_1, lmk_2, merged_lmk;
int remaining_from, remaining_to, merged_from, merged_to;
while (!_lmk_list.empty())
@@ -988,13 +1038,17 @@ void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list,
if (lmk_2->isRemoving())
continue;
- std::cout << "Candidates to be merged: " << lmk_1->id() << " " << lmk_2->id() << "\n";
+ //std::cout << "Candidates to be merged: " << lmk_1->id() << " " << lmk_2->id() << "\n";
// check best correspondence
- Eigen::MatrixXs points_1 = lmk_1->computePointsGlobal();
- Eigen::MatrixXs points_2 = lmk_2->computePointsGlobal();
- MatchPolyline2DPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(),
- points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(), _sq_dist_tol);
+ Eigen::MatrixXd points_1 = lmk_1->computePointsGlobal();
+ Eigen::MatrixXd points_2 = lmk_2->computePointsGlobal();
+ MatchPolyline2dPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(),
+ points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(),
+ max_sq_error,
+ min_overlapping_dist,
+ min_N_overlapped,
+ min_N_defined_overlapped);
// match
if ( match )
@@ -1032,19 +1086,19 @@ void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list,
lmk_1.reset();
lmk_2.reset();
assert(merged_lmk->getHits() == 0);
- std::cout << "merged\n";
+ //std::cout << "merged\n";
}
// no match (remaining: most factors)
else
remaining_lmk = (lmk_1->getHits() > lmk_2->getHits() ? lmk_1 : lmk_2);
}
- std::cout << "tryMergeLandmarks::done\n";
+ //std::cout << "tryMergeLandmarks::done\n";
}
// Register landmark creator
namespace
{
-const bool WOLF_UNUSED registered_lmk_polyline_2D = LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
+const bool WOLF_UNUSED registered_lmk_polyline_2d = FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
}
} /* namespace wolf */
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2d_utils.cpp
similarity index 52%
rename from src/processor/polyline_2D_utils.cpp
rename to src/processor/polyline_2d_utils.cpp
index 8198e4326fef9fef243acfa07fec27280ee84c4a..d935ec63d985d40d5c3a2729256a9b7e3c5e90d4 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2d_utils.cpp
@@ -1,67 +1,88 @@
-#include "laser/processor/polyline_2D_utils.h"
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/processor/polyline_2d_utils.h"
namespace wolf {
-Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta)
+Eigen::Matrix3d pose2T2d(const Eigen::Vector2d& t, const double& theta)
{
- Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
+ Eigen::Matrix3d T(Eigen::Matrix3d::Identity());
- T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(theta).matrix();
+ T.topLeftCorner(2,2) = Eigen::Rotation2D<double>(theta).matrix();
T.topRightCorner(2,1) = t;
return T;
}
-Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose)
+Eigen::Matrix3d pose2T2d(const Eigen::Vector3d& pose)
{
- Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
+ Eigen::Matrix3d T(Eigen::Matrix3d::Identity());
- T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(pose(2)).matrix();
+ T.topLeftCorner(2,2) = Eigen::Rotation2D<double>(pose(2)).matrix();
T.topRightCorner(2,1) = pose.head(2);
return T;
}
-Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T)
+Eigen::Vector3d T2pose2d(const Eigen::Matrix3d& T)
{
- Eigen::Vector3s pose;
+ Eigen::Vector3d pose;
pose.head(2) = T.topRightCorner(2,1);
pose(2) = atan2(T(1,0),T(0,0));
return pose;
}
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B, const bool& first_defined, const bool& last_defined)
+Eigen::Vector3d computeRigidTrans(const Eigen::MatrixXd& _points_A, const Eigen::MatrixXd& _points_B, const bool& first_defined, const bool& last_defined)
{
assert(_points_A.cols() == _points_B.cols());
assert(_points_A.rows() >= 2);
assert(_points_B.rows() >= 2);
-
int N_defined_points = _points_A.cols() - (first_defined ? 0 : 1) - (last_defined ? 0 : 1);
bool all_defined = first_defined && last_defined;
- Eigen::Vector3s t_A_B;
+ Eigen::Vector3d t_A_B;
assert(N_defined_points > 0 && _points_A.cols() > 1);
// Rotation
- Eigen::VectorXs angles_A_B(Eigen::VectorXs::Zero(_points_A.cols()));
- Eigen::Vector2s points_A_mean((all_defined ? _points_A.row(0).mean() : _points_A.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
+ Eigen::VectorXd angles_A_B(Eigen::VectorXd::Zero(_points_A.cols()));
+ Eigen::Vector2d points_A_mean((all_defined ? _points_A.row(0).mean() : _points_A.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
(all_defined ? _points_A.row(1).mean() : _points_A.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
- Eigen::Vector2s points_B_mean((all_defined ? _points_B.row(0).mean() : _points_B.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
+ Eigen::Vector2d points_B_mean((all_defined ? _points_B.row(0).mean() : _points_B.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
(all_defined ? _points_B.row(1).mean() : _points_B.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
- Eigen::MatrixXs points_A_centered = _points_A.topRows<2>().colwise()-points_A_mean;
- Eigen::MatrixXs points_B_centered = _points_B.topRows<2>().colwise()-points_B_mean;
+ Eigen::MatrixXd points_A_centered = _points_A.topRows<2>().colwise()-points_A_mean;
+ Eigen::MatrixXd points_B_centered = _points_B.topRows<2>().colwise()-points_B_mean;
for (auto i = 0; i < _points_A.cols(); i++ )
angles_A_B(i) = pi2pi(atan2(points_A_centered(1,i),points_A_centered(0,i)) - atan2(points_B_centered(1,i),points_B_centered(0,i)));
- // fix 2nd&3rd quadrant angles bad resulting mean
+ // fix 2nd&3rd quadrant angles bad resulting mean:
+ // if simultaneously in 2nd and 3rd quadrants, add 2PI in 3rd and 4th
if (angles_A_B.maxCoeff() > M_PI / 2 && angles_A_B.minCoeff() < -M_PI / 2)
angles_A_B = (angles_A_B.array() >= 0).select(angles_A_B, angles_A_B.array()+2*M_PI);
- // if only one defined point, mean of not defined points angles (mean is pivot)
+ // if only one defined point, mean of not defined points angles (mean point is pivot, atan2 is not defined)
if (N_defined_points == 1)
- t_A_B(2) = ((first_defined ? 0 : angles_A_B(0)) + (last_defined ? 0 : angles_A_B(_points_A.cols()-1))) / ((first_defined ? 0 : 1) + (last_defined ? 0 : 1));
+ t_A_B(2) = ((first_defined ? 0 : angles_A_B(0)) + (last_defined ? 0 : angles_A_B(_points_A.cols()-1))) / (_points_A.cols() - 1);
// use all points if all defined
else if (all_defined)
t_A_B(2) = angles_A_B.mean();
@@ -70,19 +91,19 @@ Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen:
t_A_B(2) = angles_A_B.segment(first_defined ? 0 : 1, N_defined_points).mean();
// Translation
- t_A_B.head<2>() = points_A_mean - Eigen::Rotation2D<Scalar>(t_A_B(2))*points_B_mean;
+ t_A_B.head<2>() = points_A_mean - Eigen::Rotation2D<double>(t_A_B(2))*points_B_mean;
return t_A_B;
}
/** \brief Computes the squared distance from a point to a segment defined by two points
**/
-Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b)
+double sqDistPoint2Segment(const Eigen::Vector2d& p, const Eigen::Vector2d& a, const Eigen::Vector2d& b)
{
- Scalar ap_sq = (p-a).squaredNorm();
- Scalar bp_sq = (p-b).squaredNorm();
- Scalar ab_sq = (b-a).squaredNorm();
- Scalar sqDist;
+ double ap_sq = (p-a).squaredNorm();
+ double bp_sq = (p-b).squaredNorm();
+ double ab_sq = (b-a).squaredNorm();
+ double sqDist;
// If the projection of p onto ab is inside the segment (acute triangle: all angles < 90º), return the sq distance point-line.
if (ap_sq <= bp_sq + ab_sq && bp_sq <= ap_sq + ab_sq) // acute triangle: applying pitagoras twice, angles ABP and PAB <= 90º
@@ -95,8 +116,8 @@ Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, c
return sqDist;
}
-Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict)
+double sqDistPointToLine(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux,
+ const Eigen::Vector3d& _B, bool _A_defined, bool _B_defined, bool strict)
{
/* Squared distance from B to the line A_aux-A (match evaluated is A-B)
*
@@ -129,9 +150,9 @@ Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_au
assert((!_A_defined || !_B_defined) && "ProcessorPolyline::sqDistPointToLine: at least one point must not be defined.");
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
+ double AB_sq = (_B-_A).head<2>().squaredNorm();
+ double AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
+ double AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
// squared distance to line
// Case 1
@@ -165,15 +186,30 @@ Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_au
return 1e12;
}
-MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error )
+MatchPolyline2dPtr computeBestSqDist(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& sq_error_max,
+ const double& overlap_dist_min,
+ const int& overlap_N_min,
+ const int& overlap_N_defined_min,
+ bool both_exceeding_A_allowed_,
+ bool both_exceeding_B_allowed_)
{
- //std::cout << "computeBestSqDist...\n";
- //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "OPEN") << " has "<< _points_A.cols() << " points\n";
- //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "OPEN") << " has "<< _points_B.cols() << " points\n";
- //std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
- //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
+ bool print = false;
+ if (print)
+ {
+ std::cout << "computeBestSqDist...\n";
+ std::cout << "\tA is " << (_closed_A ? "CLOSED" : "OPEN") << " has "<< _points_A.cols() << " points\n";
+ std::cout << "\tB is " << (_closed_B ? "CLOSED" : "OPEN") << " has "<< _points_B.cols() << " points\n";
+ std::cout << "\tA extremes defined: " << (_first_defined_A ? "1 " : "0 ") << (_last_defined_A ? "1" : "0") << std::endl;
+ std::cout << "\tB extremes defined: " << (_first_defined_B ? "1 " : "0 ") << (_last_defined_B ? "1" : "0") << std::endl;
+ std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
+ std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
+ std::cout << "\tsq_error_max: " << sq_error_max << std::endl;
+ std::cout << "\toverlap_dist_min: " << overlap_dist_min << std::endl;
+ std::cout << "\toverlap_N_min: " << overlap_N_min << std::endl;
+ std::cout << "\toverlap_N_defined_min: " << overlap_N_defined_min << std::endl;
+ }
// ASSERTIONS if closed -> all points defined
assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
@@ -192,7 +228,7 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
* (d) - B.(defined_)points == A.(defined_)points --> Otherwise: No matching
*/
- MatchPolyline2DPtr best_match = nullptr;
+ MatchPolyline2dPtr best_match = nullptr;
// ----------------- Call switching A<->B
if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
@@ -200,8 +236,14 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
{
//std::cout << "Auto-calling switching A<->B...\n";
- MatchPolyline2DPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
- _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
+ MatchPolyline2dPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
+ _points_A, _first_defined_A, _last_defined_A, _closed_A,
+ sq_error_max,
+ overlap_dist_min,
+ overlap_N_min,
+ overlap_N_defined_min,
+ both_exceeding_B_allowed_,
+ both_exceeding_A_allowed_);
// undo switching
if (best_match != nullptr)
{
@@ -226,7 +268,7 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
* 2. A closed: [-last_A, 0]
*/
- int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlapped;
+ int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlap;
last_A = _points_A.cols() - 1;
last_B = _points_B.cols() - 1;
@@ -248,44 +290,97 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
from_B = std::max(0, offset);
if (!_closed_A && !_closed_B)
- N_overlapped = std::min(last_B - from_B, last_A - from_A)+1;
+ N_overlap = std::min(last_B - from_B, last_A - from_A)+1;
else if(_closed_A)
- N_overlapped = _points_B.cols();
+ N_overlap = _points_B.cols();
else
std::runtime_error("Impossible state. Function should auto-called reversing A&B");
- to_A = from_A+N_overlapped-1;
- to_B = from_B+N_overlapped-1;
+ to_A = from_A+N_overlap-1;
+ to_B = from_B+N_overlap-1;
while (to_A > last_A && _closed_A)
to_A -= _points_A.cols();
- //std::cout << "\toffset " << offset << std::endl;
- //std::cout << "\t\tfrom_A " << from_A << std::endl;
- //std::cout << "\t\tto_A " << to_A << std::endl;
- //std::cout << "\t\tfrom_B " << from_B << std::endl;
- //std::cout << "\t\tto_B " << to_B << std::endl;
- //std::cout << "\t\tlast_A " << last_A << std::endl;
- //std::cout << "\t\tlast_B " << last_B << std::endl;
- //std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
+ // exceeding points for both extremes allowed?
+ if (!both_exceeding_A_allowed_ && from_A != 0 && to_A != last_A)
+ continue;
+ if (!both_exceeding_B_allowed_ && from_B != 0 && to_B != last_B)
+ continue;
- bool from_A_defined = ((from_A != 0 && from_A != last_A) || _first_defined_A);
- bool from_B_defined = ((from_B != 0 && from_B != last_B) || _first_defined_B);
- bool to_A_defined = ((to_A != 0 && to_A != last_A) || _last_defined_A);
- bool to_B_defined = ((to_B != 0 && to_B != last_B) || _last_defined_B);
+ if (print)
+ {
+ std::cout << "\toffset " << offset << std::endl;
+ std::cout << "\t\tfrom_A " << from_A << std::endl;
+ std::cout << "\t\tto_A " << to_A << std::endl;
+ std::cout << "\t\tfrom_B " << from_B << std::endl;
+ std::cout << "\t\tto_B " << to_B << std::endl;
+ std::cout << "\t\tlast_A " << last_A << std::endl;
+ std::cout << "\t\tlast_B " << last_B << std::endl;
+ std::cout << "\t\tN_overlap " << N_overlap << std::endl;
+ std::cout << "\t\tfirst_defined_A " << _first_defined_A << std::endl;
+ std::cout << "\t\tfirst_defined_B " << _first_defined_B << std::endl;
+ std::cout << "\t\tlast_defined_A " << _last_defined_A << std::endl;
+ std::cout << "\t\tlast_defined_B " << _last_defined_B << std::endl;
+ }
- //std::cout << "\t\tfrom_A_defined " << from_A_defined << (from_A == 0) << (from_A == last_A) << _first_defined_A << std::endl;
- //std::cout << "\t\tfrom_B_defined " << from_B_defined << (from_B == 0) << (from_B == last_B) << _first_defined_B << std::endl;
- //std::cout << "\t\tto_A_defined " << to_A_defined << (to_A == 0) << (to_A == last_A) << _last_defined_A << std::endl;
- //std::cout << "\t\tto_B_defined " << to_B_defined << (to_B == 0) << (to_B == last_B) << _last_defined_B << std::endl;
+ bool from_A_defined = (from_A != 0 || _first_defined_A) && (from_A != last_A || _last_defined_A); //((from_A != 0 && from_A != last_A) || _first_defined_A);
+ bool from_B_defined = (from_B != 0 || _first_defined_B) && (from_B != last_B || _last_defined_B); //((from_B != 0 && from_B != last_B) || _first_defined_B);
+ bool to_A_defined = (to_A != 0 || _first_defined_A) && (to_A != last_A || _last_defined_A); //((to_A != 0 && to_A != last_A) || _last_defined_A);
+ bool to_B_defined = (to_B != 0 || _first_defined_B) && (to_B != last_B || _last_defined_B); //((to_B != 0 && to_B != last_B) || _last_defined_B);
+
+ // SUFFICIENT CONDITIONS
+ // COND 1: sufficient overlapping points
+ // COND 2: sufficient overlapping defined points
+ // COND 3: sufficient overlapping distance
+ int N_defined_overlap = std::max(0,N_overlap - (from_A_defined && from_B_defined ? 0 : 1) - (to_A_defined && to_B_defined ? 0 : 1));
+ double overlap_dist;
+ if (N_overlap <= 1)
+ overlap_dist = 0;
+ else
+ {
+ Eigen::Vector2d p_from_A = _points_A.col(from_A).head(2);
+ Eigen::Vector2d p_to_A = _points_A.col(to_A).head(2);
+ Eigen::Vector2d p_from_B = _points_B.col(from_B).head(2);
+ Eigen::Vector2d p_to_B = _points_B.col(to_B).head(2);
+ Eigen::Vector2d vA = p_to_A - p_from_A;
+ Eigen::Vector2d vB = p_to_B - p_from_B;
+
+ // THE MINIMUM OF ALL 6 possible overlap distances
+ Eigen::Vector6d overlap_dists;
+ // 1: fromA - toA
+ overlap_dists(0) = vA.norm();
+ // 2: fromB - toB
+ overlap_dists(1) = vB.norm();
+ // 3: fromA - projected(toB)
+ overlap_dists(2) = (vA.normalized()).dot(p_to_B - p_from_A);
+ // 4: fromB - projected(toA)
+ overlap_dists(3) = (vB.normalized()).dot(p_to_A - p_from_B);
+ // 5: projected(fromB) - toA
+ overlap_dists(4) = (-vA.normalized()).dot(p_from_B - p_to_A);
+ // 6: projected(fromA) - toB
+ overlap_dists(5) = (-vB.normalized()).dot(p_from_A - p_to_B);
+
+ //std::cout << "\tdist2.mean() = " << dist2.mean() << std::endl;
+ //std::cout << "\tmax_sq_error - dist2.mean() = " << max_sq_error - dist2.mean() << std::endl;
+ if (print)
+ std::cout << "\toverlap_dists = " << overlap_dists.transpose() << std::endl;
+ overlap_dist = overlap_dists.minCoeff();
+ }
- // If only one overlapped point, both should be defined
- if ( N_overlapped== 1 &&
- (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
+ // if none of the sufficient conditions hold, continue
+ if ( N_overlap < overlap_N_min &&
+ N_defined_overlap < overlap_N_defined_min &&
+ N_defined_overlap <= 0 && N_overlap < 2 &&
+ overlap_dist < overlap_dist_min)
+ {
+ if (print)
+ std::cout << "\t\tnone of the sufficient conditions hold\n";
continue;
+ }
- assert(N_overlapped <= _points_B.cols());
- assert(N_overlapped <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlapped can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
+ assert(N_overlap <= _points_B.cols());
+ assert(N_overlap <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlap can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
assert(from_A >= 0);
assert(from_B >= 0);
assert(from_B == 0 || !_closed_A);
@@ -295,88 +390,115 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
assert(to_A < _points_A.cols());
assert(to_B < _points_B.cols());
- // POINT-POINT DISTANCES for all overlapped points (in case of closed A in two parts)
- // Fill with B points
- Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlapped).array());
+ // NECESSARY COND: All squared distances should be lower than a threshold
+ // squared distances for all overlapping points
+ Eigen::Array<double,Eigen::Dynamic,1> dist2 = computeSqDist(_points_A, from_A, to_A, from_A_defined, to_A_defined,
+ _points_B, from_B, to_B, from_B_defined, to_B_defined,
+ last_A, N_overlap );
+ if (print)
+ std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
- // Substract A points
- if (to_A >= from_A && N_overlapped <= _points_A.cols())
- d -= _points_A.block(0,from_A, 2, N_overlapped).array();
- else
+ // if the necessary condition does not hold, continue
+ if ((dist2 > sq_error_max).any())
{
- d.leftCols(_points_A.cols()-from_A) -= _points_A.block(0,from_A, 2, _points_A.cols()-from_A).array();
- d.rightCols(to_A+1) -= _points_A.block(0, 0, 2, to_A+1).array();
+ if (print)
+ std::cout << "\t\ttoo much error\n";
+ continue;
}
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
- // CORRECT POINT-LINE DISTANCES for not defined extremes
- // First
- if (!from_B_defined || !from_A_defined)
+ // COMPUTE SCORE (distance based)
+ double normalized_score = (sq_error_max - dist2.mean()) / sq_error_max;
+ if (best_match != nullptr && print)
{
- // take care of closed A: next of from_A
- int next_from_A = (from_A+1 > last_A ? 0 : from_A+1);
- //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
-
- // std::cout << "\t\t\tA " << _points_A.col(from_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(next_from_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(from_B).transpose() << std::endl;
- dist2(0) = sqDistPointToLine(_points_A.col(from_A),
- _points_A.col(next_from_A),
- _points_B.col(from_B),
- from_A_defined,
- from_B_defined);
- assert(N_overlapped > 1);
+ std::cout << "\tnormalized_score = " << normalized_score << std::endl;
+ std::cout << "\tbest_match->normalized_score_ = " << best_match->normalized_score_ << std::endl;
}
- // Last
- if (!to_B_defined || !to_A_defined)
+
+ // BEST MATCH
+ if (best_match == nullptr || normalized_score > best_match->normalized_score_ )
{
- // take care of closed A: next of to_A
- int prev_to_A = (to_A == 0 ? last_A : to_A-1);
- //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
+ if (print)
+ std::cout << "\tBEST MATCH normalized_score = " << normalized_score << std::endl;
- // std::cout << "\t\t\tA " << _points_A.col(to_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(to_B).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(_points_A.col(to_A),
- _points_A.col(prev_to_A),
- _points_B.col(to_B),
- to_A_defined,
- to_B_defined);
- assert(N_overlapped > 1);
- }
- //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+ if (best_match == nullptr)
+ best_match = std::make_shared<MatchPolyline2d>();
- // All squared distances should be within a threshold
- if ((dist2 < max_sq_error).all())
- {
- Scalar normalized_score = (max_sq_error - dist2.mean()) / max_sq_error;
-
- // Choose the most overlapped match, if equally overlapped: the best score
- if (best_match == nullptr ||
- N_overlapped > best_match->to_A_id_-best_match->from_A_id_+1 ||
- (N_overlapped == best_match->to_A_id_-best_match->from_A_id_+1 && normalized_score > best_match->normalized_score_ ))
- {
- //std::cout << "\tBEST MATCH" << std::endl;
- if (best_match == nullptr)
- best_match = std::make_shared<MatchPolyline2D>();
-
- best_match->from_A_id_= from_A;
- best_match->to_A_id_= to_A;
- best_match->from_B_id_= from_B;
- best_match->to_B_id_= to_B;
- best_match->normalized_score_ = normalized_score;
- }
+ best_match->from_A_id_= from_A;
+ best_match->to_A_id_= to_A;
+ best_match->from_B_id_= from_B;
+ best_match->to_B_id_= to_B;
+ best_match->normalized_score_ = normalized_score;
+ best_match->overlap_dist_ = overlap_dist;
+
+ if (print)
+ best_match->print();
}
}
return best_match;
}
-MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error )
+Eigen::Array<double,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXd& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
+ const Eigen::MatrixXd& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
+ const int& last_A, const int& N_overlap )
{
- std::cout << "computeBestRigidTrans...\n";
+ // POINT-POINT DISTANCES for all overlap points (in case of closed A in two parts)
+ // Fill with B points
+ Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlap).array());
+
+ // Substract A points
+ if (to_A >= from_A && N_overlap <= _points_A.cols())
+ d -= _points_A.block(0,from_A, 2, N_overlap).array();
+ else
+ {
+ d.leftCols(_points_A.cols()-from_A) -= _points_A.block(0,from_A, 2, _points_A.cols()-from_A).array();
+ d.rightCols(to_A+1) -= _points_A.block(0, 0, 2, to_A+1).array();
+ }
+ Eigen::Array<double,Eigen::Dynamic,1> dist2 = d.row(0).pow(2) + d.row(1).pow(2);
+
+ // CORRECT POINT-LINE DISTANCES for not defined extremes
+ // First
+ if (!from_B_defined || !from_A_defined)
+ {
+ // take care of closed A: next of from_A
+ int next_from_A = (from_A+1 > last_A ? 0 : from_A+1);
+ //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
+
+ // std::cout << "\t\t\tA " << _points_A.col(from_A).transpose() << std::endl;
+ // std::cout << "\t\t\tAaux" << _points_A.col(next_from_A).transpose() << std::endl;
+ // std::cout << "\t\t\tB " << _points_B.col(from_B).transpose() << std::endl;
+ dist2(0) = sqDistPointToLine(_points_A.col(from_A),
+ _points_A.col(next_from_A),
+ _points_B.col(from_B),
+ from_A_defined,
+ from_B_defined);
+ }
+ // Last
+ if (!to_B_defined || !to_A_defined)
+ {
+ // take care of closed A: next of to_A
+ int prev_to_A = (to_A == 0 ? last_A : to_A-1);
+ //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
+
+ // std::cout << "\t\t\tA " << _points_A.col(to_A).transpose() << std::endl;
+ // std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
+ // std::cout << "\t\t\tB " << _points_B.col(to_B).transpose() << std::endl;
+ dist2(N_overlap-1) = sqDistPointToLine(_points_A.col(to_A),
+ _points_A.col(prev_to_A),
+ _points_B.col(to_B),
+ to_A_defined,
+ to_B_defined);
+ }
+ //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+
+ return dist2;
+}
+
+MatchPolyline2dList computeBestRigidTrans(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& max_sq_error )
+{
+ //std::cout << "computeBestRigidTrans...\n";
// ASSERTIONS if closed -> all points defined
assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
@@ -395,7 +517,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
* (d) - B.(defined_)points == A.(defined_)points --> Otherwise: No matching
*/
- MatchPolyline2DList matches;
+ MatchPolyline2dList matches;
auto N_defined_points_A = _points_A.cols() - (_first_defined_A ? 0 : 1) - (_last_defined_A ? 0 : 1);
auto N_defined_points_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
@@ -403,10 +525,10 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
if (N_defined_points_A <= 1 || N_defined_points_B <= 1 )
return matches;
- std::cout << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points (" << N_defined_points_A << " defined)\n";
- std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " has "<< _points_B.cols() << " points (" << N_defined_points_B << " defined)\n";
- std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
- std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
+ //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points (" << N_defined_points_A << " defined)\n";
+ //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " has "<< _points_B.cols() << " points (" << N_defined_points_B << " defined)\n";
+ //std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
+ //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
// ----------------- Call switching A<->B
if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
@@ -441,7 +563,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
* 2. A closed: [-last_A, 0]
*/
- int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlapped;
+ int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlap(0);
last_A = _points_A.cols() - 1;
last_B = _points_B.cols() - 1;
@@ -463,14 +585,14 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
from_B = std::max(0, offset);
if (!_closed_A && !_closed_B)
- N_overlapped = std::min(last_B - from_B, last_A - from_A)+1;
+ N_overlap = std::min(last_B - from_B, last_A - from_A)+1;
else if(_closed_A)
- N_overlapped = _points_B.cols();
+ N_overlap = _points_B.cols();
else
std::runtime_error("Impossible state. Function should auto-called reversing A&B");
- to_A = from_A+N_overlapped-1;
- to_B = from_B+N_overlapped-1;
+ to_A = from_A+N_overlap-1;
+ to_B = from_B+N_overlap-1;
while (to_A > last_A && _closed_A)
to_A -= _points_A.cols();
@@ -482,7 +604,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
//std::cout << "\t\tto_B " << to_B << std::endl;
//std::cout << "\t\tlast_A " << last_A << std::endl;
//std::cout << "\t\tlast_B " << last_B << std::endl;
- //std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
+ //std::cout << "\t\tN_overlap " << N_overlap << std::endl;
bool from_A_defined = ((from_A != 0 && from_A != last_A) || _first_defined_A);
bool from_B_defined = ((from_B != 0 && from_B != last_B) || _first_defined_B);
@@ -494,16 +616,16 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
//std::cout << "\t\tto_A_defined " << to_A_defined << (to_A == 0) << (to_A == last_A) << _last_defined_A << std::endl;
//std::cout << "\t\tto_B_defined " << to_B_defined << (to_B == 0) << (to_B == last_B) << _last_defined_B << std::endl;
- // If only one overlapped point, no rigid transformation possible
- if ( N_overlapped == 1 )
+ // If only one overlap point, no rigid transformation possible
+ if ( N_overlap == 1 )
continue;
- // If only two overlapped points, all should be defined
- if ( N_overlapped == 2 &&
+ // If only two overlap points, all should be defined
+ if ( N_overlap == 2 &&
(!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
continue;
- assert(N_overlapped <= _points_B.cols());
- assert(N_overlapped <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlapped can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
+ assert(N_overlap <= _points_B.cols());
+ assert(N_overlap <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlap can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
assert(from_A >= 0);
assert(from_B >= 0);
assert(from_B == 0 || !_closed_A);
@@ -518,11 +640,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
int from_B_def = from_B_defined ? from_B : from_B+1;
int from_A_def = from_A_defined ? from_A : from_A+1;
int to_A_def = to_A_defined ? to_A : to_A+1;
- int N_overlapped_def = N_overlapped - (from_B_defined && from_A_defined ? 0 : 1) - (to_B_defined && to_A_defined ? 0 : 1);
- Eigen::MatrixXs points_B_def = _points_B.middleCols(from_B_def, N_overlapped_def);
- Eigen::MatrixXs points_A_def(_points_A.rows(), N_overlapped_def);
+ int N_overlap_def = N_overlap - (from_B_defined && from_A_defined ? 0 : 1) - (to_B_defined && to_A_defined ? 0 : 1);
+ Eigen::MatrixXd points_B_def = _points_B.middleCols(from_B_def, N_overlap_def);
+ Eigen::MatrixXd points_A_def(_points_A.rows(), N_overlap_def);
if (to_A >= from_A)
- points_A_def = _points_A.middleCols(from_A_def, N_overlapped_def);
+ points_A_def = _points_A.middleCols(from_A_def, N_overlap_def);
else
{
points_A_def.leftCols(_points_A.cols()-from_A_def) = _points_A.middleCols(from_A_def, _points_A.cols()-from_A_def);
@@ -531,13 +653,13 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
//std::cout << "\tpoints filled...\n";
//std::cout << "\tcomputeRigidTrans\n";
- Eigen::Vector3s pose_A_B = computeRigidTrans(points_A_def, points_B_def);
- Eigen::Matrix3s T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2));
+ Eigen::Vector3d pose_A_B = computeRigidTrans(points_A_def, points_B_def);
+ Eigen::Matrix3d T_A_B = pose2T2d(pose_A_B.head(2), pose_A_B(2));
// POINT-POINT DISTANCES of the matching of defined points
- Eigen::MatrixXs d = (points_A_def - T_A_B * points_B_def).topRows(2);
- Eigen::ArrayXd dist2(N_overlapped);
- dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlapped_def) = d.colwise().squaredNorm().transpose().array();
+ Eigen::MatrixXd d = (points_A_def - T_A_B * points_B_def).topRows(2);
+ Eigen::ArrayXd dist2(N_overlap);
+ dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlap_def) = d.colwise().squaredNorm().transpose().array();
// POINT-LINE DISTANCES for not defined extremes
//std::cout << "\tPOINT-LINE DISTANCES\n";
@@ -556,7 +678,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
T_A_B * _points_B.col(from_B),
from_A_defined,
from_B_defined);
- assert(N_overlapped > 1);
+ assert(N_overlap > 1);
}
// Last
if (!to_B_defined || !to_A_defined)
@@ -568,19 +690,19 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
// std::cout << "\t\t\tA " << _points_A.col(to_A).transpose() << std::endl;
// std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
// std::cout << "\t\t\tB " << _points_B.col(to_B).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(_points_A.col(to_A),
+ dist2(N_overlap-1) = sqDistPointToLine(_points_A.col(to_A),
_points_A.col(prev_to_A),
T_A_B * _points_B.col(to_B),
to_A_defined,
to_B_defined);
- assert(N_overlapped > 1);
+ assert(N_overlap > 1);
}
//std::cout << "\t\tsquared distances = " << dist2.transpose() << "( should be < " << max_sq_error << ")" << std::endl;
// All squared distances should be within a threshold
if ((dist2 < max_sq_error).all())
{
- auto match = std::make_shared<MatchPolyline2D>();
+ auto match = std::make_shared<MatchPolyline2d>();
match->from_A_id_= from_A;
match->to_A_id_= to_A;
@@ -595,11 +717,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
}
}
- std::cout << matches.size() << " rigid transformations found\n";
+ //std::cout << matches.size() << " rigid transformations found\n";
return matches;
}
-bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B)
+bool isProjectedOverSegment(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux, const Eigen::Vector3d& _B)
{
/* The orthogonal projection of B over the line A-Aaux is in the segment [A,Aaux].
*
@@ -608,25 +730,25 @@ bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A
*
*/
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
+ double AB_sq = (_B-_A).head<2>().squaredNorm();
+ double AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
+ double AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
return (AauxB_sq <= AB_sq + AAaux_sq) && (AB_sq <= AauxB_sq + AAaux_sq);
}
-Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
+Eigen::VectorXd computeSquaredMahalanobisDistances(const Eigen::Vector2d& _feature,
+ const Eigen::Matrix2d& _feature_cov,
+ const Eigen::Vector2d& _expected_feature,
+ const Eigen::Matrix2d& _expected_feature_cov,
+ const Eigen::MatrixXd& _mu)
{
// ------------------------ d
- Eigen::Vector2s d = _feature - _expected_feature;
+ Eigen::Vector2d d = _feature - _expected_feature;
// ------------------------ Sigma_d
- Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
+ Eigen::Matrix2d iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
+ Eigen::VectorXd squared_mahalanobis_distances(_mu.cols());
for (unsigned int i = 0; i < _mu.cols(); i++)
{
squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
diff --git a/src/processor/processor_loop_closure_falko.cpp b/src/processor/processor_loop_closure_falko.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..92ba29fa13ecaecfb684287e56426c4102ecc467
--- /dev/null
+++ b/src/processor/processor_loop_closure_falko.cpp
@@ -0,0 +1,37 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/processor/processor_loop_closure_falko.h"
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoNnBsc);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoNnBsc);
+
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoNnCgh);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoNnCgh);
+
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoAhtBsc);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoAhtBsc);
+
+// WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoAhtCgh);
+// WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoAhtCgh);
+} // namespace wolf
diff --git a/src/processor/processor_loop_closure_falko_icp.cpp b/src/processor/processor_loop_closure_falko_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b59063b17e2c5aecf7e7ebcb00c4a7efb6a7ae4f
--- /dev/null
+++ b/src/processor/processor_loop_closure_falko_icp.cpp
@@ -0,0 +1,38 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/processor/processor_loop_closure_falko_icp.h"
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+ WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpNnBsc);
+ WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpNnBsc);
+
+ WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpAhtBsc);
+ WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpAhtBsc);
+
+ WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpNnCgh);
+ WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpNnCgh);
+
+ WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpAhtCgh);
+ WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpAhtCgh);
+
+} // namespace wolf
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a57559753b2cd4cf8503ab1f10a422ba18b52dfc
--- /dev/null
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -0,0 +1,335 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/processor/processor_loop_closure_icp.h"
+#include "laser/sensor/sensor_laser_2d.h"
+#include "core/math/covariance.h"
+#include <random>
+
+using namespace laserscanutils;
+using namespace Eigen;
+
+namespace wolf{
+
+ProcessorLoopClosureIcp::ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params) :
+ ProcessorLoopClosure("ProcessorLoopClosureIcp", 2, _params),
+ params_loop_closure_icp_(_params),
+ last_loop_closure_(nullptr)
+{
+ icp_tools_ptr_ = std::make_shared<ICP>();
+}
+
+void ProcessorLoopClosureIcp::configure(SensorBasePtr _sensor)
+{
+ sensor_laser_ = std::static_pointer_cast<SensorLaser2d>(_sensor);
+ laser_scan_params_ = sensor_laser_->getScanParams();
+}
+
+ParamsProcessorLoopClosureIcpPtr ProcessorLoopClosureIcp::getParams() const
+{
+ return params_loop_closure_icp_;
+}
+
+bool ProcessorLoopClosureIcp::voteFindLoopClosures(CaptureBasePtr cap)
+{
+ if (not last_loop_closure_)
+ return true;
+
+ if (params_loop_closure_icp_->frames_ignored_after_loop <= 0)
+ return true;
+
+ auto last_frm = last_loop_closure_->capture_target->getFrame();
+ for (auto i = 1; i <= params_loop_closure_icp_->frames_ignored_after_loop; i++)
+ {
+ last_frm = last_frm->getNextFrame();
+ if (not last_frm or last_frm == cap->getFrame())
+ {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(CaptureBasePtr _capture)
+{
+ std::map<double,MatchLoopClosurePtr> match_lc_map;
+
+ auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(_capture);
+ assert(cap_laser != nullptr);
+
+ // generate search list
+ auto frames_search_list = generateSearchList(_capture);
+
+ // Iterate search list looking for loop closure candidates
+ for (auto frm : frames_search_list)
+ {
+ // Frame can have more than one laser capture
+ for (auto cap_ref : frm->getCapturesOfType<CaptureLaser2d>())
+ {
+ auto cap_ref_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap_ref);
+ assert(cap_ref_laser != nullptr);
+
+ // Match Scans
+ auto match = matchScans(cap_ref_laser, cap_laser);
+
+ // IT'S A MATCH!
+ if (match != nullptr)
+ {
+ // avoid duplicated scores (std::map)
+ while (match_lc_map.count(match->normalized_score))
+ match->normalized_score -= 1e-9;
+
+ match_lc_map.emplace(match->normalized_score, match);
+
+ // one factor to each frame (in case of more than one laser capture..)
+ break;
+ }
+ }
+
+ // stop if enough candidates
+ if (match_lc_map.size() == params_loop_closure_icp_->max_candidates)
+ return match_lc_map;
+ }
+
+ return match_lc_map;
+}
+
+MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_tar) const
+{
+ auto frm_tar = cap_tar->getFrame();
+ auto frm_ref = cap_ref->getFrame();
+ auto sen_tar = std::static_pointer_cast<wolf::SensorLaser2d>(cap_tar->getSensor());
+ auto sen_ref = std::static_pointer_cast<wolf::SensorLaser2d>(cap_ref->getSensor());
+
+ // INITIAL GUESS
+ // Transformations
+ Isometry2d T_w_r_tar = Translation2d(frm_tar->getP()->getState()) * Rotation2Dd(frm_tar->getO()->getState()(0));
+ Isometry2d T_w_r_ref = Translation2d(frm_ref->getP()->getState()) * Rotation2Dd(frm_ref->getO()->getState()(0));
+ Isometry2d T_r_s_tar = Translation2d(sen_tar->getP()->getState()) * Rotation2Dd(sen_tar->getO()->getState()(0));
+ Isometry2d T_r_s_ref = Translation2d(sen_ref->getP()->getState()) * Rotation2Dd(sen_ref->getO()->getState()(0));
+ Isometry2d T_s_ref_s_tar = T_r_s_ref.inverse() * T_w_r_ref.inverse() * T_w_r_tar * T_r_s_tar;
+
+ // Fill up initial guess
+ Vector3d transform_guess;
+ transform_guess.head(2) = T_s_ref_s_tar.translation();
+ Rotation2Dd R(T_s_ref_s_tar.rotation());
+ transform_guess(2) = R.angle();
+
+ WOLF_DEBUG("Attempting to close loop between frames ", frm_tar->id(), " and ", frm_ref->id());
+ try
+ {
+ icpOutput icp_out = icp_tools_ptr_->align(cap_tar->getScan(),
+ cap_ref->getScan(),
+ sen_tar->getScanParams(),
+ sen_ref->getScanParams(),
+ params_loop_closure_icp_->icp_params,
+ transform_guess);
+
+ double points_coeff_used = ((double)icp_out.nvalid / (double)fmin(cap_tar->getScan().ranges_raw_.size(),
+ cap_ref->getScan().ranges_raw_.size()));
+ double mean_error = icp_out.error / icp_out.nvalid;
+
+ WOLF_DEBUG("DBG ------------------------------");
+ WOLF_DEBUG_COND(not icp_out.valid, "ICP not valid");
+ WOLF_DEBUG_COND(icp_out.valid,
+ "ICP valid ",
+ " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", frm_tar->id(),
+ " other_id: ", frm_ref->id());
+ WOLF_DEBUG("own_POSE: ", frm_tar->getState().vector("PO").transpose(),
+ " other_POSE: ", frm_ref->getState().vector("PO").transpose(),
+ " icp_guess: ", transform_guess.transpose(),
+ " icp_trf: ", icp_out.res_transf.transpose());
+ // WOLF_DEBUG("Covariance \n", icp_out.res_covar);
+
+ // Valid output
+ if (icp_out.valid == 1 and
+ mean_error < params_loop_closure_icp_->max_error_threshold and
+ points_coeff_used * 100 > params_loop_closure_icp_->min_points_percent)
+ {
+ WOLF_DEBUG("MATCH CONFIRMED ", frm_tar->id(), " and ", frm_ref->id());
+
+ auto match = std::make_shared<MatchLoopClosureIcp>();
+ match->capture_reference = cap_ref;
+ match->capture_target = cap_tar;
+ match->normalized_score = points_coeff_used; // ratio points normalized (0, 1]
+ //match->normalized_score_ = exp(-mean_error); // error normalized (0, 1]
+ match->icp_result = icp_out;
+
+ return match;
+ }
+ else
+ {
+ WOLF_DEBUG("MATCH DISCARDED ", frm_tar->id(), " and ", frm_ref->id());
+ return nullptr;
+ }
+
+ }
+ catch (std::exception &e)
+ {
+ WOLF_WARN("ProcessorLoopClosureIcp::matchScans ICP failed with output: ", e.what())
+ }
+ return nullptr;
+}
+
+void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr match)
+{
+ auto match_icp = std::static_pointer_cast<MatchLoopClosureIcp>(match);
+
+ assert(match_icp->icp_result.valid == 1);
+ if (not isCovariance(match_icp->icp_result.res_covar))
+ match_icp->icp_result.res_covar = 1e-4 * Matrix3d::Identity();
+
+ auto ftr = FeatureBase::emplace<FeatureICPAlign>(match_icp->capture_target,
+ match_icp->icp_result);
+
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr,
+ ftr,
+ match_icp->capture_reference->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_LOOP);
+
+ // store last frame
+ last_loop_closure_ = match;
+}
+
+FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap) const
+{
+ assert(_cap->getFrame() and _cap->getFrame()->getTrajectory());
+
+ int N = getProblem()->getTrajectory()->getFrameMap().size() - 1 - params_loop_closure_icp_->recent_frames_ignored;
+
+ WOLF_DEBUG("ProcessorLoopClosureIcp::generateSearchList N = ", N);
+
+ if (N <= 0)
+ return FrameBasePtrList();
+
+ auto map_begin = getProblem()->getTrajectory()->getFrameMap().begin();
+
+ //std::vector<int> idxs;
+ std::vector<bool> done(N, false);
+ FrameBasePtrList frame_list;
+
+ if (params_loop_closure_icp_->candidate_generation == "tree" or
+ params_loop_closure_icp_->candidate_generation == "TREE" or
+ params_loop_closure_icp_->candidate_generation == "Tree")
+ {
+ // generates the series:
+ // 0, 1/2, 1/4, 3/4, 1/8, 3/8, 5/8, 7/8,...
+
+ double frac = N;
+ int checked = 1;
+
+ // initialize with first: 0
+ done.at(0) = true;
+ if (not map_begin->second->getCapturesOfType<CaptureLaser2d>().empty())
+ {
+ //idxs.push_back(0);
+ frame_list.push_back(map_begin->second);
+ }
+
+ // continue with tree
+ while (checked < N and
+ frame_list.size() < params_loop_closure_icp_->max_attempts)
+ {
+ frac *= 0.5;
+ for (auto i = 1; ; i+=2)
+ {
+ int idx = (int)round(i*frac);
+ if (idx >= N)
+ break;
+
+ if (not done.at(idx))
+ {
+ done.at(idx) = true;
+ checked++;
+ auto frame_idx = std::next(map_begin, idx)->second;
+ assert(frame_idx);
+ assert(frame_idx != _cap->getFrame());
+ if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
+ {
+ //idxs.push_back(idx);
+ frame_list.push_back(frame_idx);
+ }
+ }
+ }
+ }
+ }
+ else if (params_loop_closure_icp_->candidate_generation == "random" or
+ params_loop_closure_icp_->candidate_generation == "RANDOM" or
+ params_loop_closure_icp_->candidate_generation == "Random")
+ {
+ int checked = 0;
+ while (checked < N and
+ frame_list.size() < params_loop_closure_icp_->max_attempts)
+ {
+ std::random_device rd; // obtain a random number from hardware
+ std::mt19937 gen(rd()); // seed the generator
+ std::uniform_int_distribution<> distr(0, N-1); // define the range
+
+ int idx = distr(gen); // generate random number
+
+ // already added to list
+ if (done.at(idx))
+ continue;
+
+ // mark as done
+ done.at(idx) = true;
+ checked++;
+
+ // consider adding
+ auto frame_idx = std::next(map_begin, idx)->second;
+ assert(frame_idx);
+ assert(frame_idx != _cap->getFrame());
+ if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
+ {
+ //idxs.push_back(idx);
+ frame_list.push_back(frame_idx);
+ }
+ }
+ }
+ else
+ {
+ std::cout << params_loop_closure_icp_->print() << std::endl;
+ throw std::runtime_error("ParamsProcessorLoopClosureIcp::candidate_generation only accepts 'tree' or 'random'");
+ }
+
+ WOLF_DEBUG("ProcessorLoopClosureIcp::generateSearchList: in mode ",
+ params_loop_closure_icp_->candidate_generation,
+ " resulting list:");
+ #ifdef _WOLF_DEBUG
+ std::cout << "\t";
+ for (auto frm : frame_list)
+ std::cout << frm->id() << ", ";
+ std::cout << std::endl;
+ #endif
+
+ return frame_list;
+}
+
+}
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureIcp)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureIcp)
+} /* namespace wolf */
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..100dd66185496d1c923ba4e1ba48fe24f549a7b4
--- /dev/null
+++ b/src/processor/processor_odom_icp.cpp
@@ -0,0 +1,354 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/processor/processor_odom_icp.h"
+#include "laser/math/laser_tools.h"
+#include "core/math/covariance.h"
+
+using namespace laserscanutils;
+namespace wolf
+{
+
+ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
+ ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params),
+ MotionProvider("PO", _params),
+ odom_origin_(Eigen::Vector3d::Zero()),
+ odom_last_(Eigen::Vector3d::Zero()),
+ odom_incoming_(Eigen::Vector3d::Zero()),
+ params_odom_icp_(_params)
+{
+ // Icp algorithm
+ icp_tools_ptr_ = std::make_shared<ICP>();
+
+ // Frame transforms
+ trf_origin_last_.res_transf = Eigen::Vector3d::Zero();
+ trf_origin_incoming_.res_transf = Eigen::Vector3d::Zero();
+ trf_last_incoming_.res_transf = Eigen::Vector3d::Zero();
+ trf_origin_last_.res_covar = Eigen::Matrix3d::Identity();
+ trf_origin_incoming_.res_covar = Eigen::Matrix3d::Identity();
+ trf_last_incoming_.res_covar = Eigen::Matrix3d::Identity();
+}
+
+ProcessorOdomIcp::~ProcessorOdomIcp()
+{
+
+}
+
+void ProcessorOdomIcp::preProcess()
+{
+ auto cap_ptr = std::dynamic_pointer_cast<CaptureLaser2d>(incoming_ptr_);
+ assert(cap_ptr != nullptr && ("Capture type mismatch. " + getName() + " can only process captures of type CaptureLaser2d").c_str());
+
+ if (params_odom_icp_->initial_guess == "odom")
+ odom_incoming_ = getProblem()->getOdometry("PO").vector("PO");
+ else if (params_odom_icp_->initial_guess == "state" )
+ {
+ odom_incoming_ = getProblem()->getState("PO").vector("PO");
+ if(last_ptr_)
+ {
+ odom_last_ = getProblem()->getState(last_ptr_->getTimeStamp(), "PO").vector("PO");
+ }
+ if(origin_ptr_)
+ {
+ odom_origin_ = getProblem()->getState(origin_ptr_->getTimeStamp(), "PO").vector("PO");
+ }
+ }
+
+ assert(odom_incoming_.size() == 3);
+}
+
+void ProcessorOdomIcp::configure(SensorBasePtr _sensor)
+{
+ sensor_laser_ = std::static_pointer_cast<SensorLaser2d>(_sensor);
+ laser_scan_params_ = sensor_laser_->getScanParams();
+}
+
+unsigned int ProcessorOdomIcp::processKnown()
+{
+ // Match the incoming with the origin, passing the transform from origin to last as initialization
+
+ if (origin_ptr_) // Make sure it's not the FIRST_TIME
+ {
+ CaptureLaser2dPtr origin_ptr = std::static_pointer_cast<CaptureLaser2d>(origin_ptr_);
+ CaptureLaser2dPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2d>(incoming_ptr_);
+
+ Eigen::Vector3d initial_guess = this->trf_origin_last_.res_transf;
+ if (params_odom_icp_->initial_guess == "odom" or params_odom_icp_->initial_guess == "state" )
+ {
+ //TODO: Add sensor extrinsics
+ initial_guess.head(2) += Eigen::Rotation2Dd(-odom_origin_(2)) * (odom_last_.head(2) - odom_incoming_.head(2));
+ initial_guess(2) += -(odom_incoming_(2) - odom_last_(2));
+ }
+ else if (params_odom_icp_->initial_guess != "zero")
+ throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
+
+ trf_origin_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
+ origin_ptr->getScan(),
+ this->laser_scan_params_,
+ params_odom_icp_->icp_params,
+ initial_guess); // Check order
+ WOLF_DEBUG("ProcessorOdomIcp::processKnown odom_origin_: ", odom_origin_.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processKnown odom_incoming_: ", odom_incoming_.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processKnown initial guess: ", initial_guess.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processKnown ICP transform: ", trf_origin_incoming_.res_transf.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processKnown ICP cov: \n", trf_origin_incoming_.res_covar);
+
+ //trf_origin_incoming_.valid = trf_origin_incoming_.valid && trf_origin_incoming_.error / trf_origin_incoming_.nvalid < 5e-2;
+ }
+ return 0;
+}
+
+unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
+{
+
+ CaptureLaser2dPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2d>(incoming_ptr_);
+ CaptureLaser2dPtr last_ptr = std::static_pointer_cast<CaptureLaser2d>(last_ptr_);
+
+ Eigen::Vector3d initial_guess(Eigen::Vector3d::Zero());
+ if (params_odom_icp_->initial_guess == "odom" or params_odom_icp_->initial_guess == "state" )
+ {
+ initial_guess.head(2) = Eigen::Rotation2Dd(-odom_incoming_(2)) * (odom_last_.head(2) - odom_incoming_.head(2));
+ initial_guess(2) = -(odom_incoming_(2) - odom_last_(2));
+ }
+ else if (params_odom_icp_->initial_guess != "zero")
+ throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
+
+ trf_last_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
+ last_ptr->getScan(),
+ this->laser_scan_params_,
+ params_odom_icp_->icp_params,
+ initial_guess);
+ WOLF_DEBUG("ProcessorOdomIcp::processNew odom_incoming_: ", odom_incoming_.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processNew odom_last_: ", odom_last_.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processNew initial guess: ", initial_guess.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processNew ICP transform: ", trf_origin_incoming_.res_transf.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processNew ICP cov: \n", trf_origin_incoming_.res_covar);
+
+ //trf_last_incoming_.valid = trf_last_incoming_.valid && trf_last_incoming_.error / trf_last_incoming_.nvalid < 5e-2;
+
+ return 0;
+}
+
+bool ProcessorOdomIcp::voteForKeyFrame() const
+{
+ return trf_origin_last_.valid == 1 &&
+ (voteForKeyFrameDistance() ||
+ voteForKeyFrameAngle() ||
+ voteForKeyFrameTime() ||
+ voteForKeyFrameMatchQuality());
+}
+
+inline bool ProcessorOdomIcp::voteForKeyFrameDistance() const
+{
+ bool vote = (trf_origin_incoming_.res_transf.head<2>().norm() > params_odom_icp_->vfk_min_dist);
+ WOLF_DEBUG_COND(vote, "ProcessorOdomIcp::voteForKeyFrameDistance: Voting...");
+
+ return vote;
+}
+
+inline bool ProcessorOdomIcp::voteForKeyFrameAngle() const
+{
+ bool vote = (fabs(trf_origin_incoming_.res_transf(2)) > params_odom_icp_->vfk_min_angle);
+ WOLF_DEBUG_COND(vote, "ProcessorOdomIcp::voteForKeyFrameAngle: Voting...");
+
+ return vote;
+}
+
+inline bool ProcessorOdomIcp::voteForKeyFrameTime() const
+{
+ double secs = incoming_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp();
+
+ bool vote = (secs > params_odom_icp_->vfk_min_time);
+
+ WOLF_DEBUG_COND(vote, "ProcessorOdomIcp::voteForKeyFrameTime: Voting...");
+
+ return vote;
+}
+
+inline bool ProcessorOdomIcp::voteForKeyFrameMatchQuality() const
+{
+ bool origin_incoming_invalid = trf_origin_incoming_.error/trf_origin_incoming_.nvalid > params_odom_icp_->vfk_min_error ||
+ trf_origin_incoming_.nvalid < params_odom_icp_->vfk_max_points ||
+ trf_origin_incoming_.valid != 1;
+ bool origin_last_invalid = trf_origin_last_.error/trf_origin_last_.nvalid > params_odom_icp_->vfk_min_error ||
+ trf_origin_last_.nvalid < params_odom_icp_->vfk_max_points ||
+ trf_origin_last_.valid != 1;
+
+ if (origin_incoming_invalid and not origin_last_invalid)
+ {
+ WOLF_DEBUG("ProcessorOdomIcp::voteForKeyFrameMatchQuality: Voting...");
+ WOLF_DEBUG_COND(trf_origin_incoming_.error / trf_origin_incoming_.nvalid > params_odom_icp_->vfk_min_error, "error greater than min. threshold" );
+ WOLF_DEBUG_COND(trf_origin_incoming_.nvalid < params_odom_icp_->vfk_max_points, "nvalid points below max" );
+ WOLF_DEBUG_COND(trf_origin_incoming_.valid != 1, "CSM transform not valid");
+ }
+ return origin_incoming_invalid and not origin_last_invalid;
+}
+
+void ProcessorOdomIcp::advanceDerived()
+{
+ using namespace Eigen;
+
+ odom_last_ = odom_incoming_;
+
+ // computing odometry
+ auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
+ auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
+ Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
+ odometry_['P'] = sl_T_si.translation();
+ odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
+
+ // advance transforms
+ trf_origin_last_ = trf_origin_incoming_;
+}
+
+void ProcessorOdomIcp::resetDerived()
+{
+ // Using processing_step_ to ensure that origin, last and incoming are different
+ if (processing_step_ != FIRST_TIME_WITH_KEYFRAME && processing_step_ != FIRST_TIME_WITHOUT_KEYFRAME)
+ {
+ // Notation explanation:
+ // x1_R_x2: Rotation from frame x1 to frame x2
+ // x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
+
+ // computing odometry
+ auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
+ auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
+ Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
+ odometry_['P'] = sl_T_si.translation();
+ odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
+
+ // advance transforms
+ trf_origin_last_ = trf_last_incoming_;
+
+ odom_origin_ = odom_last_;
+ odom_last_ = odom_incoming_;
+ }
+}
+
+void ProcessorOdomIcp::establishFactors()
+{
+ if (last_ptr_ != origin_ptr_)
+ {
+ auto ftr_ptr = emplaceFeature(last_ptr_);
+ emplaceFactor(ftr_ptr);
+ }
+}
+
+FeatureBasePtr ProcessorOdomIcp::emplaceFeature(CaptureBasePtr _capture_laser)
+{
+ assert(_capture_laser == last_ptr_);
+
+ if (not isCovariance(trf_origin_last_.res_covar))
+ trf_origin_last_.res_covar = 1e-4 * Eigen::Matrix3d::Identity();
+
+ return FeatureBase::emplace<FeatureICPAlign>(_capture_laser,
+ trf_origin_last_);
+}
+
+FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
+{
+ WOLF_DEBUG("ProcessorOdomIcp::emplaceFactor: feature = ", _feature->getMeasurement().transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::emplaceFactor: cov = \n", _feature->getMeasurementCovariance());
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
+ _feature,
+ origin_ptr_->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
+}
+
+VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) const
+{
+ 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.
+ {
+ WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
+ return VectorComposite(); // return empty state
+ }
+
+ auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
+ auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
+ const auto& rl_T_sl = ro_T_so; // A reference just to have nice names without computing overhead
+
+ auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
+ Isometry2d w_T_rl = w_T_ro * ro_T_so * so_T_sl * rl_T_sl.inverse();
+
+ VectorComposite state("PO", {w_T_rl.translation(), Vector1d(Rotation2Dd(w_T_rl.rotation()).angle())});
+
+ return state;
+}
+
+TimeStamp ProcessorOdomIcp::getTimeStamp() const
+{
+ if( last_ptr_ == nullptr )
+ return TimeStamp::Invalid();
+ else
+ return last_ptr_->getTimeStamp();
+}
+
+VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStructure& _structure) const
+{
+ // todo fix this code to get any state in the whole trajectory!
+
+ //
+ if (last_ptr_ != nullptr)
+ {
+ double d = fabs(_ts - last_ptr_->getTimeStamp());
+ if( d < params_->time_tolerance )
+ {
+ return getState( );
+ }
+ else
+ {
+ WOLF_WARN("Requested state with time stamp out of tolerance. Returning zero state");
+ return getProblem()->stateZero("PO"); // return zero
+ }
+ }
+ // return state at origin if possible
+ else
+ {
+ if (origin_ptr_ == nullptr || fabs(_ts - last_ptr_->getTimeStamp()) > params_->time_tolerance)
+ return VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
+ else
+ return origin_ptr_->getFrame()->getState("PO");
+ }
+
+
+}
+
+void ProcessorOdomIcp::setProblem(ProblemPtr _problem_ptr)
+{
+ NodeBase::setProblem(_problem_ptr);
+ addToProblem(_problem_ptr, std::dynamic_pointer_cast<MotionProvider>(shared_from_this()));
+}
+
+} // namespace wolf
+
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+ WOLF_REGISTER_PROCESSOR(ProcessorOdomIcp);
+ WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdomIcp);
+} // namespace wolf
diff --git a/src/processor/processor_polyline.cpp b/src/processor/processor_polyline.cpp
deleted file mode 100644
index 52208cbaabaa8a9cb9bddaaa337a09a0b916121a..0000000000000000000000000000000000000000
--- a/src/processor/processor_polyline.cpp
+++ /dev/null
@@ -1,925 +0,0 @@
-/*
- * processor_polyline.cpp
- *
- * Created on: Sep 14, 2017
- * Author: jvallve
- */
-
-#include "laser/processor/processor_polyline.h"
-#include <algorithm> // std::swap
-
-namespace wolf
-{
-
-ProcessorPolyline::ProcessorPolyline(ProcessorParamsPolylinePtr _params) :
- ProcessorBase("POLYLINES",_params),
- last_ptr_(nullptr),
- incoming_ptr_(nullptr),
- line_finder_(_params->line_finder_params),
- params_(_params),
- extrinsics_transformation_computed_(false)
-{
-}
-
-ProcessorPolyline::~ProcessorPolyline()
-{
-}
-
-// ALGORITHM //////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
-{
- //WOLF_DEBUG( "PROCESS ------------------" );
- incoming_ptr_ = _incoming_ptr;
-
- // extract corners of incoming
- extractPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), new_features_incoming_);
- //std::cout << new_features_incoming_.size() << " polylines extracted" << std::endl;
-
- // compute transformations
- computeTransformations(incoming_ptr_->getTimeStamp());
-
- // FIRST TIME: create KF
- if (last_ptr_ == nullptr)
- {
- //WOLF_DEBUG( "FIRST TIME" );
-
- // advance this
- last_ptr_ = incoming_ptr_;
- new_features_last_ = std::move(new_features_incoming_);
- incoming_ptr_ = nullptr;
-
- // keyframe for last
- PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( last_ptr_, params_->time_tolerance);
- // Exist a KF within the time tolerance
- if (KF_pack)
- {
- KF_pack->key_frame->addCapture(last_ptr_);
- WOLF_TRACE( "PR ",getName() ," - FIRST TIME: Last capture ", last_ptr_->id(), " appended to existing KF: " , KF_pack->key_frame->id() );
- }
- else
- {
- // Make KF
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, last_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
- WOLF_TRACE( "PR ",getName() ," - FIRST TIME: Last capture ", last_ptr_->id(), " appended to a new KF: " , new_frame_ptr->id() );
- getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_->time_tolerance);
- }
-
- // Create landmarks from new_features_last_
- processNew();
-
- // Establish factors from last
- establishFactors();
- }
- // OTHER TIMES
- else
- {
- assert(last_ptr_->getFrame() && "last_ not linked to any frame in OTHER TIMES");
- //WOLF_DEBUG("OTHER TIMES");
-
- // 1. First we track the known Features and create new factors as needed
-
- // Find landmarks in new_features_incoming_ (and extract from it)
- findLandmarks(getProblem()->getMap()->getLandmarkList(), known_features_incoming_, matches_landmark_from_incoming_);
- //WOLF_DEBUG("incoming new_features: " , new_features_incoming_.size());
- //WOLF_DEBUG("incoming known features: ", known_features_incoming_.size());
-
- // 2 CASES in which last should be moved to a (new or existing) KF:
- // - voteForKF() && permittedKF() --> CREATE KF
- // - There's a new KF within last time_tolerance --> EXISTING KF
- // 2 NECESSARY CONDITIONS:
- // - Last is not KF
- // - KF has not another capture of the same sensor
- PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( last_ptr_, params_->time_tolerance);
- bool KF_has_capture = KF_pack && (KF_pack->key_frame->getCaptureOf(getSensor()) != nullptr);
- bool create_KF = !KF_has_capture && !last_ptr_->getFrame()->isKey() && voteForKeyFrame() && permittedKeyFrame();
- bool existing_KF = !KF_has_capture && !last_ptr_->getFrame()->isKey() && KF_pack;
-
- FrameBasePtr closest_key_frm_to_last = getProblem()->getTrajectory()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
-
- //WOLF_DEBUG("last is KF? ", (last_is_KF ? "YES" : "NO"));
-
- // KEYFRAME (already created or decided to be created)
- if (create_KF || existing_KF )
- {
- // Append all new Features to the Capture's list of Features
- last_ptr_->addFeatureList(known_features_last_);
-
- // Create landmarks from new_features_last_, find them from new_features_incoming_
- processNew();
-
- // create KF
- if (create_KF)
- {
- // Make a non-key-frame to hold incoming
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(incoming_ptr_);
- //WOLF_DEBUG( "Incoming adhered to new F" , new_frame_ptr->id() );
-
- // Make the last Capture's Frame a KeyFrame
- setKeyFrame(last_ptr_);
-
- WOLF_DEBUG("PR ",getName() ," - Set KEY to last F", last_ptr_->getFrame()->id());
- }
- // existing KF within the time tolerance
- else
- {
- // Move incomming to the last F
- last_ptr_->getFrame()->addCapture(incoming_ptr_);
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
-
- // Move last to the existing KF
- last_ptr_->getFrame()->unlinkCapture(last_ptr_);
- KF_pack->key_frame->addCapture(last_ptr_);
-
- WOLF_DEBUG("PR ",getName() ," - Last capture ", last_ptr_->id(), " appended to existing KF ", last_ptr_->getFrame()->id());
- }
-
- // Establish factors for last matches
- establishFactors();
-
- // Reset the Tracker
- // Move matches and new features from incoming to last
- matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- known_features_last_ = std::move(known_features_incoming_);
-
- // Reset this
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
- }
- // NOT KEYFRAME -> Advance the tracker
- else
- {
- // Move matches and new features from incoming to last
- matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- known_features_last_ = std::move(known_features_incoming_);
- //WOLF_DEBUG("new_features_last_ " , new_features_last_.size());
-
- if (last_ptr_->getFrame()->isKey())
- {
- //WOLF_DEBUG("Last is in a KF \n");
- // Make a non-key-frame to hold incoming
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(incoming_ptr_);
- //WOLF_DEBUG( "Incoming adhered to new F" , new_frame_ptr->id() );
- }
- else
- {
- //WOLF_DEBUG("Last is NOT in a KF \n");
- // Add incoming Capture to the tracker's last Frame
- last_ptr_->getFrame()->addCapture(incoming_ptr_);
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
- // discard last capture
- last_ptr_->remove();
- }
-
- // Incoming Capture takes the place of last Capture
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
-
- //WOLF_DEBUG("last <-- incoming");
- }
- }
- //WOLF_DEBUG( "-----End of process():");
-
- // post-process
- // try to merge landmarks (stored in merge_candidates_list_)
- while (!merge_candidates_list_.empty())
- {
- // load next list of candidates
- LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front());
- merge_candidates_list_.pop_front();
-
- // change already merged lmks with the corresponding remaining lmk
- for (auto&& lmk : merge_candidates)
- if (lmk->getMergedInLandmark() != nullptr)
- lmk = lmk->getMergedInLandmark();
-
- // remove repeated lmks
- merge_candidates.sort();
- merge_candidates.unique();
-
- if (merge_candidates.size() == 1)
- continue;
-
- // Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
- LandmarkPolyline2D::tryMergeLandmarks(merge_candidates, params_->position_error_th_);
- }
- //WOLF_DEBUG( "-----End of post-process():");
-}
-
-void ProcessorPolyline::processNew()
-{
- // create landmarks from new_features_last
- createNewLandmarks();
-
- // Find the new landmarks in known_features_incoming_ (if it's not nullptr)
- if (incoming_ptr_ != nullptr && !new_landmarks_.empty())
- findLandmarks(new_landmarks_, known_features_incoming_, matches_landmark_from_incoming_);
-
- // Append all new Features to the Capture's list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
- // Append new landmarks to the map
- getProblem()->addLandmarkList(new_landmarks_);
-}
-
-bool ProcessorPolyline::voteForKeyFrame()
-{
- //std::cout << "------------- ProcessorPolyline::voteForKeyFrame:" << std::endl;
- // option 1: more than TH new features in last
- if (new_features_last_.size() >= params_->new_features_th)
- {
- WOLF_DEBUG("PR ",getName(),"------------- NEW KEY FRAME: Option 1 - Enough new features: ", new_features_last_.size(), " >= ", params_->new_features_th);
- return true;
- }
- // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
- for (auto new_ftr : last_ptr_->getFeatureList())
- {
- assert(matches_landmark_from_last_.find(new_ftr) != matches_landmark_from_last_.end());
- assert(matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().size() > 0);
- if (std::fabs(last_ptr_->getTimeStamp() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getTimeStamp()) > params_->loop_time_th)
- {
- WOLF_DEBUG("PR ",getName(),"------------- NEW KEY FRAME: Option 2 - Loop closure");
- return true;
- }
- }
- return false;
-}
-
-// IMPLEMENTATION /////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list)
-{
- //WOLF_DEBUG("ProcessorPolyline::extractPolylines: ");
- // TODO: sort corners by bearing
- std::list<laserscanutils::Polyline> polylines;
-
- line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
- //WOLF_DEBUG("Polylines extracted: ", polylines.size());
-
- for (auto&& pl : polylines)
- {
- //WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ );
- //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
- _polyline_list.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
- //WOLF_DEBUG("new polyline detected: ");
- }
-}
-
-unsigned int ProcessorPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
- FeatureBasePtrList& _features_found,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- //std::cout << "ProcessorPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << new_features_incoming_.size() << std::endl;
-
- //std::map<LandmarkPolyline2DPtr,LandmarkPolyline2DPtr> merged_lmks;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkPolyline2DPtr, Eigen::MatrixXs> expected_features;
- std::map<LandmarkPolyline2DPtr, Eigen::MatrixXs> expected_features_covs;
- for (auto lmk : _landmarks_searched)
- if (lmk->getType() == "POLYLINE 2D" && !lmk->isRemoving())
- {
- LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk);
- expected_features[pl_lmk] = Eigen::MatrixXs(3, pl_lmk->getNPoints());
- expected_features_covs[pl_lmk] = Eigen::MatrixXs(2, 2*pl_lmk->getNPoints());
- expectedFeature(pl_lmk, expected_features[pl_lmk], expected_features_covs[pl_lmk]);
- }
-
- // NAIVE NEAREST NEIGHBOR MATCHING
- LandmarkMatchPolyline2DPtr best_match = nullptr;
- FeaturePolyline2DPtr pl_ftr;
- LandmarkPolyline2DPtr pl_lmk;
- LandmarkPolyline2DPtrList merging_candidates;
- auto feature_it = new_features_incoming_.begin();
-
- // iterate over all polylines features
- while (feature_it != new_features_incoming_.end())
- {
- pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*feature_it);
-
- // Check with all landmarks
- for (auto landmark_it = _landmarks_searched.begin(); landmark_it != _landmarks_searched.end(); landmark_it++)
- {
- pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(*landmark_it);
-
- if (pl_lmk->isRemoving())
- continue;
-
- bool print = false;//(pl_lmk->id() == 13);
- if (print)
- std::cout << "\tcompute Best Sq Dist of landmark " << pl_lmk->id() << " (" << pl_lmk->getNPoints()
- << ") and feature " << pl_ftr->id() << "(" << pl_ftr->getNPoints() << ")\n";
- LandmarkMatchPolyline2DPtr best_correspondence = tryMatch(expected_features[pl_lmk],pl_lmk,pl_ftr);
-
- // best match
- if (best_correspondence != nullptr &&
- (best_match == nullptr ||
- (best_correspondence->feature_to_id_-best_correspondence->feature_from_id_ >= best_match->feature_to_id_-best_match->feature_from_id_ &&
- best_correspondence->normalized_score_ < best_match->normalized_score_ ) ) )
- {
- if (print)
- std::cout << "BEST MATCH" << std::endl;
- // More than one match -> landmark candidates to be merged
- if (best_match != nullptr)
- {
- if (merging_candidates.empty()) // add first best match
- merging_candidates.push_back(std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_));
- merging_candidates.push_back(pl_lmk); // add current best match
- }
- best_match = best_correspondence;
- }
- }
-
- // next feature
- feature_it++;
-
- // Match found for this feature
- if (best_match)
- {
- // make a copy of current iterator
- auto it_found = std::prev(feature_it);
-
- assert(best_match->feature_from_id_ == 0 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
- assert(best_match->feature_to_id_ == std::static_pointer_cast<FeaturePolyline2D>(*it_found)->getNPoints()-1 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
-
- bool print = false;//best_match->landmark_ptr_->id() == 13;
- if (print)
- {
- std::cout << "\tlandmark : " << best_match->landmark_ptr_->id() << std::endl;
- std::cout << "\tlandmark version: " << best_match->landmark_version_ << std::endl;
- std::cout << "\tfeature : " << (*it_found)->id() << std::endl;
- std::cout << "\tlandmark from id: " << best_match->landmark_from_id_ << std::endl;
- std::cout << "\tlandmark to id : " << best_match->landmark_to_id_ << std::endl;
- std::cout << "\tfeature from id : " << best_match->feature_from_id_ << std::endl;
- std::cout << "\tfeature to id : " << best_match->feature_to_id_ << std::endl;
- }
- // match
- _feature_landmark_correspondences[*it_found] = best_match;
- // move corner feature to output list
- _features_found.splice(_features_found.end(), new_features_incoming_, it_found);
- // reset match
- best_match = nullptr;
- }
-
- // Store landmarks that matched with the same feature as candidates to be merged
- if (!merging_candidates.empty())
- merge_candidates_list_.push_back(std::move(merging_candidates));
- }
-
- //std::cout << matches_landmark_from_incoming_.size() << " landmarks found from " << getProblem()->getMap()->getLandmarkList().size() << std::endl;
- //std::cout << "done" << std::endl;
- return matches_landmark_from_incoming_.size();
-}
-
-void ProcessorPolyline::createNewLandmarks()
-{
- //std::cout << "ProcessorPolyline::createNewLandmarks - new_features_last_" << new_features_last_.size() << std::endl;
- FeaturePolyline2DPtr polyline_ft_ptr;
- LandmarkBasePtr new_lmk_ptr;
- for (auto new_feature_ptr : new_features_last_)
- {
- // create new landmark
- new_lmk_ptr = createLandmark(new_feature_ptr);
- //WOLF_DEBUG("PR ",getName()," - New Landmark ", new_lmk_ptr->id());
- //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl;
- //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< std::static_pointer_cast<LandmarkPolyline2D>(new_lmk_ptr)->getNPoints() << " points" << std::endl;
-
- // cast
- polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr);
- // create new correspondence
- LandmarkMatchPolyline2DPtr match = std::make_shared<LandmarkMatchPolyline2D>();
- match->feature_from_id_= 0; // all points match
- match->landmark_from_id_ = 0;
- match->feature_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match
- match->landmark_to_id_ = polyline_ft_ptr->getNPoints()-1;
- match->normalized_score_ = 1.0; // max score
- match->landmark_ptr_ = new_lmk_ptr;
- match->landmark_version_ = 0;
-
- // store match
- matches_landmark_from_last_[new_feature_ptr] = match;
-
- // store new landmark
- new_landmarks_.push_back(new_lmk_ptr);
-
- // std::cout << "new match: feature " << new_feature_ptr->id() << " with landmark " << new_lmk_ptr->id() << std::endl;
- // std::cout << "\tfeature_from_id_ = " << match->feature_from_id_ << std::endl;
- // std::cout << "\tlandmark_from_id_ = " << match->landmark_from_id_ << std::endl;
- // std::cout << "\tfeature_to_id_ = " << match->feature_to_id_ << std::endl;
- // std::cout << "\tlandmark_to_id_ = " << match->landmark_to_id_ << std::endl;
- // std::cout << "\tnormalized_score_ = " << match->normalized_score_ << std::endl;
- }
- //std::cout << "done" << std::endl;
-}
-
-LandmarkBasePtr ProcessorPolyline::createLandmark(FeatureBasePtr _feature_ptr)
-{
- //std::cout << "ProcessorPolyline::createLandmark" << std::endl;
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
- assert(_feature_ptr->getType() == "POLYLINE 2D");
-
-
- FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
- // compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
- // std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- // std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
- // Create new landmark
- return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
- //std::cout << "done" << std::endl;
-}
-
-void ProcessorPolyline::establishFactors()
-{
- //TODO: update with new index in landmarks
- //std::cout << "ProcessorPolyline::establishFactors" << std::endl;
- //std::cout << "\tlast_ id: " << last_ptr_->id() << " linked to frame: " << last_ptr_->getFrame()->id() << " is linked to problem? " << (last_ptr_->getProblem() ? "YES" : "NO") << std::endl;
- unsigned int N_factors = 0;
- LandmarkMatchPolyline2DPtr pl_match;
- FeaturePolyline2DPtr pl_ftr;
- LandmarkPolyline2DPtr pl_lmk;
- LandmarkPolyline2DPtrList modified_lmks;
-
- for (auto last_ftr : last_ptr_->getFeatureList())
- {
- pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(last_ftr);
- pl_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(matches_landmark_from_last_[last_ftr]);
- pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(pl_match->landmark_ptr_);
-
- assert(pl_match->feature_from_id_ >= 0 && pl_match->feature_to_id_ >= 0);
- assert(pl_lmk != nullptr && pl_match != nullptr);
-
- if (pl_lmk->isRemoving())
- continue;
-
- bool print = false; // pl_lmk->id() == 13
-
- // CHECK IF LANDMARK CHANGED AFTER MATCHING
- if (pl_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
- {
- // get the lmk in which lmk was merged
- auto merged_in_lmk = pl_lmk->getMergedInLandmark();
- if (merged_in_lmk)
- pl_lmk = merged_in_lmk;
-
- // redo matching
- LandmarkMatchPolyline2DPtr updated_match = tryMatch(pl_lmk, pl_ftr);
-
- // if still matching, overwrite match
- if (updated_match)
- pl_match = updated_match;
-
- // if not matching, erase match and jump to next match
- else
- {
- matches_landmark_from_last_.erase(last_ftr);
- continue;
- }
-
- assert(pl_match->feature_from_id_ >= 0 && pl_match->feature_to_id_ >= 0);
- }
-
- // MODIFY LANDMARK (only for not closed and not recently created)
- if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
- {
- if (print)
- {
- std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl;
- std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl;
- std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl;
- std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl;
- }
- Eigen::MatrixXs points_global = R_world_sensor_ * pl_ftr->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
-
- // MODIFY LANDMARK
- // -----------------GROW Front-----------------
- // Add new front points (if any feature point not matched)
- if ( pl_match->feature_from_id_ > 0 ) // && !pl_lmk->isClosed()
- {
- assert(!pl_lmk->isClosed() && "feature has not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add new front points. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- pl_match->feature_from_id_-1, // last feature point index to be added
- pl_ftr->isFirstDefined(), // is defined
- false); // front
-
- modified_lmks.push_back(pl_lmk);
-
- pl_match->landmark_from_id_ = pl_lmk->getFirstId();
- pl_match->feature_from_id_ = 0;
- //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- // -----------------CHANGE Front-----------------
- // Change first not defined point if landmark first not defined matched with first feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (pl_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
- {
- if (print)
- {
- std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isFirstDefined() ||
- // case b
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))).squaredNorm() >
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm())
- {
- pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
- modified_lmks.push_back(pl_lmk);
- }
- }
- assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- // -----------------GROW Back-----------------
- // Add new back points (if any feature point not matched)
- if (pl_match->feature_to_id_ < pl_ftr->getNPoints()-1)
- {
- assert(!pl_lmk->isClosed() && "feature not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add back points. Defined: " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- pl_match->feature_to_id_+1, // last feature point index to be added
- pl_ftr->isLastDefined(), // is defined
- true); // back
-
- modified_lmks.push_back(pl_lmk);
-
- pl_match->landmark_to_id_ = pl_lmk->getLastId();
- pl_match->feature_to_id_ = pl_ftr->getNPoints()-1;
- //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
- //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
- //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
- //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
- }
- // -----------------CHANGE Back-----------------
- // Change last not defined point if landmark last not defined matched with last feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (pl_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
- {
- if (print)
- {
- std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isLastDefined() ||
- // case b
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))).squaredNorm() >
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm())
- {
- pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined());
- modified_lmks.push_back(pl_lmk);
- }
- }
- assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
-
- if (print)
- {
- std::cout << "MODIFIED LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- }
- }
- else
- {
- // add recently created landmarks to the list of modified landmark
- if (pl_lmk->getHits() == 0)
- modified_lmks.push_back(pl_lmk);
-
- assert((pl_lmk->getHits() != 0 || pl_match->feature_from_id_ == 0) && "recently created landmark with not full feature match");
- assert((pl_lmk->getHits() != 0 || pl_match->feature_to_id_ == pl_ftr->getNPoints()-1) && "recently created landmark with not full feature match");
- assert((!pl_lmk->isClosed() || pl_match->feature_from_id_ == 0) && "closed landmark with not full feature match");
- assert((!pl_lmk->isClosed() || pl_match->feature_to_id_ == pl_ftr->getNPoints()-1) && "closed landmark with not full feature match");
- }
-
- // ESTABLISH CONSTRAINTS ------------------------------------------------------------------------
- assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + pl_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly");
- //std::cout << "Establishing factor between" << std::endl;
- //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is liked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
- //std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
- //std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl;
-
- // Factors for all feature points
- int lmk_point_id = pl_match->landmark_from_id_;
-
- for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
- {
- //std::cout << "feature point " << ftr_point_id << std::endl;
- //std::cout << "landmark point " << lmk_point_id << std::endl;
-
- // First not defined point
- if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
- // first point to line factor
- {
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
- assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
- N_factors++;
- //std::cout << "factor added" << std::endl;
- }
-
- // Last not defined point
- else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
- // last point to line factor
- {
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
- assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
- N_factors++;
- //std::cout << "factor added" << std::endl;
- }
-
- // Defined point
- else
- // point to point factor
- {
- //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
- //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
- //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
- emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
- //std::cout << "factor added" << std::endl;
- }
-
- if (ftr_point_id < pl_ftr->getNPoints()-1)
- lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
- }
- }
- //std::cout << N_factors << " factors established, trying to close and classify " << modified_lmks.size() << std::endl;
-
- // Try to close & classify modified landmarks
- for (auto lmk_ptr : modified_lmks)
- {
- assert(!lmk_ptr->isRemoving());
-
- // try classify
- if (lmk_ptr->getClassification().type == UNCLASSIFIED && lmk_ptr->getNDefinedPoints() >= 3 && lmk_ptr->getNDefinedPoints() <= 4)
- lmk_ptr->tryClassify(params_->class_position_error_th, params_->polyline_classes);
- // try close
- if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 3 || lmk_ptr->getNPoints() >= 4))
- lmk_ptr->tryClose(params_->class_position_error_th);
- }
-
- // Outlier rejection
- FactorBasePtrList new_fac_list;
- last_ptr_->getFactorList(new_fac_list);
- for (auto fac : new_fac_list)
- rejectOutlier(fac);
-
- //std::cout << "ProcessorPolyline::establishFactors: done\n";
-}
-
-void ProcessorPolyline::classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list)
-{
- //std::cout << "ProcessorPolyline::classifyPolylineLandmarks: " << _lmk_list.size() << std::endl;
-
- LandmarkPolyline2DPtr lmk_polyline_ptr;
- for (auto lmk_ptr : _lmk_list)
- {
- if (lmk_ptr->isRemoving())
- continue;
-
- //std::cout << "landmark " << lmk_ptr->id() << " of type " << lmk_ptr->getType() << std::endl;
- if ( (lmk_polyline_ptr = std::dynamic_pointer_cast<LandmarkPolyline2D>(lmk_ptr)) &&
- lmk_polyline_ptr->getClassification().type == UNCLASSIFIED &&
- lmk_polyline_ptr->getNDefinedPoints() > 2 && lmk_polyline_ptr->getNDefinedPoints() < 5)
- lmk_polyline_ptr->tryClassify(params_->class_position_error_th, params_->polyline_classes);
- }
- //std::cout << "ProcessorPolyline::classifyPolylineLandmarks: done\n";
-}
-
-
-
-LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr)
-{
- // updated expected feature
- Eigen::MatrixXs exp_feat, exp_cov;
- expectedFeature(_lmk_ptr, exp_feat, exp_cov);
-
- // try match
- return tryMatch(exp_feat,_lmk_ptr, _feat_ptr);
-}
-
-LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
-{
- LandmarkMatchPolyline2DPtr lmk_match = nullptr;
-
- // compute best sq distance matching
- MatchPolyline2DPtr match = computeBestSqDist(_lmk_expected_feature_points, // <--landmark points in local coordinates
- _lmk_ptr->isFirstDefined(),
- _lmk_ptr->isLastDefined(),
- _lmk_ptr->isClosed(),
- _feat_ptr->getPoints(), // <--feature points
- _feat_ptr->isFirstDefined(),
- _feat_ptr->isLastDefined(),
- false, // <--feature is not closed for sure
- params_->position_error_th_*params_->position_error_th_);
- // valid match
- if (match)
- {
- lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
- lmk_match->landmark_ptr_=_lmk_ptr;
- lmk_match->landmark_version_=_lmk_ptr->getVersion();
- // landmark point id's
- std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
- lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_];
- lmk_match->landmark_to_id_ = lmk_valid_ids[match->to_A_id_];
- // feature point id's
- lmk_match->feature_from_id_ = match->from_B_id_;
- lmk_match->feature_to_id_ = match->to_B_id_;
-
- assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
- assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
- }
-
- return lmk_match;
-}
-
-bool ProcessorPolyline::rejectOutlier(FactorBasePtr& fac_ptr)
-{
- // Cast Point2Line/Point2Point factor
- auto fac_p2l_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
- auto fac_p2p_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
-
- // Cast feature and landmark
- auto pl_ftr_ptr = std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature());
- auto pl_lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(fac_ptr->getLandmarkOther());
-
- // copy states
- Eigen::VectorXs frameP(pl_ftr_ptr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXs frameO(pl_ftr_ptr->getCapture()->getFrame()->getO()->getState());
- Eigen::VectorXs lmkOriginP(pl_lmk_ptr->getP()->getState());
- Eigen::VectorXs lmkOriginO(pl_lmk_ptr->getO()->getState());
- Eigen::VectorXs lmkPoint(pl_lmk_ptr->getPointVector((fac_p2l_ptr ? fac_p2l_ptr->getLandmarkPointId() : fac_p2p_ptr->getLandmarkPointId())));
- Eigen::VectorXs lmkPointAux((fac_p2l_ptr ? pl_lmk_ptr->getPointVector(fac_p2l_ptr->getLandmarkPointAuxId()) : Eigen::VectorXs::Zero(1)));
-
- // create Scalar* array of a copy of the state
- Scalar* parameters[6] = {frameP.data(),
- frameO.data(),
- lmkOriginP.data(),
- lmkOriginO.data(),
- lmkPoint.data(),
- lmkPointAux.data()}; // last wont be used in p2p evaluate()
-
- // create residuals pointer
- Eigen::VectorXs residuals(fac_p2l_ptr ? 1 : 2);
-
- // evaluate the factor in this state (without jacobians, no need)
- fac_ptr->evaluate(parameters, residuals.data(), nullptr);
-
- // discard if residual too high evaluated at the current estimation
- if (residuals.norm() > 1e3)
- {
- WOLF_WARN((fac_p2l_ptr ? "Discarding Point 2 Line Polyline Factor, considered OUTLIER" : "Discarding Point 2 Point Polyline Factor, considered OUTLIER"));
- fac_ptr->remove();
- return true;
- }
- return false;
-}
-
-// GETS & SETS ////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorPolyline::setKeyFrame(CaptureBasePtr _capture_ptr)
-{
-
- assert(_capture_ptr != nullptr && _capture_ptr->getFrame() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame");
- if (!_capture_ptr->getFrame()->isKey())
- {
- // Set key
- _capture_ptr->getFrame()->setKey();
- // Set state to the keyframe
- _capture_ptr->getFrame()->setState(getProblem()->getState(_capture_ptr->getTimeStamp()));
- // Call the new keyframe callback in order to let the other processors to establish their factors
- getProblem()->keyFrameCallback(_capture_ptr->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_->time_tolerance);
- }
-}
-
-// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorPolyline::computeTransformations(const TimeStamp& _ts)
-{
- Eigen::Vector3s vehicle_pose = getProblem()->getState(_ts);
- t_world_robot_ = vehicle_pose.head<2>();
-
- // world_robot
- Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->isExtrinsicDynamic() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_ = R_world_robot * R_robot_sensor_;
- t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_ = R_robot_sensor_.transpose() * R_world_robot.transpose();
- t_sensor_world_ = -R_robot_sensor_.transpose() * t_robot_sensor_ -R_sensor_world_ * t_world_robot_ ;
-
- //std::cout << "t_world_robot_ " << t_world_robot_.transpose() << std::endl;
- //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
- //std::cout << "t_world_sensor_ " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_world_sensor_ " << std::endl << R_world_sensor_ << std::endl;
- //std::cout << "t_sensor_world_ " << t_sensor_world_.transpose() << std::endl;
- //std::cout << "R_sensor_world_ " << std::endl << R_sensor_world_ << std::endl;
-
-}
-
-void ProcessorPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_ftr_, Eigen::MatrixXs& expected_ftr_cov_)
-{
- assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorPolyline::expectedFeature: Bad landmark type");
- LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
-
- //std::cout << "ProcessorPolyline::expectedFeature" << std::endl;
- //std::cout << "t_world_sensor_: " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_sensor_world_: " << std::endl << R_sensor_world_ << std::endl;
-
- expected_ftr_ = pl_lmk->computePointsGlobal();
- expected_ftr_.topRows(2) = R_sensor_world_ * (expected_ftr_.topRows(2).colwise() - t_world_sensor_);
- expected_ftr_cov_ = pl_lmk->computePointsCovGlobal();
- // TODO Rotate covariances
-}
-
-ProcessorBasePtr ProcessorPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
- ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params);
- ProcessorPolylinePtr prc_ptr = std::make_shared<ProcessorPolyline>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} /* namespace wolf */
-
-// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorPolyline)
-} /* namespace wolf */
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
deleted file mode 100644
index ec39aa406a4b930d1c03c5ac99ab2320eaf53a8d..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ /dev/null
@@ -1,1140 +0,0 @@
-/*
- * processor_tracker_feature_polyline_2D.cpp
- *
- * Created on: Mar 11, 2019
- * Author: jvallve
- */
-
-#include "laser/processor/processor_tracker_feature_polyline_2D.h"
-
-namespace wolf
-{
-
-ProcessorTrackerFeaturePolyline2D::ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params) :
- ProcessorTrackerFeature("TRACKER FEATURE POLYLINE",_params),
- params_tracker_feature_polyline_(_params),
- line_finder_(_params->line_finder_params),
- extrinsics_transformation_computed_(false)
-{
-
-}
-
-ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D()
-{
-}
-
-unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
-{
- WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_last_in.size());
-
- if (_features_last_in.empty())
- return 0;
-
- // prior transformations
- Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity());
- T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_;
- T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
-
- // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: all_features_incoming_)
- auto ftr_it = all_features_incoming_.begin();
-
- // iterate over all polylines features
- while (ftr_it != all_features_incoming_.end())
- {
- bool matched = false;
- auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- FeatureMatchPolyline2DScalarMap best_matches;
-
- std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n";
-
- // Check matching with all features in last
- // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior
- for (auto ftr_last_ : _features_last_in)
- tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior);
-
- std::cout << "\t" << best_matches.size() << " matches with features last found\n";
-
- // Try the match for all candidates (sorted by difference from movement prior)
- for (auto best_ftr_match_pair : best_matches)
- {
- auto best_ftr_match = best_ftr_match_pair.second;
- auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(best_ftr_match->feature_ptr_);
-
- std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << "\n";
-
- // If it has not been created in processNew: check/update/remove landmark match
- if(find(new_features_last_.begin(), new_features_last_.end(), pl_last) == new_features_last_.end())
- {
- // the match with this last_ feature was removed after any updateMatchWithLandmark
- if(landmark_match_map_.find(pl_last) == landmark_match_map_.end())
- {
- std::cout << "\t\tremoved feature last\n";
- continue;
- }
-
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
-
- // Landmark changed or was merged -> Redo match with last feature
- if (landmark_match_map_[pl_last]->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
- {
- std::cout << "\t\tupdating match...\n";
- if (!updateMatchWithLandmark(landmark_match_map_[pl_last],pl_lmk,pl_last))
- {
- // not successful update -> remove match and feature
- std::cout << "\t\t\tNot success: removing feature " << pl_last->id() << " and landmark match\n";
- landmark_match_map_.erase(pl_last);
- last_ptr_->getFeatureList().remove(pl_last);
- continue;
- }
- }
- // removed -> not valid match with landmark
- else if (pl_lmk->isRemoving())
- continue;
-
- // try to concatenate landmark match
- auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
- best_ftr_match,
- landmark_match_map_[pl_last]);
- // Add the incoming match to landmark_match_map
- if (pl_lmk_match_incoming != nullptr)
- landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
- else
- {
- WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming track lost common points with landmark");
- continue;
- }
- }
-
- // TRACK valid
- std::cout << "\t\tvalid match, storing...\n";
- // add best match to match_map _feature_correspondences[feature_out_ptr] = feature_in_ptr
- _feature_correspondences[pl_incoming] = best_ftr_match;
-
- // add feature to list of tracked features
- _features_incoming_out.push_back(pl_incoming);
-
- // match for this feature has been found
- matched = true;
- std::cout << "\t\tmatch found\n";
- break;
- }
-
- // next feature
- if (matched)
- ftr_it = all_features_incoming_.erase(ftr_it); // remove from all_features_incoming
- else
- ftr_it++;
- }
-
- WOLF_DEBUG(_feature_correspondences.size(), " features tracked");
- return _feature_correspondences.size();
-}
-
-bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
-{
- //WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
- // If incoming observed new landmark points & landmark has points
- // Check if match is still valid and update the match
- // TODO
-
-// int& new_first_points = best_ftr_match->feature_incoming_from_id_;
-// int new_last_points = pl_incoming->getNPoints() - best_ftr_match->feature_incoming_to_id_ - 1;
-// int overlapping = best_ftr_match->feature_incoming_to_id_ - best_ftr_match->feature_incoming_from_id_;
-
- return true;
-}
-
-bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame()
-{
- //WOLF_DEBUG("PTFP ", getName(), "::voteForKeyFrame: ");
- // TODO
- /* IDEAS:
- * - Any feature incoming derived too much from projected Landmark
- * - Any feature incoming overlapping with Landmark less than param (1 or 2) defined points
- * - Any feature last would define extreme or add points to the Landmark
- * - Some (more than param) tracks lost
- */
- return false;
-}
-
-unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _max_features)
-{
- WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
- unsigned int n = ProcessorTrackerFeature::processNew(_max_features);
-
- WOLF_DEBUG("Processing ", n, " new last features");
-
- // prior transformations
- Eigen::Matrix3s T_sensor_world_last(Eigen::Matrix3s::Identity());
- T_sensor_world_last.topLeftCorner(2,2) = R_sensor_world_last_;
- T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_;
-
- // For each new feature: Either create a landmark or match with an existent landmark
- LandmarkBasePtrList new_landmarks;
- for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n);
- ftr_it != last_ptr_->getFeatureList().end();
- ftr_it++)
- {
- auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- LandmarkMatchPolyline2DScalarMap best_lmk_matches;
- WOLF_DEBUG("Processing feature: ", pl_ftr->id());
-
- // Check matching with all existing polyline landmarks
- for (auto lmk : getProblem()->getMap()->getLandmarkList())
- // Check for polylines landmarks that hasn't been just created
- if (lmk->getType() == "POLYLINE 2D" && std::find(new_landmarks.begin(), new_landmarks.end(), lmk) == new_landmarks.end())
- // Store all matches consistent with T_sensor_world_last in best_lmk_matches sorted by difference from T_sensor_world_last
- tryMatchWithLandmark(best_lmk_matches, std::static_pointer_cast<LandmarkPolyline2D>(lmk), pl_ftr, T_sensor_world_last);
-
- // If not matched with any existing landmark: create a new one
- if (best_lmk_matches.empty())
- {
- // create a landmark
- auto new_lmk_ptr = createLandmark(pl_ftr);
-
- // Add a new match to landmark_match_map_
- auto new_lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
- new_lmk_match->landmark_ptr_ = new_lmk_ptr;
- new_lmk_match->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(new_lmk_ptr)->getVersion();
- new_lmk_match->feature_from_id_= 0; // all points match
- new_lmk_match->landmark_from_id_ = 0; // all points match
- new_lmk_match->feature_to_id_= pl_ftr->getNPoints()-1; // all points match
- new_lmk_match->landmark_to_id_ = pl_ftr->getNPoints()-1; // all points match
- new_lmk_match->normalized_score_ = 1.0; // max score
- new_lmk_match->T_feature_landmark_ = Eigen::Matrix3s::Identity();
- new_lmk_match->T_feature_landmark_.topLeftCorner(2,2) = R_sensor_world_last_;
- new_lmk_match->T_feature_landmark_.topRightCorner(2,1) = t_sensor_world_last_;
-
- // Add the new landmark to the map
- getProblem()->addLandmark(new_lmk_ptr);
- // Store in new_landmarks
- new_landmarks.push_back(new_lmk_ptr);
- // store match in map
- landmark_match_map_[pl_ftr] = new_lmk_match;
- }
- // If matched
- else
- {
- // store best match (lowest difference from prior) in map
- landmark_match_map_[pl_ftr] = best_lmk_matches.begin()->second;
-
- // store landmark candidates to be merged
- if (best_lmk_matches.size() > 1)
- {
- LandmarkPolyline2DPtrList merge_candidates;
- for (auto lmk_match_pair_it : best_lmk_matches)
- merge_candidates.push_back(std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_pair_it.second->landmark_ptr_));
- merge_candidates_list_.push_back(merge_candidates);
- }
- }
- }
-
- // store in landmark_match_map_ the incoming features
- auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n);
- while (ftr_it != incoming_ptr_->getFeatureList().end())
- {
- auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- assert(matches_last_from_incoming_.find(pl_incoming) != matches_last_from_incoming_.end() && "last-incoming match not found");
- auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[pl_incoming]);
- assert(landmark_match_map_.find(pl_ftr_match->feature_ptr_) != landmark_match_map_.end() && "last-lmk match not found");
- auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
- pl_ftr_match,
- landmark_match_map_[pl_ftr_match->feature_ptr_]);
- if (pl_lmk_match_incoming != nullptr)
- {
- landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
- ftr_it++;
- }
- else
- {
- WOLF_DEBUG("PTFP ", getName(), "::processNew: incoming track lost common points with landmark");
- matches_last_from_incoming_.erase(pl_incoming);
- // move feature from known to all features
- all_features_incoming_.push_back(pl_incoming);
- ftr_it = incoming_ptr_->getFeatureList().erase(ftr_it);
- }
- }
- return n;
-}
-
-unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _max_new_features,
- FeatureBasePtrList& _features_last_out)
-{
- WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
-
- _features_last_out = std::move(all_features_last_);
- if (_max_new_features != -1 && _features_last_out.size() > _max_new_features)
- _features_last_out.resize(_max_new_features);
-
- WOLF_DEBUG(_features_last_out.size(), " were provided");
-
- return _features_last_out.size();
-}
-
-void ProcessorTrackerFeaturePolyline2D::establishFactors()
-{
- WOLF_DEBUG("PTFP ", getName(), "::establishFactors: ");
- unsigned int N_factors = 0;
-
- // Create a factor for each match in last features
- auto ftr_it = last_ptr_->getFeatureList().begin();
- while (ftr_it != last_ptr_->getFeatureList().end())
- {
- WOLF_DEBUG("\tLast feature: ", (*ftr_it)->id());
- assert(landmark_match_map_.find(*ftr_it) != landmark_match_map_.end() && "feature without landmark match in last features");
- auto lmk_match = landmark_match_map_[*ftr_it];
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
- auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- WOLF_DEBUG("\tLandmark: ", pl_lmk->id());
-
- // LANDMARK CHANGED: update match
- WOLF_DEBUG("\tLandmark changed?");
- if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
- {
- WOLF_DEBUG("\tLandmark changed");
- // not successful update
- if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr))
- {
- std::cout << "\t\tNot success: removing feature " << pl_ftr->id() << " and landmark match\n";
- // remove from match map
- landmark_match_map_.erase(*ftr_it);
- // remove from last feature list
- ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
- continue;
- }
- }
-
- // LANDMARK REMOVED: remove match and feature
- else
- {
- WOLF_DEBUG("\tLandmark removed?");
- if (pl_lmk->isRemoving())
- {
- std::cout << "\t\tLandmark was removed: removing feature " << pl_ftr->id() << " and landmark match\n";
- // remove from match map
- landmark_match_map_.erase(*ftr_it);
- // remove from last feature list
- ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
- continue;
- }
- }
-
- // MODIFY LANDMARK (only for not closed and not recently created)
- WOLF_DEBUG("\tModifying..");
- if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
- // If modified, add lmk to modified_lmks_ list
- if (modifyLandmarkAndMatch(lmk_match, pl_ftr))
- modified_lmks_.push_back(pl_lmk);
-
- // ESTABLISH CONSTRAINTS
- WOLF_DEBUG("\tEstablishing factors..");
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
-
- std::cout << "Establishing factor between" << std::endl;
- std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
- std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
- std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
-
- // Factors for all feature points
- int lmk_point_id = lmk_match->landmark_from_id_;
- for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
- {
- std::cout << "feature point " << ftr_point_id << std::endl;
- std::cout << "landmark point " << lmk_point_id << std::endl;
-
- // First not defined point
- if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
- // first point to line factor
- {
- std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
- assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
- N_factors++;
- std::cout << "factor added" << std::endl;
- }
-
- // Last not defined point
- else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
- // last point to line factor
- {
- std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl;
- assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
-
- // emplace factor
- emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
- N_factors++;
- std::cout << "factor added" << std::endl;
- }
-
- // Defined point
- else
- // point to point factor
- {
- std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
- //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
- //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
- emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
- N_factors++;
- std::cout << "factor added" << std::endl;
- }
-
- // Next lmk point
- if (ftr_point_id < pl_ftr->getNPoints()-1)
- lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
- }
- // next ftr
- ftr_it++;
- }
- std::cout << N_factors << " factors established" << std::endl;
-}
-
-void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id)
-{
- assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
-
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id)
-{
- // CREATE CONSTRAINT --------------------
- FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
-
- // ADD CONSTRAINT --------------------
- _polyline_feature->addFactor(new_fac);
- _polyline_landmark->addConstrainedBy(new_fac);
-}
-
-LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr)
-{
- WOLF_DEBUG("PTFP ", getName(), "::createLandmark: ");
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
- assert(_feature_ptr->getType() == "POLYLINE 2D");
- assert(std::find(last_ptr_->getFeatureList().begin(),last_ptr_->getFeatureList().end(),_feature_ptr) != last_ptr_->getFeatureList().end() && "creating a landmark for a feature which is not in last capture");
-
-
- FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
- // compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_last_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_last_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
- // std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- // std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
- // Create new landmark
- return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true),
- std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true),
- points_global,
- polyline_ptr->isFirstDefined(),
- polyline_ptr->isLastDefined());
- //std::cout << "done" << std::endl;
-}
-
-bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
-{
- WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
-
- bool print = true;
- bool modified = false;
-
- if (print)
- {
- std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
- }
- //Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
- // t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
- Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
-
- // MODIFY LANDMARK
- // -----------------GROW Front-----------------
- // Add new front points (if any feature point not matched)
- if ( lmk_match->feature_from_id_ > 0 ) // && !pl_lmk->isClosed()
- {
- assert(!pl_lmk->isClosed() && "feature has not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add new front points. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- lmk_match->feature_from_id_-1, // last feature point index to be added
- pl_ftr->isFirstDefined(), // is defined
- false); // front
-
- lmk_match->landmark_from_id_ = pl_lmk->getFirstId();
- lmk_match->feature_from_id_ = 0;
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- modified = true;
- if (print)
- {
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
-
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- // -----------------CHANGE Front-----------------
- // Change first not defined point if landmark first not defined matched with first feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
- {
- if (print)
- {
- std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isFirstDefined() ||
- // case b
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))).squaredNorm() >
- (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm())
- {
- pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
- modified = true;
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- }
-
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- // -----------------GROW Back-----------------
- // Add new back points (if any feature point not matched)
- if (lmk_match->feature_to_id_ < pl_ftr->getNPoints()-1)
- {
- assert(!pl_lmk->isClosed() && "feature not matched points in a closed landmark");
- if (print)
- {
- std::cout << "Add back points. Defined: " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
- pl_lmk->addPoints(points_global, // points matrix in global coordinates
- lmk_match->feature_to_id_+1, // last feature point index to be added
- pl_ftr->isLastDefined(), // is defined
- true); // back
-
- lmk_match->landmark_to_id_ = pl_lmk->getLastId();
- lmk_match->feature_to_id_ = pl_ftr->getNPoints()-1;
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- modified = true;
- if (print)
- {
- std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
- std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
- std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
- std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
- }
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- // -----------------CHANGE Back-----------------
- // Change last not defined point if landmark last not defined matched with last feature point that:
- // a. is defined
- // b. would extend the line (check if angle is bigger than 90º)
- else if (lmk_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
- {
- if (print)
- {
- std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
- std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
- std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
- }
- if (// case a
- pl_ftr->isLastDefined() ||
- // case b
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))).squaredNorm() >
- (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm() +
- (pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm())
- {
- pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined());
- lmk_match->landmark_version_ = pl_lmk->getVersion();
- modified = true;
- }
-
- // checks
- assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
- assert(lmk_match->check() && "Landmark didn't grow properly");
- }
- assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
-
- if (print)
- {
- std::cout << "MODIFIED LANDMARK" << std::endl;
- std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
- std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
- std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
- std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- }
-
- return modified;
-}
-
-void ProcessorTrackerFeaturePolyline2D::advanceDerived()
-{
- WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: ");
- // remove last features from landmark_match_map_
- if (last_ptr_ != nullptr)
- for (auto ftr : last_ptr_->getFeatureList())
- {
- assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
- landmark_match_map_.erase(ftr);
- }
-
- WOLF_DEBUG("removing ", all_features_last_.size() , " features in all_features_last_");
- all_features_last_.clear();
- all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
- if (last_ptr_)
- WOLF_DEBUG("all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
-
- ProcessorTrackerFeature::advanceDerived();
-}
-
-void ProcessorTrackerFeaturePolyline2D::resetDerived()
-{
- WOLF_DEBUG("PTFP ", getName(), "::resetDerived: ");
- // remove last features from landmark_match_map_
- if (last_ptr_ != nullptr)
- for (auto ftr : last_ptr_->getFeatureList())
- {
- assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
- landmark_match_map_.erase(ftr);
- }
-
- WOLF_DEBUG("PTF ", getName(), ": ", "removing ", all_features_last_.size() , " features in all_features_last_");
- all_features_last_.clear();
- all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
- WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
- if (last_ptr_)
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
-
- ProcessorTrackerFeature::resetDerived();
-}
-
-void ProcessorTrackerFeaturePolyline2D::preProcess()
-{
- WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: ");
- // Extract all polylines from incoming_
- // TODO: sort corners by bearing
- std::list<laserscanutils::Polyline> polylines;
-
- line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
- WOLF_DEBUG("PTFP ", getName(), " Polylines extracted: ", polylines.size());
-
- for (auto&& pl : polylines)
- {
- //WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ );
- //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
- all_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
- //WOLF_DEBUG("new polyline detected: ");
- }
-
- // compute transformations
- if (last_ptr_ != nullptr && incoming_ptr_ != nullptr)
- computeTransformations();
-
- WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
- if (last_ptr_)
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
-}
-
-void ProcessorTrackerFeaturePolyline2D::postProcess()
-{
- WOLF_DEBUG("PTFP ", getName(), "::postProcess: ");
- // Try to close & classify modified landmarks
- while (!modified_lmks_.empty())
- {
- auto lmk_ptr = modified_lmks_.front();
- modified_lmks_.pop_front();
-
- assert(!lmk_ptr->isRemoving());
-
- // try classify
- if (lmk_ptr->getClassification().type == UNCLASSIFIED && lmk_ptr->getNDefinedPoints() >= 3 && lmk_ptr->getNDefinedPoints() <= 4)
- lmk_ptr->tryClassify(params_tracker_feature_polyline_->class_position_error_th, params_tracker_feature_polyline_->polyline_classes);
- // try close
- if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 3 || lmk_ptr->getNPoints() >= 4))
- lmk_ptr->tryClose(params_tracker_feature_polyline_->class_position_error_th);
- }
-
- // Try to merge landmarks
- while (!merge_candidates_list_.empty())
- {
- // load next list of candidates
- LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front());
- merge_candidates_list_.pop_front();
-
- // change already merged lmks with the corresponding remaining lmk
- for (auto&& lmk : merge_candidates)
- if (lmk->getMergedInLandmark() != nullptr)
- lmk = lmk->getMergedInLandmark();
-
- // remove repeated lmks
- merge_candidates.sort();
- merge_candidates.unique();
-
- if (merge_candidates.size() == 1)
- continue;
-
- // Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
- LandmarkPolyline2D::tryMergeLandmarks(merge_candidates, params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
- }
- WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
- if (last_ptr_)
- WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
-}
-
-void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
- const FeaturePolyline2DPtr& _ftr_L,
- const FeaturePolyline2DPtr& _ftr_I,
- const Eigen::Matrix3s& _T_last_incoming_prior) const
-{
- //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: ");
-
- // compute best sq distance matching
- Eigen::MatrixXs last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
- MatchPolyline2DPtr best_match = computeBestSqDist(_ftr_I->getPoints(), // <--feature incoming points
- _ftr_I->isFirstDefined(),
- _ftr_I->isLastDefined(),
- false, // <--feature is not closed for sure
- last_expected_points, // <--feature last points
- _ftr_L->isFirstDefined(),
- _ftr_L->isLastDefined(),
- false, // <--feature is not closed for sure
- params_tracker_feature_polyline_->match_feature_pose_sq_norm_th);
- //valid match
- if (best_match != nullptr)
- {
-
- auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
- // feature incoming point id's
- ftr_match->feature_incoming_from_id_ = best_match->from_A_id_;
- ftr_match->feature_incoming_to_id_ = best_match->to_A_id_;
- // feature last point id's
- ftr_match->feature_last_from_id_ = best_match->from_B_id_;
- ftr_match->feature_last_to_id_ = best_match->to_B_id_;
- // incoming last transformation
- bool from_defined = (ftr_match->feature_incoming_from_id_ != 0 || _ftr_I->isFirstDefined()) &&
- (ftr_match->feature_last_from_id_ != 0 || _ftr_L->isFirstDefined());
- bool to_defined = (ftr_match->feature_incoming_to_id_ != _ftr_I->getNPoints()-1 || _ftr_I->isLastDefined()) &&
- (ftr_match->feature_last_to_id_ != _ftr_L->getNPoints()-1 || _ftr_L->isLastDefined());
- int N_points = ftr_match->feature_incoming_to_id_ - ftr_match->feature_incoming_from_id_;
- int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1);
- if (N_def_points > 0 && N_points > 2)
- ftr_match->T_incoming_last_ = pose2T2D(computeRigidTrans(_ftr_I->getPoints().middleCols(ftr_match->feature_incoming_from_id_,N_points),
- _ftr_L->getPoints().middleCols(ftr_match->feature_last_from_id_,N_points),
- from_defined,
- to_defined));
- else
- ftr_match->T_incoming_last_ = _T_last_incoming_prior;
- // score
- ftr_match->normalized_score_ = best_match->normalized_score_;
- // feature correspondence
- ftr_match->feature_ptr_ = _ftr_L;
- // store in list
- ftr_matches[ftr_match->normalized_score_] = ftr_match;
- WOLF_DEBUG("match stored!");
- }
-
-// WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose());
-// WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose());
-// // compute best rigid transformations
-// MatchPolyline2DPtrList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points
-// _ftr_I->isFirstDefined(),
-// _ftr_I->isLastDefined(),
-// false, // <--feature is not closed for sure
-// _ftr_L->getPoints(), // <--feature last points
-// _ftr_L->isFirstDefined(),
-// _ftr_L->isLastDefined(),
-// false, // <--feature is not closed for sure
-// params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
-// // valid match
-// for (auto match : matches)
-// {
-// WOLF_DEBUG("match with pose ", T2pose2D(match->T_A_B_).transpose());
-// Scalar sq_norm = T2pose2D(_T_last_incoming_prior * match->T_A_B_).squaredNorm();
-//
-// if (sq_norm > params_tracker_feature_polyline_->match_feature_pose_sq_norm_th)
-// {
-// WOLF_DEBUG("match with squared norm ", sq_norm, " discarded");
-// continue;
-// }
-// WOLF_DEBUG("match with squared norm ", sq_norm, " stored!");
-//
-// auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
-// // feature incoming point id's
-// ftr_match->feature_incoming_from_id_ = match->from_A_id_;
-// ftr_match->feature_incoming_to_id_ = match->to_A_id_;
-// // feature last point id's
-// ftr_match->feature_last_from_id_ = match->from_B_id_;
-// ftr_match->feature_last_to_id_ = match->to_B_id_;
-// // incoming last transformation
-// ftr_match->T_incoming_last_ = match->T_A_B_;
-// // score
-// ftr_match->normalized_score_ = match->normalized_score_;
-// // feature correspondence
-// ftr_match->feature_ptr_ = _ftr_L;
-// // store in list
-// ftr_matches[sq_norm] = ftr_match;
-// }
-}
-
-void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const
-{
- //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
-
- // compute best sq distance matching
- Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
- MatchPolyline2DPtr best_match = computeBestSqDist(lmk_expected_points, // <--landmark points in local coordinates
- _lmk_ptr->isFirstDefined(),
- _lmk_ptr->isLastDefined(),
- _lmk_ptr->isClosed(),
- _feat_ptr->getPoints(), // <--feature points
- _feat_ptr->isFirstDefined(),
- _feat_ptr->isLastDefined(),
- false, // <--feature is not closed for sure
- params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
- //valid match
- if (best_match != nullptr)
- {
- auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
-
- // landmark
- lmk_match->landmark_ptr_=_lmk_ptr;
- lmk_match->landmark_version_=_lmk_ptr->getVersion();
- // landmark point id's
- std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
- lmk_match->landmark_from_id_ = lmk_valid_ids[best_match->from_A_id_];
- lmk_match->landmark_to_id_ = lmk_valid_ids[best_match->to_A_id_];
- // feature point id's
- lmk_match->feature_from_id_ = best_match->from_B_id_;
- lmk_match->feature_to_id_ = best_match->to_B_id_;
-
- // feature landmark transformation
- lmk_match->T_feature_landmark_ = best_match->T_A_B_.inverse().eval();
-
- bool from_defined = (lmk_match->landmark_from_id_ != _lmk_ptr->getFirstId() || _lmk_ptr->isFirstDefined()) &&
- (lmk_match->feature_from_id_ != 0 || _feat_ptr->isFirstDefined());
- bool to_defined = (lmk_match->landmark_to_id_ != _lmk_ptr->getLastId() || _lmk_ptr->isLastDefined()) &&
- (lmk_match->feature_to_id_ != _feat_ptr->getNPoints()-1 || _feat_ptr->isLastDefined());
- int N_points = lmk_match->feature_to_id_ - lmk_match->feature_from_id_;
- int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1);
- if (N_def_points > 0 && N_points > 2)
- lmk_match->T_feature_landmark_ = pose2T2D(computeRigidTrans(_feat_ptr->getPoints().middleCols(lmk_match->feature_from_id_,N_points),
- _lmk_ptr->computePointsGlobal().middleCols(lmk_match->landmark_from_id_,N_points),
- from_defined,
- to_defined));
- else
- lmk_match->T_feature_landmark_ = _T_feature_landmark_prior;
- // score
- lmk_match->normalized_score_ = best_match->normalized_score_;
-
- assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
- assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
-
- // store in list
- lmk_matches[lmk_match->normalized_score_]= lmk_match;
- WOLF_DEBUG("match stored!");
- assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
- }
-
-// // compute best rigid transformation matching
-// MatchPolyline2DPtrList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates
-// _lmk_ptr->isFirstDefined(),
-// _lmk_ptr->isLastDefined(),
-// _lmk_ptr->isClosed(),
-// _feat_ptr->getPoints(), // <--feature points
-// _feat_ptr->isFirstDefined(),
-// _feat_ptr->isLastDefined(),
-// false, // <--feature is not closed for sure
-// params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
-// // valid match
-// for (auto match : matches)
-// {
-// Scalar sq_norm = T2pose2D(_T_feature_landmark_prior * match->T_A_B_).squaredNorm();
-//
-// if (sq_norm > params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th)
-// continue;
-//
-// auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
-//
-// // landmark
-// lmk_match->landmark_ptr_=_lmk_ptr;
-// lmk_match->landmark_version_=_lmk_ptr->getVersion();
-// // landmark point id's
-// std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
-// lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_];
-// lmk_match->landmark_to_id_ = lmk_valid_ids[match->to_A_id_];
-// // feature point id's
-// lmk_match->feature_from_id_ = match->from_B_id_;
-// lmk_match->feature_to_id_ = match->to_B_id_;
-//
-// // incoming last transformation
-// lmk_match->T_feature_landmark_ = match->T_A_B_.inverse().eval();
-// // score
-// lmk_match->normalized_score_ = match->normalized_score_;
-//
-// assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
-// assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
-//
-// // store in list
-// lmk_matches[sq_norm]= lmk_match;
-// }
-}
-
-bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match,
- LandmarkPolyline2DPtr& _lmk_ptr,
- FeaturePolyline2DPtr& _feat_ptr) const
-{
- assert(_lmk_match->check() && "updateMatchWithLandmark: input _lmk_match wrong");
- WOLF_DEBUG("PTFP ", getName(), "::updateMatchWithLandmark: ");
- assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id());
-
- // merged: update the pl_lmk pointer
- if (_lmk_ptr->getMergedInLandmark() != nullptr)
- _lmk_ptr = _lmk_ptr->getMergedInLandmark();
-
- // try match with _feat_ptr again
- LandmarkMatchPolyline2DScalarMap new_lmk_matches;
- tryMatchWithLandmark(new_lmk_matches,_lmk_ptr,_feat_ptr, _lmk_match->T_feature_landmark_);
-
- // not valid match
- if (new_lmk_matches.empty())
- {
- _lmk_match = nullptr;
- return false;
- }
-
- // valid match: update landmark match of last
- assert(_lmk_match->check() && "updateMatchWithLandmark: wrongly updated");
- _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match
- return true;
-}
-
-LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
- FeatureMatchPolyline2DPtr ftr_match,
- LandmarkMatchPolyline2DPtr lmk_match_last) const
-{
- assert(lmk_match_last->check() && "input lmk_match_last wrong");
- WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: ");
- std::cout << "ftr_match:";
- std::cout << "\n\tincoming_from: " << ftr_match->feature_incoming_from_id_;
- std::cout << "\n\tincoming_to: " << ftr_match->feature_incoming_to_id_;
- std::cout << "\n\tlast_from: " << ftr_match->feature_last_from_id_;
- std::cout << "\n\tlast_to: " << ftr_match->feature_last_to_id_;
- std::cout << "\nlmk_match_last:";
- std::cout << "\n\tlast_from: " << lmk_match_last->feature_from_id_;
- std::cout << "\n\tlast_to: " << lmk_match_last->feature_to_id_;
- std::cout << "\n\tlmk_from: " << lmk_match_last->landmark_from_id_;
- std::cout << "\n\tlmk_to: " << lmk_match_last->landmark_to_id_;
-
- auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
- auto lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_);
-
- // feature correspondence from&to
- lmk_match_incoming->feature_from_id_ = ftr_match->feature_incoming_from_id_;
- lmk_match_incoming->feature_to_id_ = ftr_match->feature_incoming_to_id_;
-
- // landmark correspondence from&to
- // Not adding new points nor checks (done in correctFeatureDrift)
- int from_offset = ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
- int to_offset = ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_;
- std::cout << "\nfrom_offset: " << from_offset;
- std::cout << "\nto_offset: " << from_offset;
- lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_;
- lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_;
- while (from_offset > 0)
- {
- if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
- return nullptr;
- lmk_match_incoming->landmark_from_id_ = lmk_ptr->getNextValidId(lmk_match_incoming->landmark_from_id_);
- from_offset--;
- }
- while (from_offset < 0)
- {
- lmk_match_incoming->feature_from_id_++;
- from_offset++;
- }
- while (to_offset < 0)
- {
- if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
- return nullptr;
- lmk_match_incoming->landmark_to_id_ = lmk_ptr->getPrevValidId(lmk_match_incoming->landmark_to_id_);
- to_offset++;
- }
- while (to_offset > 0)
- {
- lmk_match_incoming->feature_to_id_--;
- to_offset--;
- }
- std::cout << "\nlmk_match_incoming:";
- std::cout << "\n\tincoming_from: " << lmk_match_incoming->feature_from_id_;
- std::cout << "\n\tincoming_to: " << lmk_match_incoming->feature_to_id_;
- std::cout << "\n\tlmk_from: " << lmk_match_incoming->landmark_from_id_;
- std::cout << "\n\tlmk_to: " << lmk_match_incoming->landmark_to_id_ << std::endl;
-
- // other match attributes
- lmk_match_incoming->landmark_ptr_ = lmk_ptr;
- lmk_match_incoming->landmark_version_ = lmk_ptr->getVersion();
- lmk_match_incoming->normalized_score_ = ftr_match->normalized_score_;
- lmk_match_incoming->T_feature_landmark_ = ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
-
- assert(lmk_match_incoming->check() && "lmk_match_incoming wrongly concatenated");
-
- return lmk_match_incoming;
-}
-
-// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ProcessorTrackerFeaturePolyline2D::computeTransformations()
-{
- //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
- Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
- Eigen::Vector3s vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp());
-
- // world_robot
- Eigen::Matrix2s R_world_robot_incoming = Eigen::Rotation2Ds(vehicle_pose_incoming(2)).matrix();
- Eigen::Matrix2s R_world_robot_last = Eigen::Rotation2Ds(vehicle_pose_last(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->isExtrinsicDynamic() ||
- !getSensor()->getP()->isFixed() ||
- !getSensor()->getO()->isFixed() ||
- !extrinsics_transformation_computed_)
- {
- t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // INCOMING
- // global_sensor
- R_world_sensor_incoming_ = R_world_robot_incoming * R_robot_sensor_;
- t_world_sensor_incoming_ = vehicle_pose_incoming.head<2>() + R_world_robot_incoming * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_incoming_ = R_world_sensor_incoming_.transpose();
- t_sensor_world_incoming_ = -R_sensor_world_incoming_ * t_world_sensor_incoming_;
-
- // LAST
- // global_sensor
- R_world_sensor_last_ = R_world_robot_last * R_robot_sensor_;
- t_world_sensor_last_ = vehicle_pose_last.head<2>() + R_world_robot_last * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_last_ = R_world_sensor_last_.transpose();
- t_sensor_world_last_ = -R_sensor_world_last_ * t_world_sensor_last_;
-
- // INCOMING-LAST
- R_incoming_last_ = R_sensor_world_incoming_ * R_world_sensor_last_;
- t_incoming_last_ = t_sensor_world_incoming_ + R_sensor_world_incoming_ * t_world_sensor_last_;
-
- // last_incoming
- R_last_incoming_ = R_world_sensor_last_.transpose() * R_world_sensor_incoming_;
- t_last_incoming_ = t_sensor_world_last_ + R_sensor_world_last_ * t_world_sensor_incoming_;
-
- //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
- //std::cout << "t_world_sensor_incoming_ " << t_world_sensor_incoming_.transpose() << std::endl;
- //std::cout << "R_world_sensor_incoming_ " << std::endl << R_world_sensor_incoming_ << std::endl;
- //std::cout << "t_sensor_world_incoming_ " << t_sensor_world_incoming_.transpose() << std::endl;
- //std::cout << "R_sensor_world_incoming_ " << std::endl << R_sensor_world_incoming_ << std::endl;
- //std::cout << "t_world_sensor_last_ " << t_world_sensor_last_.transpose() << std::endl;
- //std::cout << "R_world_sensor_last_ " << std::endl << R_world_sensor_last_ << std::endl;
- //std::cout << "t_sensor_world_last_ " << t_sensor_world_last_.transpose() << std::endl;
- //std::cout << "R_sensor_world_last_ " << std::endl << R_sensor_world_last_ << std::endl;
- //std::cout << "t_incoming_last_ " << t_incoming_last_.transpose() << std::endl;
- //std::cout << "R_incoming_last_ " << std::endl << R_incoming_last_ << std::endl;
- //std::cout << "t_last_incoming_ " << t_last_incoming_.transpose() << std::endl;
- //std::cout << "R_last_incoming_ " << std::endl << R_last_incoming_ << std::endl;
-
-}
-
-ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr)
-{
- auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline2D>(_params);
- auto prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline2D>(params);
- prc_ptr->setName(_unique_name);
- prc_ptr->configure(_sensor_ptr);
- return prc_ptr;
-}
-
-} /* namespace wolf */
-
-// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D)
-} /* namespace wolf */
diff --git a/src/processor/processor_tracker_feature_polyline_2d.cpp b/src/processor/processor_tracker_feature_polyline_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be4e0ad864a4d5ec1aafd8d9b71aa6f5bf90f3a8
--- /dev/null
+++ b/src/processor/processor_tracker_feature_polyline_2d.cpp
@@ -0,0 +1,1395 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_tracker_feature_polyline_2d.cpp
+ *
+ * Created on: Mar 11, 2019
+ * Author: jvallve
+ */
+
+#include "laser/processor/processor_tracker_feature_polyline_2d.h"
+
+namespace wolf
+{
+
+ProcessorTrackerFeaturePolyline2d::ProcessorTrackerFeaturePolyline2d(ParamsProcessorTrackerFeaturePolyline2dPtr _params) :
+ ProcessorTrackerFeature("TRACKER FEATURE POLYLINE", "PO", 3, _params),
+ params_tracker_feature_polyline_(_params),
+ line_finder_(_params->line_finder_params),
+ extrinsics_transformation_computed_(false)
+{
+
+}
+
+ProcessorTrackerFeaturePolyline2d::~ProcessorTrackerFeaturePolyline2d()
+{
+}
+
+unsigned int ProcessorTrackerFeaturePolyline2d::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences)
+{
+ WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_in.size());
+
+ if (_features_in.empty())
+ return 0;
+
+ // prior transformations
+ Eigen::Matrix3d T_last_incoming_prior(Eigen::Matrix3d::Identity());
+ T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_;
+ T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
+
+ // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: untracked_features_incoming_)
+ auto ftr_it = untracked_features_incoming_.begin();
+
+ // iterate over all untracked_features_incoming_
+ while (ftr_it != untracked_features_incoming_.end())
+ {
+ bool matched = false;
+ auto pl_incoming = std::static_pointer_cast<FeaturePolyline2d>(*ftr_it);
+ FeatureMatchPolyline2dScalarMap best_matches;
+
+ // std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n";
+
+ // Check matching with all features in last
+ // Store all matches consistent with T_last_incoming_prior in best_matches sorted by normalized score
+ for (auto ftr_last_ : _features_in)
+ tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2d>(ftr_last_), pl_incoming, T_last_incoming_prior);
+
+ // std::cout << "\t" << best_matches.size() << " matches with features last found\n";
+
+ // Try the match for all candidates (sorted by normalized score, higher scores at the end!)
+ for (auto best_ftr_match_pair_it = best_matches.rbegin(); best_ftr_match_pair_it != best_matches.rend(); best_ftr_match_pair_it++ )
+ {
+ auto best_ftr_match = best_ftr_match_pair_it->second;
+ auto pl_last = std::static_pointer_cast<FeaturePolyline2d>(best_ftr_match->feature_ptr_);
+
+ //std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << " with score " << best_ftr_match->normalized_score_ << std::endl;
+
+ // VALIDATE LAST-LANDMARK --------------------------------------------------------------
+ // only if last feature not recently emplaced in processNew
+ if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last))
+ {
+ // Removed match
+ if(landmark_match_map_.count(pl_last) == 0)
+ {
+ //std::cout << "\t\tremoved feature last\n";
+ continue;
+ }
+
+ auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2d>(landmark_match_map_[pl_last]->landmark_ptr_);
+
+ // Landmark changed or merged -> update match with last feature
+ if (landmark_match_map_[pl_last]->isDeprecated())
+ {
+ //std::cout << "\t\tupdating match last-landmark...\n";
+ if (!updateLandmarkMatch(landmark_match_map_[pl_last]))
+ {
+ // not successful update -> remove match and feature
+ //std::cout << "\t\t\tNot success: removing last feature " << pl_last->id() << " and removing landmark match\n";
+ landmark_match_map_.erase(pl_last);
+ pl_last->remove();
+ continue;
+ }
+ }
+ // Landmark removed -> not valid match with landmark
+ else if (pl_lmk->isRemoving())
+ {
+ //std::cout << "\t\t\tRemoved landmark, removing last feature " << pl_last->id() << " and removing landmark match\n";
+ landmark_match_map_.erase(pl_last);
+ pl_last->remove();
+ continue;
+ }
+
+ }
+ //std::cout << "\t\tlast-landmark still valid...\n";
+
+ // INCOMING-LANDMARK --------------------------------------------------------------
+ // only if there is a landmark, so if last feature is not in new_features_last_
+ if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last))
+ {
+ // try to concatenate landmark match
+ auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(best_ftr_match,
+ landmark_match_map_[pl_last]);
+
+ // if concatenation did not succees -> next ftr match
+ if (pl_lmk_match_incoming == nullptr)
+ {
+ WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming-last + last-landmark could not be concatenated into incoming-landmark");
+ continue;
+ }
+ //std::cout << "\t\tlast-incoming valid, storing...\n";
+
+ // Add the incoming match to landmark_match_map
+ landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
+ }
+
+ // STORE INCOMING-LAST --------------------------------------------------------------
+ // add best match to feature-feature correspondences
+ _feature_correspondences[pl_incoming] = best_ftr_match;
+
+ // add feature to list of tracked features
+ _features_out.push_back(pl_incoming);
+
+ // link the incoming eature with the capture (since it was just created in preProcess())
+ pl_incoming->link(_capture);
+
+ // match for this feature has been found
+ matched = true;
+ // std::cout << "Feature tracked!\n";
+ // best_ftr_match->print();
+ break;
+ }
+
+ // next feature
+ if (matched)
+ ftr_it = untracked_features_incoming_.erase(ftr_it); // remove feature from untracked features list
+ else
+ ftr_it++;
+ }
+
+ // WOLF_DEBUG(_feature_correspondences.size(), " features tracked");
+ return _feature_correspondences.size();
+}
+
+bool ProcessorTrackerFeaturePolyline2d::correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature)
+{
+ WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
+
+ /** After finding a valid feature match last-incoming, we have to check that the concatenated match
+ * incoming-landmark is also valid.
+ *
+ * We will only have to check IN BOTH EXTREMES if:
+ * 1. There are exceeding points in incoming-last match.
+ * AND
+ * 2. Landmark has exceeding points in the same extreme OR is closed.
+ *
+ * It this is the case, we have to check that the exceeding points match with the corresponding existing landmark points.
+ */
+
+ assert(matches_last_from_incoming_.count(_incoming_feature) != 0);
+ assert(landmark_match_map_.count(_incoming_feature) != 0);
+
+ auto lmk_inc_match = std::static_pointer_cast<LandmarkMatchPolyline2d>(landmark_match_map_[_incoming_feature]);
+ auto pl_lmk = lmk_inc_match->pl_landmark_;
+ auto pl_inc = lmk_inc_match->pl_feature_;
+
+ // WOLF_DEBUG("LANDMARK-INCOMING:");
+ // lmk_inc_match->print();
+
+ assert(lmk_inc_match->check(false) && "before correct drift, check not passed");//without checking partial match
+
+ // COMPLETE PARTIAL MATCH
+ if (!tryCompletePartialMatch(lmk_inc_match))
+ {
+ WOLF_WARN("correctFeatureDrift: tryCompletePartialMatch didn't success");
+ landmark_match_map_.erase(_incoming_feature);
+ return false;
+ }
+
+ // CORRECT DRIFT
+ // feature landmark transformation
+ // WOLF_DEBUG("try update match...");
+ tryUpdateMatchTransformation(lmk_inc_match);
+
+ // RETRY LANDMARK-MATCH IF TOO MUCH ERROR
+ if ((lmk_inc_match->computeSqDistArray() >= 0.5).any())
+ {
+ WOLF_WARN("too much error: ", lmk_inc_match->computeSqDistArray().transpose(), " retrying landmark match" );
+ LandmarkMatchPolyline2dScalarMap lmk_matches;
+ tryMatchWithLandmark(lmk_matches, pl_lmk, pl_inc, lmk_inc_match->T_feature_landmark_);
+
+ // not succeed
+ if (lmk_matches.empty())
+ {
+ WOLF_WARN("correctFeatureDrift: tryMatchWithLandmark didn't success");
+ landmark_match_map_.erase(_incoming_feature);
+ return false;
+ }
+ // override if succed
+ else
+ {
+ lmk_inc_match = lmk_matches.rbegin()->second;
+ landmark_match_map_[_incoming_feature] = lmk_inc_match;
+ }
+ }
+
+ assert(lmk_inc_match->check() && "after correct drift, check not passed"); // checking partial match
+
+ return true;
+}
+
+bool ProcessorTrackerFeaturePolyline2d::tryCompletePartialMatch(LandmarkMatchPolyline2dPtr& lmk_inc_match) const
+{
+ auto pl_lmk = lmk_inc_match->pl_landmark_;
+ auto pl_inc = lmk_inc_match->pl_feature_;
+
+ // FRONT POINTS
+ while (lmk_inc_match->isPartialFront())
+ {
+ WOLF_DEBUG("Partial match in front...");
+
+ auto exceeding_inc = lmk_inc_match->feature_from_id_-1;
+ auto exceeding_lmk = pl_lmk->getPrevValidId(lmk_inc_match->landmark_from_id_);
+
+ // check good overlap
+ Eigen::Vector3d lmk_point_expected(Eigen::Vector3d::Ones());
+ lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk);
+ Eigen::Vector3d ftr_point = pl_inc->getPoints().col(exceeding_inc);
+
+ double sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
+
+ // exceeding point not matched -> return not succeed
+ if (sq_dist > params_tracker_feature_polyline_->match_landmark_position_sq_norm_max)
+ {
+ WOLF_WARN("correctFeatureDrift: any point didn't match while checking incoming-lmk front exceeding point: feature point ", exceeding_inc, " - landmark point ", exceeding_lmk);
+ return false;
+ }
+ // add exceeding point to the landmark match
+ else
+ {
+ lmk_inc_match->feature_from_id_ = exceeding_inc;
+ lmk_inc_match->landmark_from_id_ = exceeding_lmk;
+ }
+ }
+ // BACK POINTS
+ while (lmk_inc_match->isPartialBack())
+ {
+ WOLF_DEBUG("Partial match in back...");
+
+ auto exceeding_inc = lmk_inc_match->feature_to_id_+1;
+ auto exceeding_lmk = pl_lmk->getNextValidId(lmk_inc_match->landmark_to_id_);
+
+ // check good overlap
+ Eigen::Vector3d lmk_point_expected(Eigen::Vector3d::Ones());
+ lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk);
+ Eigen::Vector3d ftr_point = pl_inc->getPoints().col(exceeding_inc);
+
+ double sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
+
+ // exceeding point not matched -> return not succeed
+ if (sq_dist > params_tracker_feature_polyline_->match_landmark_position_sq_norm_max)
+ {
+ WOLF_WARN("correctFeatureDrift: any point didn't match while checking incoming-lmk back exceeding point: feature point ", exceeding_inc, " - landmark point ", exceeding_lmk);
+ return false;
+ }
+ // add exceeding point to the landmark match
+ else
+ {
+ lmk_inc_match->feature_to_id_ = exceeding_inc;
+ lmk_inc_match->landmark_to_id_ = exceeding_lmk;
+ }
+ }
+
+ return true;
+}
+
+bool ProcessorTrackerFeaturePolyline2d::voteForKeyFrame() const
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::voteForKeyFrame: ");
+ // TODO
+ /* IDEAS:
+ * - Any feature incoming derived too much from projected Landmark
+ * - Any feature incoming overlap with Landmark less than param (1 or 2) defined points
+ * - Any feature last would define extreme or add points to the Landmark
+ * - Some (more than param) tracks lost
+ */
+ return false;
+}
+
+unsigned int ProcessorTrackerFeaturePolyline2d::processNew(const int& _max_features)
+{
+ // WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
+
+ // PT::processNew() ==========================================================
+ unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures() and trackFeatures()
+
+ // WOLF_DEBUG("Processing ", n, " new last features");
+
+ // prior transformations
+ Eigen::Matrix3d T_sensor_world_last(Eigen::Matrix3d::Identity());
+ T_sensor_world_last.topLeftCorner(2,2) = R_sensor_world_last_;
+ T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_;
+
+ // MATCH or EMPLACE LANDMARKS ==========================================================
+ // For each new last feature: Either emplace a landmark or match with an existent landmark
+ LandmarkBasePtrList new_landmarks;
+ for (auto ftr : new_features_last_)
+ {
+ // not matched feature
+ assert(landmark_match_map_.count(ftr) == 0 && "feature in new_features_last is already in landmark_match_map_");
+
+ auto pl_ftr = std::static_pointer_cast<FeaturePolyline2d>(ftr);
+ LandmarkMatchPolyline2dScalarMap best_lmk_matches;
+ // WOLF_DEBUG("Processing feature: ", pl_ftr->id());
+
+ // SEARCH: Check matching with all existing landmarks
+ for (auto lmk : getProblem()->getMap()->getLandmarkList())
+ // Check only polyline landmarks that hasn't been just emplaced
+ if (lmk->getType() == "POLYLINE 2d" && std::find(new_landmarks.begin(), new_landmarks.end(), lmk) == new_landmarks.end())
+ // Store all matches consistent with T_sensor_world_last in best_lmk_matches sorted by difference from T_sensor_world_last
+ tryMatchWithLandmark(best_lmk_matches, std::static_pointer_cast<LandmarkPolyline2d>(lmk), pl_ftr, T_sensor_world_last);
+
+ // EMPLACE: If not matched with any existing landmark: emplace a new one
+ if (best_lmk_matches.empty())
+ {
+ // emplace a landmark
+ auto new_lmk_ptr = emplaceLandmark(pl_ftr);
+
+ // Add a new match to landmark_match_map_
+ auto new_lmk_match = std::make_shared<LandmarkMatchPolyline2d>();
+ new_lmk_match->landmark_ptr_ = new_lmk_ptr;
+ new_lmk_match->pl_landmark_ = std::static_pointer_cast<LandmarkPolyline2d>(new_lmk_ptr);
+ new_lmk_match->pl_feature_ = pl_ftr;
+ new_lmk_match->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2d>(new_lmk_ptr)->getVersion();
+ new_lmk_match->feature_from_id_= 0; // all points match
+ new_lmk_match->landmark_from_id_ = 0; // all points match
+ new_lmk_match->feature_to_id_= pl_ftr->getNPoints()-1; // all points match
+ new_lmk_match->landmark_to_id_ = pl_ftr->getNPoints()-1; // all points match
+ new_lmk_match->normalized_score_ = 1.0; // max score
+ new_lmk_match->T_feature_landmark_ = Eigen::Matrix3d::Identity();
+ new_lmk_match->T_feature_landmark_.topLeftCorner(2,2) = R_sensor_world_last_;
+ new_lmk_match->T_feature_landmark_.topRightCorner(2,1) = t_sensor_world_last_;
+
+ WOLF_ERROR_COND((new_lmk_match->computeSqDistArray() >= Constants::EPS).any(),
+ "Created a landmark not perfectly aligned with feature: ",
+ new_lmk_match->computeSqDistArray().transpose() );
+ assert((new_lmk_match->computeSqDistArray() < Constants::EPS).all());
+
+ // Store in new_landmarks
+ new_landmarks.push_back(new_lmk_ptr);
+ // store match in map
+ landmark_match_map_[pl_ftr] = new_lmk_match;
+ }
+ // MATCH
+ else
+ {
+ // store best match (highest score) in map
+ landmark_match_map_[pl_ftr] = best_lmk_matches.rbegin()->second;
+
+ // store landmark candidates to be merged
+ if (best_lmk_matches.size() > 1)
+ {
+ LandmarkPolyline2dPtrList merge_candidates;
+ for (auto lmk_match_pair_it : best_lmk_matches)
+ merge_candidates.push_back(lmk_match_pair_it.second->pl_landmark_);
+ merge_candidates_list_.push_back(merge_candidates);
+ }
+ }
+ }
+
+ // INCOMING-LANDMARK MATCH ==========================================================
+ // store in landmark_match_map_ the incoming features
+ auto ftr_it = new_features_incoming_.begin();
+ while (ftr_it != new_features_incoming_.end())
+ {
+ auto pl_incoming = std::static_pointer_cast<FeaturePolyline2d>(*ftr_it);
+ assert(matches_last_from_incoming_.count(*ftr_it) && "last-incoming match not found");
+ auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2d>(matches_last_from_incoming_[*ftr_it]);
+ assert(landmark_match_map_.count(pl_ftr_match->feature_ptr_) && "last-lmk match not found");
+
+ // try to concatenate incoming-last + last-landmark into incoming-landmark
+ auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_ftr_match,
+ landmark_match_map_[pl_ftr_match->feature_ptr_]);
+
+ // Matched
+ if (pl_lmk_match_incoming != nullptr)
+ {
+ assert(landmark_match_map_.count(pl_incoming) == 0);
+ landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
+ ftr_it++;
+ }
+ // incoming-landmark did not match
+ else
+ {
+ WOLF_DEBUG("PTFP ", getName(), "::processNew: incoming-last + last-landmark could not be concatenated into incoming-landmark");
+ matches_last_from_incoming_.erase(pl_incoming);
+ // remove feature (unlink not allowed, we could create a new one via copy..)
+ pl_incoming->remove();
+ ftr_it = new_features_incoming_.erase(ftr_it);
+ }
+ }
+ return n;
+}
+
+unsigned int ProcessorTrackerFeaturePolyline2d::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
+{
+ WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", untracked_features_last_.size(), " in untracked_features_last_)");
+
+ // move all already created features (in preProcess())
+ _features_out = std::move(untracked_features_last_);
+
+ // resize to max_new_features
+ if (_max_new_features != -1 && _features_out.size() > _max_new_features)
+ _features_out.resize(_max_new_features);
+
+ // link detected features (only created in preProcess())
+ for (auto ftr : _features_out)
+ ftr->link(_capture);
+
+ // WOLF_DEBUG(_features_out.size(), " were provided");
+
+ return _features_out.size();
+}
+
+void ProcessorTrackerFeaturePolyline2d::establishFactors()
+{
+ // WOLF_DEBUG("PTFP ", getName(), "::establishFactors: ");
+ unsigned int N_factors = 0;
+
+ // Create a factor for each match in last features
+ FeatureBasePtrList last_features = last_ptr_->getFeatureList();
+ for (auto ftr : last_features)
+ {
+ // WOLF_DEBUG("\tLast feature: ", ftr->id());
+ assert(landmark_match_map_.count(ftr) && "feature without landmark match in last features");
+ auto lmk_match = landmark_match_map_[ftr];
+ auto pl_lmk = lmk_match->pl_landmark_;
+ auto pl_ftr = lmk_match->pl_feature_;
+ assert(pl_ftr->id() == ftr->id());
+ // WOLF_DEBUG("\tLandmark: ", pl_lmk->id());
+
+ // LANDMARK CHANGED: update match
+ // WOLF_DEBUG("\tLandmark changed?");
+ if (lmk_match->isDeprecated())
+ {
+ // WOLF_DEBUG("\tLandmark changed or was merged");
+ // not successful update
+ if (!updateLandmarkMatch(lmk_match))
+ {
+ // std::cout << "\t\tNot success: removing feature " << pl_ftr->id() << " and landmark match\n";
+ // remove from match map
+ landmark_match_map_.erase(ftr);
+ // remove feature
+ ftr->remove();
+ continue;
+ }
+ }
+
+ // LANDMARK REMOVED: remove match and feature
+ else
+ {
+ // WOLF_DEBUG("\tLandmark removed?");
+ if (pl_lmk->isRemoving())
+ {
+ // std::cout << "\t\tLandmark was removed: removing feature " << pl_ftr->id() << " and landmark match\n";
+ // remove from match map
+ landmark_match_map_.erase(ftr);
+ // remove feature
+ ftr->remove();
+ continue;
+ }
+ }
+
+ // MODIFY LANDMARK (only for not closed and not recently emplaced)
+ // WOLF_DEBUG("\tModifying..");
+ if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
+ // If modified, add lmk to modified_lmks_ list
+ if (modifyLandmarkAndMatch(lmk_match))
+ modified_lmks_.push_back(pl_lmk);
+
+ // ESTABLISH FACTORS
+ // WOLF_DEBUG("\tEstablishing factors..");
+ // checks
+ assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+ assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+ assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
+ assert(lmk_match->check() && "Landmark didn't grow properly");
+
+ // std::cout << "Establishing factor between" << std::endl;
+ // std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
+ // std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
+ // std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
+ // std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
+ // std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
+ // std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
+
+ // Factors for all feature points
+ int lmk_point_id = lmk_match->landmark_from_id_;
+ for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
+ {
+ // std::cout << "feature point " << ftr_point_id << std::endl;
+ // std::cout << "landmark point " << lmk_point_id << std::endl;
+
+ // First not defined point
+ if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
+ // first point to line factor
+ {
+ // std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
+ assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
+
+ // emplace factor
+ emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
+ N_factors++;
+ // std::cout << "factor added" << std::endl;
+ }
+
+ // Last not defined point
+ else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
+ // last point to line factor
+ {
+ // std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl;
+ assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
+
+ // emplace factor
+ emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
+ N_factors++;
+ // std::cout << "factor added" << std::endl;
+ }
+
+ // Defined point
+ else
+ // point to point factor
+ {
+ // std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
+ //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
+ //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
+ //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
+ emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
+ N_factors++;
+ // std::cout << "factor added" << std::endl;
+ }
+
+ // Next lmk point
+ if (ftr_point_id < pl_ftr->getNPoints()-1)
+ lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
+ }
+ }
+ // std::cout << N_factors << " factors established" << std::endl;
+}
+
+void ProcessorTrackerFeaturePolyline2d::emplaceFactorPointToLine(FeaturePolyline2dPtr _polyline_feature,
+ LandmarkPolyline2dPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id,
+ const int& _lmk_prev_point_id)
+{
+ assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
+
+ FactorBasePtr new_fac = FactorBase::emplace<FactorPointToLine2d>(_polyline_feature,
+ _polyline_feature,
+ _polyline_landmark,
+ shared_from_this(),
+ _ftr_point_id,
+ _lmk_point_id,
+ _lmk_prev_point_id,
+ params_->apply_loss_function);
+}
+
+void ProcessorTrackerFeaturePolyline2d::emplaceFactorPoint(FeaturePolyline2dPtr _polyline_feature,
+ LandmarkPolyline2dPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id)
+{
+ assert(_polyline_landmark->isValidId(_lmk_point_id));
+
+ FactorBasePtr new_fac = FactorBase::emplace<FactorPoint2d>(_polyline_feature,
+ _polyline_feature,
+ _polyline_landmark,
+ shared_from_this(),
+ _ftr_point_id,
+ _lmk_point_id,
+ params_->apply_loss_function);
+}
+
+LandmarkBasePtr ProcessorTrackerFeaturePolyline2d::emplaceLandmark(FeatureBasePtr _feature_ptr)
+{
+ // WOLF_DEBUG("PTFP ", getName(), "::emplaceLandmark: ");
+ //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
+ //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
+ assert(_feature_ptr->getType() == "POLYLINE 2d");
+ assert(std::find(last_ptr_->getFeatureList().begin(),last_ptr_->getFeatureList().end(),_feature_ptr) != last_ptr_->getFeatureList().end() && "creating a landmark for a feature which is not in last capture");
+
+
+ FeaturePolyline2dPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2d>(_feature_ptr);
+ // compute feature global pose
+ Eigen::MatrixXd points_global = R_world_sensor_last_ * polyline_ptr->getPoints().topRows<2>() +
+ t_world_sensor_last_ * Eigen::VectorXd::Ones(polyline_ptr->getNPoints()).transpose();
+
+ // std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
+ // std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
+ // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
+
+ // Create new landmark
+ return LandmarkBase::emplace<LandmarkPolyline2d>(getProblem()->getMap(),
+ std::make_shared<StateBlock>(Eigen::Vector2d::Zero(), true),
+ std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true),
+ points_global,
+ polyline_ptr->isFirstDefined(),
+ polyline_ptr->isLastDefined());
+ //std::cout << "done" << std::endl;
+}
+
+bool ProcessorTrackerFeaturePolyline2d::modifyLandmarkAndMatch(LandmarkMatchPolyline2dPtr& lmk_match)
+{
+ WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
+ //lmk_match->print();
+ auto pl_lmk = lmk_match->pl_landmark_;
+ auto pl_ftr = lmk_match->pl_feature_;
+ bool print = false;
+ bool modified = false;
+
+ assert(lmk_match->check() && "Bad landmark match provided");
+
+ Eigen::MatrixXd points_global2 = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
+ t_world_sensor_last_ * Eigen::VectorXd::Ones(pl_ftr->getNPoints()).transpose();
+ Eigen::MatrixXd points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
+ if (print)
+ {
+ std::cout << "landmark points" << std::endl << pl_lmk->computePointsGlobal() << std::endl;
+ std::cout << "points_global " << std::endl << points_global << std::endl;
+ //std::cout << "points_global2 " << std::endl << points_global2 << std::endl;
+ std::cout << "landmark points local" << std::endl << lmk_match->T_feature_landmark_ * pl_lmk->computePointsGlobal() << std::endl;
+ std::cout << "feature points " << std::endl << pl_ftr->getPoints() << std::endl;
+ std::cout << "feature local global" << std::endl << lmk_match->T_feature_landmark_ * points_global << std::endl;
+ std::cout << "lmk_match->T_feature_landmark_" << std::endl << lmk_match->T_feature_landmark_ << std::endl;
+ }
+
+ // MODIFY LANDMARK
+ // -----------------GROW Front-----------------
+ // Add new front points (if any feature point not matched)
+ if ( lmk_match->feature_from_id_ > 0 ) // && !pl_lmk->isClosed()
+ {
+ assert(!pl_lmk->isClosed() && "feature has not matched points in a closed landmark");
+ if (print)
+ {
+ std::cout << "Add new front points. Defined: " << pl_ftr->isFirstDefined() << std::endl;
+ std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
+ }
+ pl_lmk->addPoints(points_global, // points matrix in global coordinates
+ lmk_match->feature_from_id_-1, // last feature point index to be added
+ pl_ftr->isFirstDefined(), // is defined
+ false); // front
+
+ lmk_match->landmark_from_id_ = pl_lmk->getFirstId();
+ lmk_match->feature_from_id_ = 0;
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
+ modified = true;
+ if (print)
+ {
+ std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
+ }
+
+ // checks
+ assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+ assert(lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && "Landmark didn't grow properly");
+ assert(lmk_match->check() && "Landmark didn't grow properly");
+ }
+ // -----------------CHANGE Front-----------------
+ // Change first not defined point if landmark first not defined matched with first feature point that:
+ // a. is defined
+ // b. would extend the line (check if angle is bigger than 90º)
+ else if (lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
+ {
+ if (// case a
+ pl_ftr->isFirstDefined() ||
+ // case b
+ (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))).squaredNorm() >
+ (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm() +
+ (pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm())
+ {
+ if (print)
+ {
+ std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
+ std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
+ }
+
+ pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
+ modified = true;
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
+
+ // checks
+ assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+ assert(lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && "Landmark didn't grow properly");
+ assert(lmk_match->check() && "Landmark didn't grow properly");
+ }
+ }
+ // -----------------GROW Back-----------------
+ // Add new back points (if any feature point not matched)
+ if (lmk_match->feature_to_id_ < pl_ftr->getNPoints()-1)
+ {
+ assert(!pl_lmk->isClosed() && "feature not matched points in a closed landmark");
+ if (print)
+ {
+ std::cout << "Add back points. Defined: " << pl_ftr->isLastDefined() << std::endl;
+ std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
+ }
+ pl_lmk->addPoints(points_global, // points matrix in global coordinates
+ lmk_match->feature_to_id_+1, // last feature point index to be added
+ pl_ftr->isLastDefined(), // is defined
+ true); // back
+
+ lmk_match->landmark_to_id_ = pl_lmk->getLastId();
+ lmk_match->feature_to_id_ = pl_ftr->getNPoints()-1;
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
+ modified = true;
+ if (print)
+ {
+ std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
+ }
+ // checks
+ assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+ assert(lmk_match->landmark_to_id_ == pl_lmk->getLastId() && "Landmark didn't grow properly");
+ assert(lmk_match->check() && "Landmark didn't grow properly");
+ }
+ // -----------------CHANGE Back-----------------
+ // Change last not defined point if landmark last not defined matched with last feature point that:
+ // a. is defined
+ // b. would extend the line (check if angle is bigger than 90º)
+ else if (lmk_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
+ {
+ if (// case a
+ pl_ftr->isLastDefined() ||
+ // case b
+ (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))).squaredNorm() >
+ (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm() +
+ (pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm())
+ {
+ if (print)
+ {
+ std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
+ std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
+ }
+
+ pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined());
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
+ modified = true;
+
+ // checks
+ assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+ assert(lmk_match->landmark_to_id_ == pl_lmk->getLastId() && "Landmark didn't grow properly");
+ assert(lmk_match->check() && "Landmark didn't grow properly");
+ }
+ }
+ // all checks
+ assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+ assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+ assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
+ assert(lmk_match->check() && "Landmark didn't grow properly (should have stopped in a previous assert)");
+
+ //if (print)
+ // lmk_match->print();
+//
+// WOLF_ERROR_COND((lmk_match->computeSqDistArray() >= 0.5).any(),
+// "landmark not properly aligned with feature: ",
+// lmk_match->computeSqDistArray().transpose() );
+// assert((lmk_match->computeSqDistArray() < 0.5).all());
+
+ return modified;
+}
+
+void ProcessorTrackerFeaturePolyline2d::advanceDerived()
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: ");
+ // remove landmark matches with last features
+ if (last_ptr_ != nullptr)
+ for (auto ftr : last_ptr_->getFeatureList())
+ {
+ assert(landmark_match_map_.count(ftr));
+ landmark_match_map_.erase(ftr);
+ }
+
+ // check all not tracked features are not linked
+ assert(std::all_of(untracked_features_last_.begin(), untracked_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+ "any linked feature in untracked_features_last_");
+ assert(std::all_of(untracked_features_incoming_.begin(), untracked_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+ "any linked feature in untracked_features_incoming_");
+
+ // remove untracked features
+ //WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_");
+ untracked_features_last_.clear();
+
+ // move untracked features incoming to last
+ untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_);
+ //WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)");
+ //WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+
+ ProcessorTrackerFeature::advanceDerived();
+}
+
+void ProcessorTrackerFeaturePolyline2d::resetDerived()
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::resetDerived: ");
+ // remove landmark matches with last features
+ if (last_ptr_ != nullptr)
+ for (auto ftr : last_ptr_->getFeatureList())
+ {
+ assert(landmark_match_map_.count(ftr));
+ landmark_match_map_.erase(ftr);
+ }
+
+ // check all not tracked features are not linked
+ assert(std::all_of(untracked_features_last_.begin(), untracked_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+ "any linked feature in untracked_features_last_");
+ assert(std::all_of(untracked_features_incoming_.begin(), untracked_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+ "any linked feature in untracked_features_incoming_");
+
+ // remove untracked features
+ //WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_");
+ untracked_features_last_.clear();
+
+ // move untracked features incoming to last
+ untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_);
+ //WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)");
+ //WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+
+ ProcessorTrackerFeature::resetDerived();
+}
+
+void ProcessorTrackerFeaturePolyline2d::preProcess()
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: ");
+ // Extract all polylines from incoming_
+ // TODO: sort corners by bearing
+ std::list<laserscanutils::Polyline> polylines;
+
+ line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2d>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2d>(getSensor()))->getScanParams(), polylines);
+ WOLF_DEBUG("PTFP ", getName(), " Polylines extracted: ", polylines.size());
+
+ for (auto&& pl : polylines)
+ {
+ //WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ );
+ //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
+ auto points = pl.points_;
+ points.bottomRows(1) = Eigen::MatrixXd::Ones(1,points.cols());
+ untracked_features_incoming_.push_back(std::make_shared<FeaturePolyline2d>(points, pl.covs_, pl.first_defined_, pl.last_defined_));
+ //WOLF_DEBUG("new polyline detected: points\n",pl.points_);
+ }
+
+ // compute transformations
+ if (last_ptr_ != nullptr && incoming_ptr_ != nullptr)
+ computeTransformations();
+
+ //WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features");
+ //WOLF_DEBUG_COND(last_ptr_!=nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+}
+
+void ProcessorTrackerFeaturePolyline2d::postProcess()
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::postProcess: ");
+ // Try to close & classify modified landmarks
+ while (!modified_lmks_.empty())
+ {
+ auto lmk_ptr = modified_lmks_.front();
+ modified_lmks_.pop_front();
+
+ assert(!lmk_ptr->isRemoving());
+
+ // try classify
+ if (lmk_ptr->getClassification().type == UNCLASSIFIED && lmk_ptr->getNDefinedPoints() >= 3 && lmk_ptr->getNDefinedPoints() <= 4)
+ lmk_ptr->tryClassify(params_tracker_feature_polyline_->class_position_error_th, params_tracker_feature_polyline_->polyline_classes);
+ // try close
+ if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 4 || lmk_ptr->getNPoints() >= 5))
+ lmk_ptr->tryClose(params_tracker_feature_polyline_->class_position_error_th);
+ }
+
+ // Try to merge landmarks
+ while (!merge_candidates_list_.empty())
+ {
+ // load next list of candidates
+ LandmarkPolyline2dPtrList merge_candidates = std::move(merge_candidates_list_.front());
+ merge_candidates_list_.pop_front();
+
+ // change already merged lmks with the corresponding remaining lmk
+ for (auto&& lmk : merge_candidates)
+ if (lmk->getMergedInLandmark() != nullptr)
+ lmk = lmk->getMergedInLandmark();
+
+ // remove repeated lmks
+ merge_candidates.sort();
+ merge_candidates.unique();
+
+ if (merge_candidates.size() == 1)
+ continue;
+
+ // Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
+ //FIXME: special parameter for merging landmarks?
+ LandmarkPolyline2d::tryMergeLandmarks(merge_candidates,
+ params_tracker_feature_polyline_->match_landmark_position_sq_norm_max,
+ params_tracker_feature_polyline_->match_landmark_overlap_dist_min,
+ params_tracker_feature_polyline_->match_landmark_overlap_n_min,
+ params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min);
+ }
+ //WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features");
+ //WOLF_DEBUG_COND(last_ptr_!=nullptr,"PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+}
+
+void ProcessorTrackerFeaturePolyline2d::tryMatchWithFeature(FeatureMatchPolyline2dScalarMap& ftr_matches,
+ const FeaturePolyline2dPtr& _ftr_L,
+ const FeaturePolyline2dPtr& _ftr_I,
+ const Eigen::Matrix3d& _T_last_incoming_prior) const
+{
+ //WOLF_DEBUG("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id());
+
+ MatchPolyline2dPtr best_match = nullptr;
+
+ if (params_tracker_feature_polyline_->use_probabilistic_match)
+ {
+ // Compute Covariance of projected points (only considering noise in T_last_incoming) and computing Jacobian using the mean of the points
+ Eigen::Vector2d mean_last_points = _ftr_L->getPoints().rowwise().mean().head(2);
+ Eigen::Vector2d J_th;
+ double cos_th = _T_last_incoming_prior(0,0);
+ double sin_th = _T_last_incoming_prior(1,0);
+ J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
+ cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
+ Eigen::Matrix2d Sigma = Eigen::Matrix2d::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_max +
+ J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_max*J_th.transpose();
+
+ // sq root of inverse
+ Eigen::Matrix2d sqrtOmega = Eigen::Matrix2d(Sigma.llt().matrixL()).inverse();
+
+ // last expected points
+ Eigen::Matrix<double, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+
+ // Shape points in the equiprobabilistic space
+ Eigen::Matrix<double, 3, Eigen::Dynamic> last_shaped = last_expected_points;
+ Eigen::Matrix<double, 3, Eigen::Dynamic> inc_shaped = _ftr_I->getPoints();
+ last_shaped.topRows<2>() = sqrtOmega*last_shaped.topRows<2>();
+ inc_shaped.topRows<2>() = sqrtOmega*inc_shaped.topRows<2>();
+
+ // compute best sq Mahalanobis distance matching
+ best_match = computeBestSqDist(inc_shaped, // <--feature incoming points
+ _ftr_I->isFirstDefined(),
+ _ftr_I->isLastDefined(),
+ false, // <--feature is not closed for sure
+ last_shaped, // <--feature last points
+ _ftr_L->isFirstDefined(),
+ _ftr_L->isLastDefined(),
+ false, // <--feature is not closed for sure
+ 1,
+ params_tracker_feature_polyline_->match_feature_overlap_dist_min,
+ params_tracker_feature_polyline_->match_feature_overlap_n_min,
+ params_tracker_feature_polyline_->match_feature_overlap_n_defined_min);
+ }
+ else // not probabilistic match
+ {
+ // last expected points
+ Eigen::Matrix<double, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+
+ // compute best sq Mahalanobis distance matching
+ best_match = computeBestSqDist(_ftr_I->getPoints(), // <--feature incoming points
+ _ftr_I->isFirstDefined(),
+ _ftr_I->isLastDefined(),
+ false, // <--feature is not closed for sure
+ last_expected_points, // <--feature last points
+ _ftr_L->isFirstDefined(),
+ _ftr_L->isLastDefined(),
+ false, // <--feature is not closed for sure
+ params_tracker_feature_polyline_->match_feature_position_sq_norm_max,
+ params_tracker_feature_polyline_->match_feature_overlap_dist_min,
+ params_tracker_feature_polyline_->match_feature_overlap_n_min,
+ params_tracker_feature_polyline_->match_feature_overlap_n_defined_min);
+ }
+
+ //valid match
+ if (best_match != nullptr)
+ {
+
+ auto ftr_match = std::make_shared<FeatureMatchPolyline2d>();
+ // feature pointers
+ ftr_match->feature_ptr_ = _ftr_L;
+ ftr_match->pl_last_ = _ftr_L;
+ ftr_match->pl_incoming_ = _ftr_I;
+ // feature incoming point id's
+ ftr_match->feature_incoming_from_id_ = best_match->from_A_id_;
+ ftr_match->feature_incoming_to_id_ = best_match->to_A_id_;
+ // feature last point id's
+ ftr_match->feature_last_from_id_ = best_match->from_B_id_;
+ ftr_match->feature_last_to_id_ = best_match->to_B_id_;
+ // incoming last transformation
+ if (ftr_match->getNDefinedPoints() > 0 && ftr_match->getNPoints() > 1)
+ {
+ ftr_match->T_incoming_last_ = pose2T2d(computeRigidTrans(_ftr_I->getPoints().middleCols(ftr_match->feature_incoming_from_id_,ftr_match->getNPoints()),
+ _ftr_L->getPoints().middleCols(ftr_match->feature_last_from_id_,ftr_match->getNPoints()),
+ ftr_match->fromDefined(),
+ ftr_match->toDefined()));
+ //ftr_match->print();
+ }
+ else
+ ftr_match->T_incoming_last_ = _T_last_incoming_prior;
+ // score
+ ftr_match->normalized_score_ = best_match->normalized_score_;
+
+ assert(ftr_match->feature_incoming_from_id_ == 0 || ftr_match->feature_last_from_id_ == 0);
+ assert(ftr_match->feature_incoming_to_id_ == _ftr_I->getNPoints() -1 || ftr_match->feature_last_to_id_ == _ftr_L->getNPoints() -1);
+
+ // store in list
+ ftr_matches[ftr_match->normalized_score_] = ftr_match;
+ //WOLF_DEBUG("match stored:");
+ //ftr_match->print();
+ assert(ftr_match->check() && "tryMatchWithFeature: wrong match");
+ }
+
+}
+
+void ProcessorTrackerFeaturePolyline2d::tryMatchWithLandmark(LandmarkMatchPolyline2dScalarMap& lmk_matches,
+ const LandmarkPolyline2dPtr& _lmk_ptr,
+ const FeaturePolyline2dPtr& _feat_ptr,
+ const Eigen::Matrix3d& _T_feature_landmark_prior) const
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
+
+ MatchPolyline2dPtr best_match = nullptr;
+
+ if (params_tracker_feature_polyline_->use_probabilistic_match)
+ {
+ // Compute Covariance of projected points (only considering noise in _T_feature_landmark_prior) and computing Jacobian using the mean of the points
+ Eigen::Vector2d mean_last_points = _lmk_ptr->computePointsGlobal().rowwise().mean().head(2);
+ Eigen::Vector2d J_th;
+ double cos_th = _T_feature_landmark_prior(0,0);
+ double sin_th = _T_feature_landmark_prior(1,0);
+ J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
+ cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
+ Eigen::Matrix2d Sigma = Eigen::Matrix2d::Identity()*params_tracker_feature_polyline_->match_landmark_position_sq_norm_max +
+ J_th*params_tracker_feature_polyline_->match_landmark_orientation_sq_norm_max*J_th.transpose();
+
+ // sq root of inverse
+ Eigen::Matrix2d sqrtOmega = Eigen::Matrix2d(Sigma.llt().matrixL()).inverse();
+
+ // landmark expected points
+ Eigen::MatrixXd lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+
+ // Shape points in the equiprobabilistic space
+ Eigen::Matrix<double, 3, Eigen::Dynamic> lmk_shaped = lmk_expected_points;
+ Eigen::Matrix<double, 3, Eigen::Dynamic> feat_shaped = _feat_ptr->getPoints();
+ lmk_shaped.topRows<2>() = sqrtOmega*lmk_shaped.topRows<2>();
+ feat_shaped.topRows<2>() = sqrtOmega*feat_shaped.topRows<2>();
+
+ // compute best sq Mahalanobis distance matching
+ best_match = computeBestSqDist(lmk_shaped, // <--landmark points in local coordinates
+ _lmk_ptr->isFirstDefined(),
+ _lmk_ptr->isLastDefined(),
+ _lmk_ptr->isClosed(),
+ feat_shaped, // <--feature last points
+ _feat_ptr->isFirstDefined(),
+ _feat_ptr->isLastDefined(),
+ false, // <--feature is not closed for sure
+ 1,
+ params_tracker_feature_polyline_->match_landmark_overlap_dist_min,
+ params_tracker_feature_polyline_->match_landmark_overlap_n_min,
+ params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min,
+ true, // partial match lmk allowed
+ false);// partial match ftr allowed
+ }
+ else // not probabilistic match
+ {
+ // landmark expected points
+ Eigen::MatrixXd lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+
+ // compute best sq distance matching
+ best_match = computeBestSqDist(lmk_expected_points, // <--landmark points in local coordinates
+ _lmk_ptr->isFirstDefined(),
+ _lmk_ptr->isLastDefined(),
+ _lmk_ptr->isClosed(),
+ _feat_ptr->getPoints(), // <--feature points
+ _feat_ptr->isFirstDefined(),
+ _feat_ptr->isLastDefined(),
+ false, // <--feature is not closed for sure
+ params_tracker_feature_polyline_->match_landmark_position_sq_norm_max,
+ params_tracker_feature_polyline_->match_landmark_overlap_dist_min,
+ params_tracker_feature_polyline_->match_landmark_overlap_n_min,
+ params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min,
+ true, // exceeding points in both extremes allowed for landmark
+ false);// exceeding points in both extremes allowed for feature
+ }
+
+ //valid match
+ if (best_match != nullptr)
+ {
+ //WOLF_DEBUG("match found!");
+ auto lmk_match = std::make_shared<LandmarkMatchPolyline2d>();
+
+ // landmark
+ lmk_match->landmark_ptr_=_lmk_ptr;
+ lmk_match->pl_landmark_=_lmk_ptr;
+ lmk_match->landmark_version_=_lmk_ptr->getVersion();
+ // landmark point id's
+ std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
+ lmk_match->landmark_from_id_ = lmk_valid_ids[best_match->from_A_id_];
+ lmk_match->landmark_to_id_ = lmk_valid_ids[best_match->to_A_id_];
+ // feature
+ lmk_match->pl_feature_=_feat_ptr;
+ // feature point id's
+ lmk_match->feature_from_id_ = best_match->from_B_id_;
+ lmk_match->feature_to_id_ = best_match->to_B_id_;
+
+ // feature landmark transformation
+ lmk_match->T_feature_landmark_ = _T_feature_landmark_prior;
+ tryUpdateMatchTransformation(lmk_match);
+
+ // score
+ lmk_match->normalized_score_ = best_match->normalized_score_;
+
+ assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
+ assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
+ assert(lmk_match->feature_from_id_ == 0 || lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1);
+
+ // store in list
+ lmk_matches[lmk_match->normalized_score_]= lmk_match;
+ //WOLF_DEBUG("match stored!");
+ assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
+ }
+}
+
+/* updateLandmarkMatch
+ *
+ * landmark changed (grew,closed,merged) and the match should be updated
+ */
+bool ProcessorTrackerFeaturePolyline2d::updateLandmarkMatch(LandmarkMatchPolyline2dPtr& _lmk_match) const
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::updateLandmarkMatch: ");
+
+ auto pl_lmk = _lmk_match->pl_landmark_;
+ auto pl_ftr = _lmk_match->pl_feature_;
+
+ // merged: update the pl_lmk pointer
+ if (pl_lmk->getMergedInLandmark() != nullptr)
+ pl_lmk = pl_lmk->getMergedInLandmark();
+
+ // try match with _feat_ptr again
+ LandmarkMatchPolyline2dScalarMap new_lmk_matches;
+ tryMatchWithLandmark(new_lmk_matches,pl_lmk,pl_ftr,_lmk_match->T_feature_landmark_);
+
+ // not valid match
+ if (new_lmk_matches.empty())
+ {
+ WOLF_DEBUG("updateLandmarkMatch: did not match with landmark, returning false");
+ return false;
+ }
+
+ // valid match: overwrite match
+ _lmk_match = new_lmk_matches.rbegin()->second; // the last is the one with highest score
+ assert(_lmk_match->check() && "updateLandmarkMatch: wrongly updated");
+
+ return true;
+}
+
+LandmarkMatchPolyline2dPtr ProcessorTrackerFeaturePolyline2d::concatenateFeatureLandmarkMatches(FeatureMatchPolyline2dPtr ftr_match,
+ LandmarkMatchPolyline2dPtr lmk_match_last) const
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: ");
+ assert(lmk_match_last->check(false) && "input lmk_match_last wrong");
+ //WOLF_DEBUG("INCOMING-LAST MATCH:");
+ //ftr_match->print();
+ //WOLF_DEBUG("LANDMARK-LAST MATCH:");
+ //lmk_match_last->print();
+
+ WOLF_WARN_COND((lmk_match_last->computeSqDistArray() >= 0.5).any(),
+ "before concatenateFeatureLandmarkMatches too much error: ",
+ lmk_match_last->computeSqDistArray().transpose() );
+ //assert((lmk_match_last->computeSqDistArray() < 0.5).all());
+
+ auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2d>();
+ auto lmk_ptr = lmk_match_last->pl_landmark_;
+
+ // feature correspondence from&to
+ lmk_match_incoming->feature_from_id_ = ftr_match->feature_incoming_from_id_;
+ lmk_match_incoming->feature_to_id_ = ftr_match->feature_incoming_to_id_;
+
+ // landmark correspondence from&to
+ // Not adding new points nor checks (done in correctFeatureDrift)
+ int from_offset = ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
+ int to_offset = ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_;
+// std::cout << "\nfrom_offset: " << from_offset;
+// std::cout << "\nto_offset: " << from_offset;
+ lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_;
+ lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_;
+ while (from_offset > 0)
+ {
+ if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
+ return nullptr;
+ lmk_match_incoming->landmark_from_id_ = lmk_ptr->getNextValidId(lmk_match_incoming->landmark_from_id_);
+ from_offset--;
+ }
+ while (from_offset < 0)
+ {
+ lmk_match_incoming->feature_from_id_++;
+ from_offset++;
+ }
+ while (to_offset < 0)
+ {
+ if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
+ return nullptr;
+ lmk_match_incoming->landmark_to_id_ = lmk_ptr->getPrevValidId(lmk_match_incoming->landmark_to_id_);
+ to_offset++;
+ }
+ while (to_offset > 0)
+ {
+ lmk_match_incoming->feature_to_id_--;
+ to_offset--;
+ }
+// std::cout << "\nlmk_match_incoming:";
+// std::cout << "\n\tincoming_from: " << lmk_match_incoming->feature_from_id_;
+// std::cout << "\n\tincoming_to: " << lmk_match_incoming->feature_to_id_;
+// std::cout << "\n\tlmk_from: " << lmk_match_incoming->landmark_from_id_;
+// std::cout << "\n\tlmk_to: " << lmk_match_incoming->landmark_to_id_ << std::endl;
+
+ // other match attributes
+ lmk_match_incoming->landmark_ptr_ = lmk_ptr;
+ lmk_match_incoming->pl_landmark_ = lmk_ptr;
+ lmk_match_incoming->landmark_version_ = lmk_ptr->getVersion();
+ lmk_match_incoming->pl_feature_ = ftr_match->pl_incoming_;
+ lmk_match_incoming->normalized_score_ = ftr_match->normalized_score_;
+ lmk_match_incoming->T_feature_landmark_ = ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
+
+ assert(lmk_match_incoming->check(false) && "wrongly concatenated"); // check without partial match
+
+ //WOLF_DEBUG("LANDMARK-INCOMING MATCH:");
+ //lmk_match_incoming->print();
+
+ WOLF_WARN_COND((lmk_match_incoming->computeSqDistArray() >= 0.5).any(),
+ "after concatenateFeatureLandmarkMatches too much error: ",
+ lmk_match_incoming->computeSqDistArray().transpose() );
+ //assert((lmk_match_incoming->computeSqDistArray() < 0.5).all());
+
+ return lmk_match_incoming;
+}
+
+bool ProcessorTrackerFeaturePolyline2d::tryUpdateMatchTransformation(LandmarkMatchPolyline2dPtr& lmk_match) const
+{
+ //WOLF_DEBUG("PTFP ", getName(), "::tryUpdateMatchTransformation: ");
+
+ assert(lmk_match->check());
+
+ if (lmk_match->getNDefinedPoints() > 0 && lmk_match->getNPoints() > 1)
+ {
+ //auto T_feature_landmark_prev = lmk_match->T_feature_landmark_;
+
+ lmk_match->T_feature_landmark_ = pose2T2d(computeRigidTrans(lmk_match->pl_feature_->getPoints().middleCols(lmk_match->feature_from_id_,lmk_match->getNPoints()),
+ lmk_match->pl_landmark_->computePointsGlobal(lmk_match->landmark_from_id_,lmk_match->landmark_to_id_),
+ lmk_match->fromDefined(),
+ lmk_match->toDefined()));
+
+// WOLF_WARN_COND((T2pose2d(T_feature_landmark_prev)-T2pose2d(lmk_match->T_feature_landmark_)).head(2).norm() > 0.1 ||
+// abs(pi2pi((T2pose2d(T_feature_landmark_prev)-T2pose2d(lmk_match->T_feature_landmark_))(2))) > 0.1,
+// "tryUpdateMatchTransformation: lmk_match->T_feature_landmark_ changed significantly. \n\tPrev pose: ",
+// T2pose2d(T_feature_landmark_prev).transpose(),
+// "\n\tUpdated pose:",
+// T2pose2d(lmk_match->T_feature_landmark_).transpose());
+
+ WOLF_WARN_COND((lmk_match->computeSqDistArray() >= 0.5).any(),
+ "after tryUpdateMatchTransformation too much error: ",
+ lmk_match->computeSqDistArray().transpose() );
+
+ return true;
+ }
+ //WOLF_DEBUG("tryUpdateMatchTransformation: Not enough points: ",lmk_match->getNPoints(), " (should be > 1) or defined points: ", lmk_match->getNDefinedPoints(), " (should be > 0)");
+ return false;
+}
+
+// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void ProcessorTrackerFeaturePolyline2d::computeTransformations()
+{
+ //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
+ const auto& vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp()).vector("PO");
+ const auto& vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp()).vector("PO");
+
+ // world_robot
+ Eigen::Matrix2d R_world_robot_incoming = Eigen::Rotation2Dd(vehicle_pose_incoming(2)).matrix();
+ Eigen::Matrix2d R_world_robot_last = Eigen::Rotation2Dd(vehicle_pose_last(2)).matrix();
+
+ // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
+ if (getSensor()->isStateBlockDynamic('P') ||
+ getSensor()->isStateBlockDynamic('O') ||
+ !getSensor()->getP()->isFixed() ||
+ !getSensor()->getO()->isFixed() ||
+ !extrinsics_transformation_computed_)
+ {
+ t_robot_sensor_ = getSensor()->getP()->getState();
+ R_robot_sensor_ = Eigen::Rotation2Dd(getSensor()->getO()->getState()(0)).matrix();
+ extrinsics_transformation_computed_ = true;
+ }
+
+ // INCOMING
+ // global_sensor
+ R_world_sensor_incoming_ = R_world_robot_incoming * R_robot_sensor_;
+ t_world_sensor_incoming_ = vehicle_pose_incoming.head<2>() + R_world_robot_incoming * t_robot_sensor_;
+
+ // sensor_global
+ R_sensor_world_incoming_ = R_world_sensor_incoming_.transpose();
+ t_sensor_world_incoming_ = -R_sensor_world_incoming_ * t_world_sensor_incoming_;
+
+ // LAST
+ // global_sensor
+ R_world_sensor_last_ = R_world_robot_last * R_robot_sensor_;
+ t_world_sensor_last_ = vehicle_pose_last.head<2>() + R_world_robot_last * t_robot_sensor_;
+
+ // sensor_global
+ R_sensor_world_last_ = R_world_sensor_last_.transpose();
+ t_sensor_world_last_ = -R_sensor_world_last_ * t_world_sensor_last_;
+
+ // INCOMING-LAST
+ R_incoming_last_ = R_sensor_world_incoming_ * R_world_sensor_last_;
+ t_incoming_last_ = t_sensor_world_incoming_ + R_sensor_world_incoming_ * t_world_sensor_last_;
+
+ // last_incoming
+ R_last_incoming_ = R_world_sensor_last_.transpose() * R_world_sensor_incoming_;
+ t_last_incoming_ = t_sensor_world_last_ + R_sensor_world_last_ * t_world_sensor_incoming_;
+
+ //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
+ //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
+ //std::cout << "t_world_sensor_incoming_ " << t_world_sensor_incoming_.transpose() << std::endl;
+ //std::cout << "R_world_sensor_incoming_ " << std::endl << R_world_sensor_incoming_ << std::endl;
+ //std::cout << "t_sensor_world_incoming_ " << t_sensor_world_incoming_.transpose() << std::endl;
+ //std::cout << "R_sensor_world_incoming_ " << std::endl << R_sensor_world_incoming_ << std::endl;
+ //std::cout << "t_world_sensor_last_ " << t_world_sensor_last_.transpose() << std::endl;
+ //std::cout << "R_world_sensor_last_ " << std::endl << R_world_sensor_last_ << std::endl;
+ //std::cout << "t_sensor_world_last_ " << t_sensor_world_last_.transpose() << std::endl;
+ //std::cout << "R_sensor_world_last_ " << std::endl << R_sensor_world_last_ << std::endl;
+ //std::cout << "t_incoming_last_ " << t_incoming_last_.transpose() << std::endl;
+ //std::cout << "R_incoming_last_ " << std::endl << R_incoming_last_ << std::endl;
+ //std::cout << "t_last_incoming_ " << t_last_incoming_.transpose() << std::endl;
+ //std::cout << "R_last_incoming_ " << std::endl << R_last_incoming_ << std::endl;
+}
+
+
+} /* namespace wolf */
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+ WOLF_REGISTER_PROCESSOR(ProcessorTrackerFeaturePolyline2d);
+ WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerFeaturePolyline2d);
+} // namespace wolf
diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt
deleted file mode 100644
index 661731d27c433e5b0d9e3c14a81d6c0dd0ac6737..0000000000000000000000000000000000000000
--- a/src/sensor/CMakeLists.txt
+++ /dev/null
@@ -1,27 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_SENSOR ${HDRS_SENSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
- ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
- ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h
- )
-
-SET(SRCS_SENSOR ${SRCS_SENSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_SENSOR ${HDRS_SENSOR} PARENT_SCOPE)
-SET(SRCS_SENSOR ${SRCS_SENSOR} PARENT_SCOPE)
-
\ No newline at end of file
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
deleted file mode 100644
index 666788225c5c7b3c1a35ae8004569f38053e1573..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_GPS.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#include "core/sensor/sensor_GPS.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPS::SensorGPS(StateBlockPtr _p_ptr, //GPS sensor position with respect to the car frame (base frame)
- StateBlockPtr _o_ptr, //GPS sensor orientation with respect to the car frame (base frame)
- StateBlockPtr _bias_ptr, //GPS sensor time bias
- StateBlockPtr _map_p_ptr, //initial position of vehicle's frame with respect to starting point frame
- StateBlockPtr _map_o_ptr) //initial orientation of vehicle's frame with respect to the starting point frame
- :
- SensorBase("GPS", _p_ptr, _o_ptr, _bias_ptr, 0)
-{
- getStateBlockVec().resize(5);
- setStateBlockPtrStatic(3, _map_p_ptr); // Map position
- setStateBlockPtrStatic(4, _map_o_ptr); // Map orientation
- //
-}
-
-SensorGPS::~SensorGPS()
-{
- //
-}
-
-StateBlockPtr SensorGPS::getMapP() const
-{
- return getStateBlockPtrStatic(3);
-}
-
-StateBlockPtr SensorGPS::getMapO() const
-{
- return getStateBlockPtrStatic(4);
-}
-
-// Define the factory method
-SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_p.size() == 3 && "Bad extrinsics vector length. Should be 3 for 3D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_p, true);
- StateBlockPtr ori_ptr = nullptr;
- SensorBasePtr sen = std::make_shared<SensorGPS>(pos_ptr, ori_ptr, nullptr, nullptr, nullptr);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS",SensorGPS)
-} // namespace wolf
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
deleted file mode 100644
index ca204f36449afda97c38c4e5278859a3e752a036..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_GPS_fix.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-#include "core/sensor/sensor_GPS_fix.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
- SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), nullptr, _intrinsics.noise_std)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
-}
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) :
- SensorGPSFix(_extrinsics, *_intrinsics_ptr)
-{
- //
-}
-
-SensorGPSFix::~SensorGPSFix()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
- const IntrinsicsBasePtr _intrinsics)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
- SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
- sen->getP()->fix();
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS FIX", SensorGPSFix)
-} // namespace wolf
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
deleted file mode 100644
index de0146784289081c2adb55bd272982d9c86bd797..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_IMU.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "core/sensor/sensor_IMU.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) :
- SensorIMU(_extrinsics, *_params)
-{
- //
-}
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) :
- SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
- a_noise(_params.a_noise),
- w_noise(_params.w_noise),
- ab_initial_stdev(_params.ab_initial_stdev),
- wb_initial_stdev(_params.wb_initial_stdev),
- ab_rate_stdev(_params.ab_rate_stdev),
- wb_rate_stdev(_params.wb_rate_stdev)
-{
- assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
-}
-
-SensorIMU::~SensorIMU()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
- IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
- SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("IMU", SensorIMU)
-} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
deleted file mode 100644
index 26238eb8dcdf059a440b82ce149f5daef8ae20d7..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_base.cpp
+++ /dev/null
@@ -1,407 +0,0 @@
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/factor/factor_block_absolute.h"
-#include "core/factor/factor_quaternion_absolute.h"
-
-
-namespace wolf {
-
-unsigned int SensorBase::sensor_id_count_ = 0;
-
-SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const unsigned int _noise_size,
- const bool _extr_dyn,
- const bool _intr_dyn) :
- NodeBase("SENSOR", _type),
- hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- calib_size_(0),
- sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
- noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size)
-{
- noise_std_.setZero();
- noise_cov_.setZero();
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
- updateCalibSize();
-}
-
-SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn,
- const bool _intr_dyn) :
- NodeBase("SENSOR", _type),
- hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- calib_size_(0),
- sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
- noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size())
-{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
- setNoiseStd(_noise_std);
- updateCalibSize();
-}
-
-SensorBase::~SensorBase()
-{
- // Remove State Blocks
- removeStateBlocks();
-}
-
-void SensorBase::removeStateBlocks()
-{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr && !extrinsic_dynamic_)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlockPtrStatic(i, nullptr);
- }
- }
-}
-
-void SensorBase::fix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
- updateCalibSize();
-}
-
-void SensorBase::unfix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
- updateCalibSize();
-}
-
-void SensorBase::fixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void SensorBase::unfixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void SensorBase::fixIntrinsics()
-{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void SensorBase::unfixIntrinsics()
-{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
-{
- assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
- assert(_i < state_block_vec_.size() && "State block not found");
-
- StateBlockPtr _sb = getStateBlockPtrStatic(_i);
- bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
-
- assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
- (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
- assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
- assert(_size == -1 || _size == _x.size());
- assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion");
-
- // set StateBlock state
- if (_size == -1)
- _sb->setState(_x);
- else
- {
- Eigen::VectorXs new_x = _sb->getState();
- new_x.segment(_start_idx,_size) = _x;
- _sb->setState(new_x);
- }
-
- // remove previous prior (if any)
- if (params_prior_map_.find(_i) != params_prior_map_.end())
- params_prior_map_[_i]->remove();
-
- // create feature
- FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
-
- // set feature problem
- ftr_prior->setProblem(getProblem());
-
- // create & add factor absolute
- if (is_quaternion)
- ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
- else
- ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
-
- // store feature in params_prior_map_
- params_prior_map_[_i] = ftr_prior;
-}
-
-void SensorBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
- {
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
- {
- if (i < 2 && !isExtrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
- if (i >= 2 && !isIntrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
- }
- }
-}
-
-void SensorBase::setNoiseStd(const Eigen::VectorXs& _noise_std) {
- noise_std_ = _noise_std;
- noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
-}
-
-void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
- noise_std_ = _noise_cov.diagonal().array().sqrt();
- noise_cov_ = _noise_cov;
-}
-
-CaptureBasePtr SensorBase::lastKeyCapture(void)
-{
- // we search for the most recent Capture of this sensor which belongs to a KeyFrame
- CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
- {
- if ((*frame_rev_it)->isKey())
- {
- capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
- }
- frame_rev_it++;
- }
- return capture;
-}
-
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
-{
- // we search for the most recent Capture of this sensor before _ts
- CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-
- // We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
- {
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
- {
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
- }
- frame_rev_it++;
- }
- return capture;
-}
-
-StateBlockPtr SensorBase::getP(const TimeStamp _ts)
-{
- return getStateBlock(0, _ts);
-}
-
-StateBlockPtr SensorBase::getO(const TimeStamp _ts)
-{
- return getStateBlock(1, _ts);
-}
-
-StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
-{
- return getStateBlock(2, _ts);
-}
-
-StateBlockPtr SensorBase::getP()
-{
- return getStateBlock(0);
-}
-
-StateBlockPtr SensorBase::getO()
-{
- return getStateBlock(1);
-}
-
-StateBlockPtr SensorBase::getIntrinsic()
-{
- return getStateBlock(2);
-}
-
-SizeEigen SensorBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sb = state_block_vec_[i];
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXs SensorBase::getCalibration() const
-{
- SizeEigen index = 0;
- SizeEigen sz = getCalibSize();
- Eigen::VectorXs calib(sz);
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sb = getStateBlockPtrStatic(i);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
-bool SensorBase::process(const CaptureBasePtr capture_ptr)
-{
- capture_ptr->setSensor(shared_from_this());
-
- for (const auto processor : processor_list_)
- {
- processor->process(capture_ptr);
- }
-
- return true;
-}
-
-ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
-{
- processor_list_.push_back(_proc_ptr);
- _proc_ptr->setSensor(shared_from_this());
- _proc_ptr->setProblem(getProblem());
- return _proc_ptr;
-}
-
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
-{
- CaptureBasePtr cap;
-
- if (isStateBlockDynamic(_i, cap))
- return cap->getStateBlock(_i);
-
- return getStateBlockPtrStatic(_i);
-}
-
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
-{
- CaptureBasePtr cap;
-
- if (isStateBlockDynamic(_i, _ts, cap))
- return cap->getStateBlock(_i);
-
- return getStateBlockPtrStatic(_i);
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
-{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
- {
- cap = lastKeyCapture();
-
- return cap != nullptr;
- }
- else
- return false;
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap)
-{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
- {
- cap = lastCapture(_ts);
-
- return cap != nullptr;
- }
- else
- return false;
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i)
-{
- CaptureBasePtr cap;
-
- return isStateBlockDynamic(_i,cap);
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts)
-{
- CaptureBasePtr cap;
-
- return isStateBlockDynamic(_i,_ts,cap);
-}
-
-void SensorBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto prc : processor_list_)
- prc->setProblem(_problem);
-}
-
-} // namespace wolf
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
deleted file mode 100644
index 3e07aea5750b77c76f3abdb2bddab92ddb69d2f9..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_diff_drive.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-#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"
-
-namespace wolf {
-
-SensorDiffDrive::SensorDiffDrive(const StateBlockPtr& _p_ptr,
- const StateBlockPtr& _o_ptr,
- const StateBlockPtr& _i_ptr,
- const IntrinsicsDiffDrivePtr& _intrinsics) :
- SensorBase("DIFF DRIVE", _p_ptr, _o_ptr, _i_ptr, 2, false, false),
- intrinsics_ptr_{_intrinsics}
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
- const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- Eigen::VectorSizeCheck<3>::check(_extrinsics_po);
-
- // cast intrinsics into derived type
- IntrinsicsDiffDrivePtr params = std::dynamic_pointer_cast<IntrinsicsDiffDrive>(_intrinsics);
-
- if (params == nullptr)
- {
- WOLF_ERROR("SensorDiffDrive::create expected IntrinsicsDiffDrive !");
- return nullptr;
- }
-
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
- StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, true);
-
- SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params);
-
- odo->setName(_unique_name);
-
- /// @todo make calibration optional at creation
- //if (calibrate)
- // odo->unfixIntrinsics();
-
- return odo;
-}
-
-/// @todo Further work to enforce wheel model
-
-//const IntrinsicsDiffDrive& SensorDiffDrive::getIntrinsics()
-//{
-//// if (intrinsic_ptr_)
-//// {
-//// const auto& intrinsics = intrinsic_ptr_->getVector();
-
-//// intrinsics_.left_radius_factor_ = intrinsics(0);
-//// intrinsics_.right_radius_factor_ = intrinsics(1);
-//// intrinsics_.separation_factor_ = intrinsics(2);
-//// }
-
-// return intrinsics_;
-//}
-
-//void SensorDiffDrive::initIntrisics()
-//{
-// assert(intrinsic_ptr_ == nullptr &&
-// "SensorDiffDrive::initIntrisicsPtr should only be called once at construction.");
-
-// Eigen::Vector3s state;
-// state << intrinsics_.left_radius_factor_,
-// intrinsics_.right_radius_factor_,
-// intrinsics_.separation_factor_;
-
-// assert(state(0) > 0 && "The left_radius_factor should be rather close to 1.");
-// assert(state(1) > 0 && "The right_radius_factor should be rather close to 1.");
-// assert(state(2) > 0 && "The separation_factor should be rather close to 1.");
-
-// intrinsic_ptr_ = new StateBlock(state);
-//}
-
-//void SensorDiffDrive::computeDataCov(const Eigen::Vector2s &_data, Eigen::Matrix2s &_data_cov)
-//{
-// const double dp_std_l = intrinsics_.left_gain_ * _data(0);
-// const double dp_std_r = intrinsics_.right_gain_ * _data(1);
-
-// const double dp_var_l = dp_std_l * dp_std_l;
-// const double dp_var_r = dp_std_r * dp_std_r;
-
-// /// Wheel resolution covariance, which is like a DC offset equal to half of
-// /// the resolution, which is the theoretical average error:
-// const double dp_std_avg = Scalar(0.5) * intrinsics_.left_resolution_;
-// const double dp_var_avg = dp_std_avg * dp_std_avg;
-
-// /// Set covariance matrix (diagonal):
-// _data_cov.diagonal() << dp_var_l + dp_var_avg,
-// dp_var_r + dp_var_avg;
-//}
-
-// This overload is probably not the best solution as it forces
-// me to call addCapture from a SensorDiffDrivePtr whereas
-// problem->installSensor() return a SensorBasePtr.
-//bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
-//{
-// if (intrinsics_.data_is_position_)
-// {
-// Eigen::Vector2s data = _capture_ptr->getData();
-
-// // dt is set to one as we are dealing with wheel position
-// data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
-// intrinsics_.separation_, 1);
-
-// _capture_ptr->setData(data);
-
-// Eigen::Matrix2s data_cov;
-// data_cov << 0.00001, 0, 0, 0.00001; // Todo
-
-// computeDataCov(data, data_cov);
-
-// _capture_ptr->setDataCovariance(data_cov);
-// }
-
-// /// @todo tofix
-// return false;//SensorBase::addCapture(_capture_ptr);
-//}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
-} // namespace wolf
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
deleted file mode 100644
index 41c5fb00058dacf4172dc1ed8ee741a1dde50768..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_laser_2D.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-#include "laser/sensor/sensor_laser_2D.h"
-#include "core/state_block/state_block.h"
-
-namespace wolf {
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8)
-{
- setDefaultScanParams();
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_({ _angle_min, _angle_max, _angle_step, _scan_time, _range_min, _range_max, _range_std_dev, _angle_std_dev })
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_(_params)
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_(_params.scan_params)
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) :
- SensorLaser2D(_p_ptr, _o_ptr, *_params)
-{
- //
-}
-
-SensorLaser2D::~SensorLaser2D()
-{
- //
-}
-
-void SensorLaser2D::setDefaultScanParams()
-{
- scan_params_.angle_min_ = M_PI_2;
- scan_params_.angle_max_ = -M_PI_2;
- scan_params_.angle_step_ = -M_PI/720;
- scan_params_.scan_time_ = 0.01;//not relevant
- scan_params_.range_min_ = 0.2;
- scan_params_.range_max_ = 100;
- scan_params_.range_std_dev_ = 0.01;
-
- setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
-}
-
-void SensorLaser2D::setScanParams(const laserscanutils::LaserScanParams & _params)
-{
- scan_params_ = _params;
-}
-
-const laserscanutils::LaserScanParams& SensorLaser2D::getScanParams() const
-{
- return scan_params_;
-}
-
-// Define the factory method
-SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
- // cast intrinsics into derived type
- IntrinsicsLaser2DPtr params = std::static_pointer_cast<IntrinsicsLaser2D>(_intrinsics);
- SensorLaser2DPtr sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params->scan_params);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory and the ParameterFactory
-#include "core/sensor/sensor_factory.h"
-//#include "intrinsics_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D)
-//const bool registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
-} // namespace wolf
diff --git a/src/sensor/sensor_laser_2d.cpp b/src/sensor/sensor_laser_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55740c1a7ee57533e1b55d35e785d77f48ba89f5
--- /dev/null
+++ b/src/sensor/sensor_laser_2d.cpp
@@ -0,0 +1,116 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/sensor/sensor_laser_2d.h"
+
+#include <core/state_block/state_block.h>
+#include <core/state_block/state_angle.h>
+
+namespace wolf {
+
+SensorLaser2d::SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
+ SensorBase("SensorLaser2d", _p_ptr, _o_ptr, nullptr, 8)
+{
+ setDefaultScanParams();
+}
+
+SensorLaser2d::SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev) :
+ SensorBase("SensorLaser2d", _p_ptr, _o_ptr, nullptr, 8),
+ scan_params_({ _angle_min, _angle_max, _angle_step, _scan_time, _range_min, _range_max, _range_std_dev, _angle_std_dev })
+{
+ //
+}
+
+SensorLaser2d::SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params) :
+ SensorBase("SensorLaser2d", _p_ptr, _o_ptr, nullptr, 8),
+ scan_params_(_params)
+{
+ //
+}
+
+SensorLaser2d::SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const ParamsSensorLaser2d& _params) :
+ SensorBase("SensorLaser2d", _p_ptr, _o_ptr, nullptr, 8),
+ scan_params_(_params.scan_params)
+{
+ //
+}
+
+SensorLaser2d::SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const ParamsSensorLaser2dPtr _params) :
+ SensorBase("SensorLaser2d", _p_ptr, _o_ptr, nullptr, 8),
+ scan_params_(_params->scan_params)
+{
+ //
+}
+
+SensorLaser2d::SensorLaser2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorLaser2dPtr _params) :
+ SensorBase("SensorLaser2d",
+ std::make_shared<StateBlock>(_extrinsics.head(2), true),
+ std::make_shared<StateAngle>(_extrinsics(2), true),
+ nullptr,
+ 8),
+ scan_params_(_params->scan_params)
+{
+ //
+}
+
+SensorLaser2d::~SensorLaser2d()
+{
+ //
+}
+
+void SensorLaser2d::setDefaultScanParams()
+{
+ scan_params_.angle_min_ = M_PI_2;
+ scan_params_.angle_max_ = -M_PI_2;
+ scan_params_.angle_step_ = -M_PI/720;
+ scan_params_.scan_time_ = 0.01;//not relevant
+ scan_params_.range_min_ = 0.2;
+ scan_params_.range_max_ = 100;
+ scan_params_.range_std_dev_ = 0.01;
+
+ setNoiseStd(Eigen::VectorXd::Constant(1,scan_params_.range_std_dev_));
+
+ for(auto proc : getProcessorList())
+ proc->configure(shared_from_this());
+}
+
+void SensorLaser2d::setScanParams(const laserscanutils::LaserScanParams & _params)
+{
+ scan_params_ = _params;
+ for(auto proc : getProcessorList())
+ proc->configure(shared_from_this());
+}
+
+const laserscanutils::LaserScanParams& SensorLaser2d::getScanParams() const
+{
+ return scan_params_;
+}
+
+
+} // namespace wolf
+
+// Register in the FactorySensor and the ParameterFactory
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorLaser2d)
+WOLF_REGISTER_SENSOR_AUTO(SensorLaser2d)
+} // namespace wolf
+
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
deleted file mode 100644
index e3e5d860f2960f8775a292c371b345d51d2dd70f..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_odom_2D.cpp
+++ /dev/null
@@ -1,67 +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;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D)
-} // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
deleted file mode 100644
index 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/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
deleted file mode 100644
index 05d89142a35cc740c69c29712565d132d69fbe93..0000000000000000000000000000000000000000
--- a/src/solver_suitesparse/solver_manager.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-#include "core/ceres_wrapper/ceres_manager.h"
-
-SolverManager::SolverManager()
-{
-
-}
-
-SolverManager::~SolverManager()
-{
- removeAllStateUnits();
-}
-
-void SolverManager::solve()
-{
-
-}
-
-//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
-//{
-//}
-
-void SolverManager::update(const WolfProblemPtr _problem_ptr)
-{
- // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
- if (_problem_ptr->isReallocated())
- {
- // todo: reallocate x
- }
- else
- {
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
- {
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
-
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
- }
- //std::cout << "state units updated!" << std::endl;
-
- // REMOVE STATE UNITS
- while (!_problem_ptr->getRemovedStateList().empty())
- {
- // TODO: remove state unit
- //_problem_ptr->getRemovedStateList().pop_front();
- }
- //std::cout << "state units removed!" << std::endl;
-
- // ADD CONSTRAINTS
- FactorBasePtrList fac_list;
- _problem_ptr->getTrajectory()->getFactorList(fac_list);
- //std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
- for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
- if ((*fac_it)->getPendingStatus() == ADD_PENDING)
- addFactor(*fac_it);
-
- //std::cout << "factors updated!" << std::endl;
- }
-}
-
-void SolverManager::addFactor(FactorBasePtr _corr_ptr)
-{
- //TODO MatrixXs J; Vector e;
- // getResidualsAndJacobian(_corr_ptr, J, e);
- // solverQR->addFactor(_corr_ptr, J, e);
-
-// factor_map_[_corr_ptr->id()] = blockIdx;
- _corr_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeFactor(const unsigned int& _corr_idx)
-{
- // TODO
-}
-
-void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
-{
- //std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- // TODO
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_1D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_VECTOR:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- if (_st_ptr->isFixed())
- updateStateUnitStatus(_st_ptr);
-
- _st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeAllStateUnits()
-{
- std::vector<double*> parameter_blocks;
-
- ceres_problem_->GetParameterBlocks(¶meter_blocks);
-
- for (unsigned int i = 0; i< parameter_blocks.size(); i++)
- ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
-}
-
-void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
-{
- // TODO
-
-// if (!_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
-// else if (_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
-// else
-// printf("\nERROR: Update state unit status with unknown status");
-//
-// _st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
-{
- //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
- //_corrPtr->print();
-
- switch (_corrPtr->getFactorType())
- {
- case FAC_GPS_FIX_2D:
- {
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorGPS2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2D_COMPLEX_ANGLE:
- {
- FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2D:
- {
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_CORNER_2D:
- {
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorCorner2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_IMU:
- {
- FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorIMU,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
-}
diff --git a/src/state_block/local_parametrization_polyline_extreme.cpp b/src/state_block/local_parametrization_polyline_extreme.cpp
index 4116f5c031a64b9b54c331de31582ad881594b47..6f5a6e89b9ec1e3ef295697e222a333c0aeda596 100644
--- a/src/state_block/local_parametrization_polyline_extreme.cpp
+++ b/src/state_block/local_parametrization_polyline_extreme.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#include "laser/state_block/local_parametrization_polyline_extreme.h"
#include "core/state_block/state_block.h"
#include "core/math/rotations.h"
@@ -14,25 +35,22 @@ LocalParametrizationPolylineExtreme::~LocalParametrizationPolylineExtreme()
{
}
-bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const
+bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXd>& _point,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _point_plus_delta_theta) const
{
assert(_point.size() == global_size_ && "Wrong size of input point.");
assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
assert(_point_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
- _point_plus_delta_theta = reference_point_->getState().head(2) + Eigen::Rotation2Ds(_delta_theta(0)) * (_point - reference_point_->getState().head(2));
-
- assert(std::fabs( (_point_plus_delta_theta - reference_point_->getState().head(2)).squaredNorm()
- -(_point - reference_point_->getState().head(2)).squaredNorm() ) < Constants::EPS);
+ _point_plus_delta_theta = reference_point_->getState().head(2) + Eigen::Rotation2Dd(_delta_theta(0)) * (_point - reference_point_->getState().head(2));
return true;
}
-bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
assert(_point.size() == global_size_ && "Wrong size of input point.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
@@ -43,12 +61,12 @@ bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen
return true;
}
-bool LocalParametrizationPolylineExtreme::minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta)
+bool LocalParametrizationPolylineExtreme::minus(Eigen::Map<const Eigen::VectorXd>& _point1,
+ Eigen::Map<const Eigen::VectorXd>& _point2,
+ Eigen::Map<Eigen::VectorXd>& _delta_theta)
{
- Eigen::Vector2s v1 = _point1 - reference_point_->getState().head(2);
- Eigen::Vector2s v2 = _point2 - reference_point_->getState().head(2);
+ Eigen::Vector2d v1 = _point1 - reference_point_->getState().head(2);
+ Eigen::Vector2d v2 = _point2 - reference_point_->getState().head(2);
_delta_theta(0) = pi2pi(atan2(v2(1),v2(0)) - atan2(v1(1),v1(0)));
diff --git a/src/yaml/processor_odom_icp_yaml.cpp b/src/yaml/processor_odom_icp_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0d4d68b039b94e7d5b0f7ff879cb6b3826bb158a
--- /dev/null
+++ b/src/yaml/processor_odom_icp_yaml.cpp
@@ -0,0 +1,91 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file processor_odom_Icp_yaml.cpp
+ *
+ * Created on: Aug 6, 2019
+ * \author: jsola
+ */
+
+
+// wolf
+#include "laser/processor/processor_odom_icp.h"
+#include "core/common/factory.h"
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// yaml library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+namespace {
+// intrinsics creator
+ParamsProcessorBasePtr createParamsProcessorOdomIcp(const std::string& _filename_dot_yaml)
+{
+ WOLF_INFO("ParamsProcessorOdomIcp: Parsing file: ", _filename_dot_yaml);
+
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "ProcessorOdomIcp") {
+
+ ParamsProcessorOdomIcpPtr params = std::make_shared<ParamsProcessorOdomIcp>();
+
+ params->voting_active = config["voting_active"] .as<bool>();
+ params->time_tolerance = config["time_tolerance"] .as<double>();
+
+ params->min_features_for_keyframe = config["min_features_for_keyframe"] .as<double>();
+ params->max_new_features = config["max_new_features"] .as<SizeEigen>();
+
+ params->icp_params.cov_factor = config["cov_factor"] .as<double>();
+ params->icp_params.use_point_to_line_distance = config["use_point_to_line_distance"] .as<bool>();
+ params->icp_params.max_correspondence_dist = config["max_correspondence_dist"] .as<int>();
+ params->icp_params.max_iterations = config["max_iterations"] .as<int>();
+ params->icp_params.use_corr_tricks = config["use_corr_tricks"] .as<bool>();
+ params->icp_params.outliers_maxPerc = config["outliers_maxPerc"] .as<double>();
+ params->icp_params.outliers_adaptive_order = config["outliers_adaptive_order"] .as<double>();
+ params->icp_params.outliers_adaptive_mult = config["outliers_adaptive_mult"] .as<double>();
+ params->icp_params.min_reading = config["min_reading"] .as<double>();
+ params->icp_params.max_reading = config["max_reading"] .as<double>();
+ params->icp_params.do_compute_covariance = config["do_compute_covariance"] .as<bool>();
+
+ params->vfk_min_dist = config["vfk_min_dist"] .as<double>();
+ params->vfk_min_angle = config["vfk_min_angle"] .as<double>();
+ params->vfk_min_time = config["vfk_min_time"] .as<double>();
+ params->vfk_min_error = config["vfk_min_error"] .as<double>();
+ params->vfk_max_points = config["vfk_max_points"] .as<int>();
+
+ params->initial_guess = config["initial_guess"] .as<std::string>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No processor type found." << std::endl;
+ return nullptr;
+}
+
+// register into factory
+const bool WOLF_UNUSED registered_odom_Icp_params = FactoryParamsProcessor::registerCreator("ProcessorOdomIcp", createParamsProcessorOdomIcp);
+
+} // namespace [void]
+} // namespace wolf
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
deleted file mode 100644
index d741f365f3c12239074fb5350e4b269d0a23cfc2..0000000000000000000000000000000000000000
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/**
- * \file sensor_laser_2D_yaml.cpp
- *
- * Created on: May 13, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-//#include "core/intrinsics_factory.h"
-#include "core/common/factory.h"
-#include "laser/sensor/sensor_laser_2D.h"
-
-// yaml library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-namespace {
-// intrinsics creator
-IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
-{
- // If required: Parse YAML
-
- IntrinsicsLaser2DPtr params; // dummy
- return params;
-}
-
-// register into factory
-const bool WOLF_UNUSED registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
-
-} // namespace [void]
-} // namespace wolf
diff --git a/src/yaml/sensor_laser_2d_yaml.cpp b/src/yaml/sensor_laser_2d_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc1329162cc0f9bb3f5859fab1fe711c99fbac67
--- /dev/null
+++ b/src/yaml/sensor_laser_2d_yaml.cpp
@@ -0,0 +1,79 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file sensor_laser_2d_yaml.cpp
+ *
+ * Created on: May 13, 2016
+ * \author: jsola
+ */
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+//#include "core/intrinsics_factory.h"
+#include "core/common/factory.h"
+#include "laser/sensor/sensor_laser_2d.h"
+
+// yaml library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+namespace {
+// intrinsics creator
+ParamsSensorBasePtr createIntrinsicsLaser2d(const std::string& _filename_dot_yaml)
+{
+ // If required: Parse YAML
+
+ WOLF_INFO("ParamsSensorLaser2d: Parsing file: ", _filename_dot_yaml);
+
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "SensorLaser2d")
+ {
+ YAML::Node ls_params = config["LaserScanParams"];
+
+ ParamsSensorLaser2dPtr params = std::make_shared<ParamsSensorLaser2d>();
+
+ params->scan_params.angle_min_ = ls_params["angle_min"] .as<double>();
+ params->scan_params.angle_max_ = ls_params["angle_max"] .as<double>();
+ params->scan_params.angle_step_ = ls_params["angle_step"] .as<double>();
+ params->scan_params.scan_time_ = ls_params["scan_time"] .as<double>();
+ params->scan_params.range_min_ = ls_params["range_min"] .as<double>();
+ params->scan_params.range_max_ = ls_params["range_max"] .as<double>();
+ params->scan_params.range_std_dev_ = ls_params["range_std_dev"] .as<double>();
+ params->scan_params.angle_std_dev_ = ls_params["angle_std_dev"] .as<double>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file " << _filename_dot_yaml << ". No processor type found." << std::endl;
+ return nullptr;
+
+}
+
+// register into factory
+const bool WOLF_UNUSED registered_laser_params = FactoryParamsSensor::registerCreator("SensorLaser2d", createIntrinsicsLaser2d);
+
+} // namespace [void]
+} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c54abc72d7b5d6553c85f2f3e0aea62c0635f55a..c75a1957be69ff023966f3c9d0c59dd1fd7eb0f6 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,7 +1,6 @@
# Retrieve googletest from github & compile
add_subdirectory(gtest)
-
# Include gtest directory.
include_directories(${GTEST_INCLUDE_DIRS})
@@ -9,15 +8,30 @@ 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}) #
# #
###########################################################
wolf_add_gtest(gtest_landmark_polyline gtest_landmark_polyline.cpp)
-target_link_libraries(gtest_landmark_polyline ${PROJECT_NAME} ${wolf_LIBRARY})
+target_link_libraries(gtest_landmark_polyline ${PLUGIN_NAME})
+
+if(csm_FOUND)
+ wolf_add_gtest(gtest_processor_odom_icp gtest_processor_odom_icp.cpp)
+ target_link_libraries(gtest_processor_odom_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+ wolf_add_gtest(gtest_processor_loop_closure_icp gtest_processor_loop_closure_icp.cpp)
+ target_link_libraries(gtest_processor_loop_closure_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+endif(csm_FOUND)
+
+wolf_add_gtest(gtest_polyline_2d gtest_polyline_2d.cpp)
+target_link_libraries(gtest_polyline_2d ${PLUGIN_NAME})
-wolf_add_gtest(gtest_polyline_2D gtest_polyline_2D.cpp)
-target_link_libraries(gtest_polyline_2D ${PROJECT_NAME} ${wolf_LIBRARY})
+if(falkolib_FOUND)
+ wolf_add_gtest(gtest_processor_loop_closure_falko gtest_processor_loop_closure_falko.cpp)
+ target_link_libraries(gtest_processor_loop_closure_falko ${PLUGIN_NAME} ${wolf_LIBRARY})
-# wolf_add_gtest(gtest_processor_tracker_feature_polyline_2D gtest_processor_tracker_feature_polyline_2D.cpp)
-# target_link_libraries(gtest_processor_tracker_feature_polyline_2D ${PROJECT_NAME} ${wolf_LIBRARY})
\ No newline at end of file
+ if(csm_FOUND)
+ wolf_add_gtest(gtest_processor_loop_closure_falko_icp gtest_processor_loop_closure_falko_icp.cpp)
+ target_link_libraries(gtest_processor_loop_closure_falko_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+ endif(csm_FOUND)
+endif(falkolib_FOUND)
diff --git a/test/data/scan_data.h b/test/data/scan_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..9fea58fea0ce8aa47431f15afe9d5ea0314652dd
--- /dev/null
+++ b/test/data/scan_data.h
@@ -0,0 +1,2929 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+std::vector<float> testRanges1 = {250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 17.263999938964844,
+ 17.263999938964844,
+ 17.263999938964844,
+ 17.263999938964844,
+ 13.552000045776367,
+ 13.552000045776367,
+ 13.552000045776367,
+ 12.95199966430664,
+ 12.95199966430664,
+ 12.95199966430664,
+ 12.95199966430664,
+ 12.51200008392334,
+ 11.567999839782715,
+ 11.567999839782715,
+ 10.720000267028809,
+ 10.720000267028809,
+ 10.720000267028809,
+ 10.359999656677246,
+ 10.336000442504883,
+ 10.343999862670898,
+ 10.35200023651123,
+ 10.35200023651123,
+ 10.32800006866455,
+ 10.319999694824219,
+ 10.383999824523926,
+ 10.407999992370605,
+ 10.432000160217285,
+ 10.432000160217285,
+ 10.447999954223633,
+ 10.46399974822998,
+ 10.46399974822998,
+ 10.503999710083008,
+ 10.503999710083008,
+ 10.527999877929688,
+ 10.592000007629395,
+ 10.62399959564209,
+ 8.368000030517578,
+ 8.343999862670898,
+ 8.343999862670898,
+ 8.031999588012695,
+ 7.831999778747559,
+ 7.6479997634887695,
+ 7.552000045776367,
+ 7.415999889373779,
+ 7.415999889373779,
+ 7.263999938964844,
+ 7.144000053405762,
+ 7.0320000648498535,
+ 6.920000076293945,
+ 6.776000022888184,
+ 6.776000022888184,
+ 6.671999931335449,
+ 6.576000213623047,
+ 6.440000057220459,
+ 6.328000068664551,
+ 6.271999835968018,
+ 6.271999835968018,
+ 6.263999938964844,
+ 6.271999835968018,
+ 6.296000003814697,
+ 6.320000171661377,
+ 6.328000068664551,
+ 6.328000068664551,
+ 6.335999965667725,
+ 6.320000171661377,
+ 3.187999963760376,
+ 3.187999963760376,
+ 4.495999813079834,
+ 4.495999813079834,
+ 4.495999813079834,
+ 4.495999813079834,
+ 4.504000186920166,
+ 4.504000186920166,
+ 4.504000186920166,
+ 4.544000148773193,
+ 4.544000148773193,
+ 4.544000148773193,
+ 4.544000148773193,
+ 4.51200008392334,
+ 4.51200008392334,
+ 4.51200008392334,
+ 4.51200008392334,
+ 4.599999904632568,
+ 4.576000213623047,
+ 4.495999813079834,
+ 4.455999851226807,
+ 4.455999851226807,
+ 4.455999851226807,
+ 4.455999851226807,
+ 4.4079999923706055,
+ 4.368000030517578,
+ 4.320000171661377,
+ 4.271999835968018,
+ 4.271999835968018,
+ 4.223999977111816,
+ 4.184000015258789,
+ 4.168000221252441,
+ 4.159999847412109,
+ 4.168000221252441,
+ 4.176000118255615,
+ 4.176000118255615,
+ 4.184000015258789,
+ 4.199999809265137,
+ 4.199999809265137,
+ 4.208000183105469,
+ 4.216000080108643,
+ 4.216000080108643,
+ 4.223999977111816,
+ 4.23199987411499,
+ 4.23199987411499,
+ 4.176000118255615,
+ 250,
+ 250,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.099999904632568,
+ 4.099999904632568,
+ 4.099999904632568,
+ 4.079999923706055,
+ 4.0320000648498535,
+ 3.98799991607666,
+ 3.9719998836517334,
+ 3.9719998836517334,
+ 3.9560000896453857,
+ 3.9240000247955322,
+ 3.888000011444092,
+ 3.859999895095825,
+ 3.8320000171661377,
+ 3.8320000171661377,
+ 3.803999900817871,
+ 3.7880001068115234,
+ 3.259999990463257,
+ 3.240000009536743,
+ 3.2200000286102295,
+ 3.2200000286102295,
+ 3.200000047683716,
+ 3.1760001182556152,
+ 3.1559998989105225,
+ 3.1440000534057617,
+ 3.128000020980835,
+ 3.128000020980835,
+ 3.119999885559082,
+ 2.5959999561309814,
+ 2.5959999561309814,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.4839999675750732,
+ 2.4839999675750732,
+ 2.4839999675750732,
+ 2.4839999675750732,
+ 2.496000051498413,
+ 2.5320000648498535,
+ 250,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.6679999828338623,
+ 2.6679999828338623,
+ 2.6679999828338623,
+ 2.6679999828338623,
+ 2.640000104904175,
+ 2.640000104904175,
+ 2.640000104904175,
+ 2.628000020980835,
+ 2.615999937057495,
+ 2.615999937057495,
+ 2.6080000400543213,
+ 2.5959999561309814,
+ 2.5880000591278076,
+ 2.5759999752044678,
+ 2.563999891281128,
+ 2.563999891281128,
+ 2.555999994277954,
+ 2.5439999103546143,
+ 2.5320000648498535,
+ 2.5199999809265137,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.503999948501587,
+ 2.492000102996826,
+ 2.4839999675750732,
+ 2.4719998836517334,
+ 2.4639999866485596,
+ 2.4639999866485596,
+ 2.4600000381469727,
+ 2.4519999027252197,
+ 2.440000057220459,
+ 2.431999921798706,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4079999923706055,
+ 2.4079999923706055,
+ 2.427999973297119,
+ 2.444000005722046,
+ 2.444000005722046,
+ 2.4600000381469727,
+ 2.4719998836517334,
+ 2.48799991607666,
+ 2.5,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5280001163482666,
+ 2.5399999618530273,
+ 2.559999942779541,
+ 2.5799999237060547,
+ 2.5840001106262207,
+ 2.5840001106262207,
+ 3.0480000972747803,
+ 3.0480000972747803,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.615999937057495,
+ 2.611999988555908,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.5959999561309814,
+ 2.5920000076293945,
+ 2.5840001106262207,
+ 2.5759999752044678,
+ 2.572000026702881,
+ 2.572000026702881,
+ 2.563999891281128,
+ 2.555999994277954,
+ 2.5480000972747803,
+ 2.5439999103546143,
+ 2.5399999618530273,
+ 2.5399999618530273,
+ 2.5360000133514404,
+ 2.5320000648498535,
+ 2.5239999294281006,
+ 2.5199999809265137,
+ 2.507999897003174,
+ 2.507999897003174,
+ 2.503999948501587,
+ 2.5,
+ 2.496000051498413,
+ 2.492000102996826,
+ 2.48799991607666,
+ 2.48799991607666,
+ 2.4800000190734863,
+ 2.4760000705718994,
+ 2.4719998836517334,
+ 2.4679999351501465,
+ 2.4639999866485596,
+ 2.4560000896453857,
+ 2.4560000896453857,
+ 2.4519999027252197,
+ 2.447999954223633,
+ 2.447999954223633,
+ 2.444000005722046,
+ 2.436000108718872,
+ 2.436000108718872,
+ 2.431999921798706,
+ 2.427999973297119,
+ 2.4240000247955322,
+ 2.4240000247955322,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4079999923706055,
+ 2.4040000438690186,
+ 2.4040000438690186,
+ 2.4000000953674316,
+ 2.4000000953674316,
+ 2.3959999084472656,
+ 2.3919999599456787,
+ 2.3919999599456787,
+ 2.388000011444092,
+ 2.384000062942505,
+ 2.384000062942505,
+ 2.380000114440918,
+ 2.375999927520752,
+ 2.371999979019165,
+ 2.371999979019165,
+ 2.368000030517578,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.359999895095825,
+ 2.3559999465942383,
+ 2.3559999465942383,
+ 2.3559999465942383,
+ 2.6600000858306885,
+ 2.6600000858306885,
+ 2.6600000858306885,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.0,
+ 2.0,
+ 2.0,
+ 2.0,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 2.0,
+ 2.0,
+ 2.009999990463257,
+ 2.0220000743865967,
+ 2.0460000038146973,
+ 2.0460000038146973,
+ 2.0260000228881836,
+ 2.0260000228881836,
+ 2.00600004196167,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9900000095367432,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.996000051498413,
+ 1.9980000257492065,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.996000051498413,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 2.0,
+ 2.0,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9919999837875366,
+ 1.9520000219345093,
+ 2.0320000648498535,
+ 2.0999999046325684,
+ 2.1040000915527344,
+ 2.1040000915527344,
+ 2.0280001163482666,
+ 1.9040000438690186,
+ 1.9279999732971191,
+ 1.9520000219345093,
+ 1.9559999704360962,
+ 1.9559999704360962,
+ 1.9600000381469727,
+ 1.9579999446868896,
+ 1.9600000381469727,
+ 1.9620000123977661,
+ 1.9639999866485596,
+ 1.9639999866485596,
+ 1.965999960899353,
+ 1.965999960899353,
+ 1.968000054359436,
+ 1.972000002861023,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9800000190734863,
+ 1.9839999675750732,
+ 1.9800000190734863,
+ 1.9800000190734863,
+ 1.9819999933242798,
+ 1.9880000352859497,
+ 1.996000051498413,
+ 2.009999990463257,
+ 2.00600004196167,
+ 2.00600004196167,
+ 1.99399995803833,
+ 2.687999963760376,
+ 2.687999963760376,
+ 2.687999963760376,
+ 1.74399995803833,
+ 1.74399995803833,
+ 2.063999891281128,
+ 2.115999937057495,
+ 2.196000099182129,
+ 2.191999912261963,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5199999809265137,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5360000133514404,
+ 2.5399999618530273,
+ 2.5480000972747803,
+ 2.5480000972747803,
+ 2.5480000972747803,
+ 2.5399999618530273,
+ 2.5439999103546143,
+ 2.552000045776367,
+ 2.555999994277954,
+ 2.555999994277954,
+ 2.552000045776367,
+ 2.5480000972747803,
+ 250,
+ 1.3179999589920044,
+ 1.3179999589920044,
+ 1.3179999589920044,
+ 1.3179999589920044,
+ 1.3079999685287476,
+ 1.2999999523162842,
+ 1.2940000295639038,
+ 1.2940000295639038,
+ 1.2879999876022339,
+ 1.2860000133514404,
+ 1.2940000295639038,
+ 1.3040000200271606,
+ 1.3040000200271606,
+ 1.3040000200271606,
+ 1.2999999523162842,
+ 1.2979999780654907,
+ 1.3020000457763672,
+ 1.309999942779541,
+ 1.3140000104904175,
+ 1.3140000104904175,
+ 1.3140000104904175,
+ 1.3240000009536743,
+ 1.3359999656677246,
+ 1.3420000076293945,
+ 1.343999981880188,
+ 1.343999981880188,
+ 1.2200000286102295,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.0920000076293945,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.100000023841858,
+ 1.100000023841858,
+ 1.100000023841858,
+ 1.100000023841858,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0959999561309814,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.0920000076293945,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.1019999980926514,
+ 1.1119999885559082,
+ 1.121999979019165,
+ 1.1260000467300415,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 5.791999816894531,
+ 5.791999816894531,
+ 5.791999816894531,
+ 5.791999816894531,
+ 5.776000022888184,
+ 5.776000022888184,
+ 5.776000022888184,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.736000061035156,
+ 5.679999828338623,
+ 5.671999931335449,
+ 5.671999931335449,
+ 5.631999969482422,
+ 5.631999969482422,
+ 5.552000045776367,
+ 5.552000045776367,
+ 5.552000045776367,
+ 5.552000045776367,
+ 5.559999942779541,
+ 5.584000110626221,
+ 6.104000091552734,
+ 6.783999919891357,
+ 6.831999778747559,
+ 6.831999778747559,
+ 6.831999778747559,
+ 6.760000228881836,
+ 6.616000175476074,
+ 6.455999851226807,
+ 6.343999862670898,
+ 6.343999862670898,
+ 6.320000171661377,
+ 6.311999797821045,
+ 9.192000389099121,
+ 9.175999641418457,
+ 9.15999984741211,
+ 9.15999984741211,
+ 9.128000259399414,
+ 9.119999885559082,
+ 7.992000102996826,
+ 7.9120001792907715,
+ 7.8480000495910645,
+ 7.8480000495910645,
+ 7.800000190734863,
+ 7.74399995803833,
+ 7.71999979019165,
+ 7.711999893188477,
+ 7.711999893188477,
+ 7.711999893188477,
+ 7.711999893188477,
+ 7.71999979019165,
+ 7.71999979019165,
+ 7.711999893188477,
+ 7.703999996185303,
+ 7.703999996185303,
+ 7.703999996185303,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.671999931335449,
+ 8.67199993133545,
+ 8.640000343322754,
+ 8.607999801635742,
+ 8.583999633789062,
+ 8.583999633789062,
+ 8.5600004196167,
+ 8.552000045776367,
+ 8.53600025177002,
+ 8.520000457763672,
+ 8.503999710083008,
+ 8.503999710083008,
+ 8.472000122070312,
+ 8.447999954223633,
+ 8.416000366210938,
+ 8.383999824523926,
+ 8.383999824523926,
+ 8.383999824523926,
+ 8.368000030517578,
+ 8.35200023651123,
+ 8.35200023651123,
+ 8.336000442504883,
+ 8.303999900817871,
+ 8.303999900817871,
+ 8.263999938964844,
+ 8.239999771118164,
+ 8.279999732971191,
+ 8.392000198364258,
+ 8.496000289916992,
+ 8.496000289916992,
+ 8.656000137329102,
+ 8.807999610900879,
+ 8.928000450134277,
+ 9.015999794006348,
+ 11.767999649047852,
+ 11.767999649047852,
+ 12.071999549865723,
+ 12.175999641418457,
+ 12.567999839782715,
+ 12.663999557495117,
+ 13.064000129699707,
+ 13.064000129699707,
+ 13.168000221252441,
+ 13.607999801635742,
+ 13.640000343322754,
+ 13.53600025177002,
+ 13.968000411987305,
+ 13.968000411987305,
+ 14.008000373840332,
+ 14.104000091552734,
+ 14.175999641418457,
+ 14.104000091552734,
+ 15.4399995803833,
+ 15.4399995803833,
+ 15.543999671936035,
+ 15.48799991607666,
+ 15.48799991607666,
+ 15.503999710083008,
+ 15.479999542236328,
+ 15.479999542236328,
+ 15.447999954223633,
+ 15.399999618530273,
+ 15.37600040435791,
+ 15.35200023651123,
+ 15.319999694824219,
+ 15.319999694824219,
+ 15.272000312805176,
+ 15.272000312805176,
+ 15.336000442504883,
+ 15.303999900817871,
+ 15.248000144958496,
+ 15.248000144958496,
+ 14.744000434875488,
+ 15.303999900817871,
+ 15.272000312805176,
+ 15.248000144958496,
+ 15.215999603271484,
+ 15.223999977111816,
+ 15.223999977111816,
+ 15.248000144958496,
+ 16.784000396728516,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.672000885009766,
+ 16.687999725341797,
+ 16.719999313354492,
+ 16.70400047302246,
+ 16.607999801635742,
+ 16.607999801635742,
+ 16.624000549316406,
+ 16.6560001373291,
+ 16.23200035095215,
+ 16.143999099731445,
+ 16.06399917602539,
+ 16.06399917602539,
+ 16.016000747680664,
+ 17.472000122070312,
+ 19.007999420166016,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250};
+
+std::vector<float> testRanges2 = {250,
+ 250,
+ 250,
+ 250,
+ 16.799999237060547,
+ 16.799999237060547,
+ 16.799999237060547,
+ 16.799999237060547,
+ 14.095999717712402,
+ 14.095999717712402,
+ 14.095999717712402,
+ 14.095999717712402,
+ 13.255999565124512,
+ 13.255999565124512,
+ 13.255999565124512,
+ 12.767999649047852,
+ 12.175999641418457,
+ 12.175999641418457,
+ 10.911999702453613,
+ 10.4399995803833,
+ 10.4399995803833,
+ 10.4399995803833,
+ 10.32800006866455,
+ 10.32800006866455,
+ 10.32800006866455,
+ 10.343999862670898,
+ 10.336000442504883,
+ 10.312000274658203,
+ 10.368000030517578,
+ 10.368000030517578,
+ 10.407999992370605,
+ 10.472000122070312,
+ 10.48799991607666,
+ 10.520000457763672,
+ 10.48799991607666,
+ 10.48799991607666,
+ 10.472000122070312,
+ 10.503999710083008,
+ 10.51200008392334,
+ 10.51200008392334,
+ 10.520000457763672,
+ 10.520000457763672,
+ 8.295999526977539,
+ 8.13599967956543,
+ 7.984000205993652,
+ 7.791999816894531,
+ 7.624000072479248,
+ 7.624000072479248,
+ 7.5279998779296875,
+ 7.4079999923706055,
+ 7.263999938964844,
+ 7.119999885559082,
+ 7.007999897003174,
+ 7.007999897003174,
+ 6.9120001792907715,
+ 6.760000228881836,
+ 6.639999866485596,
+ 6.544000148773193,
+ 6.415999889373779,
+ 6.415999889373779,
+ 6.311999797821045,
+ 6.271999835968018,
+ 6.263999938964844,
+ 6.288000106811523,
+ 6.303999900817871,
+ 6.303999900817871,
+ 6.320000171661377,
+ 6.335999965667725,
+ 6.343999862670898,
+ 6.335999965667725,
+ 6.303999900817871,
+ 6.303999900817871,
+ 6.271999835968018,
+ 4.5920000076293945,
+ 250,
+ 4.480000019073486,
+ 4.480000019073486,
+ 4.480000019073486,
+ 4.480000019073486,
+ 4.4720001220703125,
+ 4.504000186920166,
+ 4.519999980926514,
+ 4.5279998779296875,
+ 4.5279998779296875,
+ 4.5279998779296875,
+ 4.5279998779296875,
+ 4.567999839782715,
+ 4.559999942779541,
+ 250,
+ 4.48799991607666,
+ 4.48799991607666,
+ 4.48799991607666,
+ 4.48799991607666,
+ 4.440000057220459,
+ 4.392000198364258,
+ 4.360000133514404,
+ 4.360000133514404,
+ 4.320000171661377,
+ 4.263999938964844,
+ 4.223999977111816,
+ 4.191999912261963,
+ 4.168000221252441,
+ 4.168000221252441,
+ 4.168000221252441,
+ 4.176000118255615,
+ 4.176000118255615,
+ 4.191999912261963,
+ 4.208000183105469,
+ 4.208000183105469,
+ 4.208000183105469,
+ 4.216000080108643,
+ 4.216000080108643,
+ 4.216000080108643,
+ 4.23199987411499,
+ 4.23199987411499,
+ 4.223999977111816,
+ 4.184000015258789,
+ 250,
+ 4.136000156402588,
+ 4.136000156402588,
+ 4.136000156402588,
+ 4.136000156402588,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.0960001945495605,
+ 4.064000129699707,
+ 4.015999794006348,
+ 4.015999794006348,
+ 3.9839999675750732,
+ 3.9679999351501465,
+ 3.947999954223633,
+ 3.9119999408721924,
+ 3.875999927520752,
+ 3.875999927520752,
+ 3.8480000495910645,
+ 3.8239998817443848,
+ 3.7039999961853027,
+ 3.5920000076293945,
+ 3.259999990463257,
+ 3.259999990463257,
+ 3.240000009536743,
+ 3.2160000801086426,
+ 3.196000099182129,
+ 3.171999931335449,
+ 3.1519999504089355,
+ 3.1519999504089355,
+ 3.140000104904175,
+ 3.124000072479248,
+ 3.115999937057495,
+ 2.5959999561309814,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 250,
+ 250,
+ 250,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 250,
+ 250,
+ 250,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.635999917984009,
+ 2.628000020980835,
+ 2.619999885559082,
+ 2.619999885559082,
+ 2.6080000400543213,
+ 2.5959999561309814,
+ 2.5880000591278076,
+ 2.5759999752044678,
+ 2.563999891281128,
+ 2.563999891281128,
+ 2.552000045776367,
+ 2.5399999618530273,
+ 2.5280001163482666,
+ 2.5199999809265137,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.5,
+ 2.48799991607666,
+ 2.4839999675750732,
+ 2.4719998836517334,
+ 2.4639999866485596,
+ 2.4639999866485596,
+ 2.4560000896453857,
+ 2.444000005722046,
+ 2.436000108718872,
+ 2.431999921798706,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4159998893737793,
+ 2.4200000762939453,
+ 2.431999921798706,
+ 2.444000005722046,
+ 2.4600000381469727,
+ 2.4600000381469727,
+ 2.4760000705718994,
+ 2.48799991607666,
+ 2.5,
+ 2.5160000324249268,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5480000972747803,
+ 2.563999891281128,
+ 2.5840001106262207,
+ 2.7079999446868896,
+ 2.684000015258789,
+ 2.684000015258789,
+ 2.5880000591278076,
+ 2.5880000591278076,
+ 2.5880000591278076,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.624000072479248,
+ 2.615999937057495,
+ 2.611999988555908,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.5920000076293945,
+ 2.5880000591278076,
+ 2.5840001106262207,
+ 2.5759999752044678,
+ 2.572000026702881,
+ 2.559999942779541,
+ 2.559999942779541,
+ 2.552000045776367,
+ 2.5480000972747803,
+ 2.5439999103546143,
+ 2.5399999618530273,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5160000324249268,
+ 2.507999897003174,
+ 2.503999948501587,
+ 2.503999948501587,
+ 2.5,
+ 2.492000102996826,
+ 2.492000102996826,
+ 2.4839999675750732,
+ 2.4800000190734863,
+ 2.4800000190734863,
+ 2.4760000705718994,
+ 2.4719998836517334,
+ 2.4679999351501465,
+ 2.4600000381469727,
+ 2.4600000381469727,
+ 2.4519999027252197,
+ 2.447999954223633,
+ 2.447999954223633,
+ 2.447999954223633,
+ 2.440000057220459,
+ 2.440000057220459,
+ 2.431999921798706,
+ 2.427999973297119,
+ 2.427999973297119,
+ 2.4240000247955322,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4119999408721924,
+ 2.4079999923706055,
+ 2.4040000438690186,
+ 2.4000000953674316,
+ 2.4000000953674316,
+ 2.4000000953674316,
+ 2.3959999084472656,
+ 2.3919999599456787,
+ 2.388000011444092,
+ 2.384000062942505,
+ 2.384000062942505,
+ 2.384000062942505,
+ 2.380000114440918,
+ 2.375999927520752,
+ 2.371999979019165,
+ 2.371999979019165,
+ 2.371999979019165,
+ 2.368000030517578,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.359999895095825,
+ 2.371999979019165,
+ 2.388000011444092,
+ 2.388000011444092,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.002000093460083,
+ 2.002000093460083,
+ 2.002000093460083,
+ 2.002000093460083,
+ 2.002000093460083,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.9980000257492065,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.996000051498413,
+ 1.996000051498413,
+ 2.0,
+ 2.009999990463257,
+ 2.009999990463257,
+ 2.0320000648498535,
+ 2.0480000972747803,
+ 2.0320000648498535,
+ 2.0179998874664307,
+ 2.007999897003174,
+ 2.007999897003174,
+ 1.996000051498413,
+ 1.9919999837875366,
+ 1.9900000095367432,
+ 1.9880000352859497,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 2.0,
+ 2.002000093460083,
+ 2.0,
+ 2.002000093460083,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9359999895095825,
+ 2.0360000133514404,
+ 2.0859999656677246,
+ 2.0439999103546143,
+ 1.9819999933242798,
+ 1.9819999933242798,
+ 1.9279999732971191,
+ 1.9559999704360962,
+ 1.9559999704360962,
+ 1.9600000381469727,
+ 1.9579999446868896,
+ 1.9559999704360962,
+ 1.9559999704360962,
+ 1.9579999446868896,
+ 1.9620000123977661,
+ 1.9639999866485596,
+ 1.968000054359436,
+ 1.9700000286102295,
+ 1.9700000286102295,
+ 1.972000002861023,
+ 1.9739999771118164,
+ 1.9739999771118164,
+ 1.9739999771118164,
+ 1.9780000448226929,
+ 1.9780000448226929,
+ 1.9839999675750732,
+ 1.9859999418258667,
+ 1.9859999418258667,
+ 1.9900000095367432,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9980000257492065,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.5999999046325684,
+ 1.6720000505447388,
+ 2.0480000972747803,
+ 2.115999937057495,
+ 2.115999937057495,
+ 2.191999912261963,
+ 2.196000099182129,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5399999618530273,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5480000972747803,
+ 2.555999994277954,
+ 2.559999942779541,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.3040000200271606,
+ 1.3040000200271606,
+ 1.2979999780654907,
+ 1.2920000553131104,
+ 1.2920000553131104,
+ 1.2860000133514404,
+ 1.2899999618530273,
+ 1.2999999523162842,
+ 1.3020000457763672,
+ 1.3020000457763672,
+ 1.3020000457763672,
+ 1.2999999523162842,
+ 1.2960000038146973,
+ 1.3020000457763672,
+ 1.312000036239624,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.3179999589920044,
+ 1.3279999494552612,
+ 1.3359999656677246,
+ 1.3420000076293945,
+ 1.2200000286102295,
+ 1.2200000286102295,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.0880000591278076,
+ 1.0880000591278076,
+ 1.0880000591278076,
+ 1.0920000076293945,
+ 1.0959999561309814,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0959999561309814,
+ 1.0959999561309814,
+ 1.0959999561309814,
+ 1.0959999561309814,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.0920000076293945,
+ 1.0920000076293945,
+ 1.0920000076293945,
+ 1.0920000076293945,
+ 1.093999981880188,
+ 1.1019999980926514,
+ 1.1139999628067017,
+ 1.1239999532699585,
+ 1.1239999532699585,
+ 1.1299999952316284,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 5.888000011444092,
+ 5.888000011444092,
+ 5.888000011444092,
+ 5.888000011444092,
+ 5.863999843597412,
+ 5.863999843597412,
+ 5.808000087738037,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.74399995803833,
+ 5.74399995803833,
+ 5.656000137329102,
+ 5.656000137329102,
+ 5.656000137329102,
+ 5.584000110626221,
+ 5.584000110626221,
+ 5.584000110626221,
+ 5.584000110626221,
+ 5.552000045776367,
+ 5.559999942779541,
+ 5.599999904632568,
+ 5.599999904632568,
+ 6.71999979019165,
+ 6.783999919891357,
+ 6.831999778747559,
+ 6.815999984741211,
+ 6.696000099182129,
+ 6.552000045776367,
+ 6.552000045776367,
+ 6.423999786376953,
+ 6.343999862670898,
+ 6.328000068664551,
+ 9.232000350952148,
+ 9.192000389099121,
+ 9.192000389099121,
+ 9.175999641418457,
+ 9.144000053405762,
+ 9.039999961853027,
+ 8.960000038146973,
+ 7.943999767303467,
+ 7.943999767303467,
+ 7.863999843597412,
+ 7.8480000495910645,
+ 7.791999816894531,
+ 7.751999855041504,
+ 7.736000061035156,
+ 7.736000061035156,
+ 7.71999979019165,
+ 7.728000164031982,
+ 7.728000164031982,
+ 7.711999893188477,
+ 7.696000099182129,
+ 7.696000099182129,
+ 7.688000202178955,
+ 7.696000099182129,
+ 7.696000099182129,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.736000061035156,
+ 7.800000190734863,
+ 8.616000175476074,
+ 8.600000381469727,
+ 8.600000381469727,
+ 8.576000213623047,
+ 8.543999671936035,
+ 8.520000457763672,
+ 8.51200008392334,
+ 8.496000289916992,
+ 8.496000289916992,
+ 8.472000122070312,
+ 8.456000328063965,
+ 8.4399995803833,
+ 8.4399995803833,
+ 8.423999786376953,
+ 8.423999786376953,
+ 8.383999824523926,
+ 8.37600040435791,
+ 8.37600040435791,
+ 8.368000030517578,
+ 8.35200023651123,
+ 8.35200023651123,
+ 8.35200023651123,
+ 8.312000274658203,
+ 8.248000144958496,
+ 8.223999977111816,
+ 8.288000106811523,
+ 8.288000106811523,
+ 8.4399995803833,
+ 8.607999801635742,
+ 8.776000022888184,
+ 8.895999908447266,
+ 9.064000129699707,
+ 9.064000129699707,
+ 9.168000221252441,
+ 11.84000015258789,
+ 12.175999641418457,
+ 12.312000274658203,
+ 12.656000137329102,
+ 12.656000137329102,
+ 12.767999649047852,
+ 13.208000183105469,
+ 13.35200023651123,
+ 13.656000137329102,
+ 13.64799976348877,
+ 13.64799976348877,
+ 13.527999877929688,
+ 14.015999794006348,
+ 13.960000038146973,
+ 14.119999885559082,
+ 14.255999565124512,
+ 14.255999565124512,
+ 13.784000396728516,
+ 15.407999992370605,
+ 15.496000289916992,
+ 15.53600025177002,
+ 15.423999786376953,
+ 15.423999786376953,
+ 15.37600040435791,
+ 15.416000366210938,
+ 15.423999786376953,
+ 15.383999824523926,
+ 15.343999862670898,
+ 15.343999862670898,
+ 15.368000030517578,
+ 15.32800006866455,
+ 15.255999565124512,
+ 15.272000312805176,
+ 15.272000312805176,
+ 15.272000312805176,
+ 15.215999603271484,
+ 15.272000312805176,
+ 15.295999526977539,
+ 15.208000183105469,
+ 15.215999603271484,
+ 15.215999603271484,
+ 15.208000183105469,
+ 15.208000183105469,
+ 15.192000389099121,
+ 15.168000221252441,
+ 15.168000221252441,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.639999389648438,
+ 16.6560001373291,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.639999389648438,
+ 16.672000885009766,
+ 16.687999725341797,
+ 16.54400062561035,
+ 16.576000213623047,
+ 16.6560001373291,
+ 16.6560001373291,
+ 16.27199935913086,
+ 16.583999633789062,
+ 16.079999923706055,
+ 16.399999618530273,
+ 16.736000061035156,
+ 16.736000061035156,
+ 16.143999099731445,
+ 250,
+ 20.304000854492188,
+ 20.304000854492188,
+ 20.304000854492188,
+ 20.304000854492188,
+ 250,
+ 250,
+ 250,
+ 20.20800018310547,
+ 20.20800018310547,
+ 20.20800018310547,
+ 20.20800018310547,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250};
+
+// std::vector<float> target_scan_1 = {2.03169,1.9677,2.02369,2.01969,1.9517,1.93171,1.93171,1.89171,1.92771,1.92771,1.89171,1.85172,1.83172,1.85972,1.85172,1.79173,1.55576,1.54376,1.53577,1.49577,1.49577,1.49577,1.45178,1.47977,1.47977,1.45578,1.59576,250,250,250,1.59576,1.59576,1.59576,1.59576,1.54776,1.55176,1.53577,1.53577,1.51977,1.50377,1.49577,1.48377,1.47977,1.44378,1.47178,1.47578,1.44378,1.42378,1.41978,1.42778,1.42378,1.41179,1.3198,1.26781,1.22781,1.24781,1.21981,1.20782,1.23581,1.19582,1.22781,1.19982,1.17982,1.20382,1.17582,1.17982,1.16782,1.16382,1.15582,1.15182,1.15982,1.14783,1.13183,1.14783,1.12383,1.15182,1.14383,1.13583,1.24781,1.24781,1.19582,1.2958,1.3118,1.3038,1.2958,1.2998,1.2998,1.3038,1.2978,1.2858,1.2818,1.27581,1.2838,1.2878,1.27981,1.3078,1.2898,1.27781,1.2878,1.27981,1.3018,1.2998,1.2938,1.2958,250,250,1.19582,1.21182,1.20382,1.18382,1.23581,1.19582,250,250,250,1.9777,1.9837,1.9777,1.9857,1.9917,2.09168,250,4.49132,4.48732,4.48332,4.48532,4.47732,4.47132,4.48532,4.47332,4.47532,4.46332,4.47332,4.47132,4.46332,4.49931,4.96124,5.04323,6.571,6.567,6.579,6.579,6.64299,7.55685,7.52685,7.56485,7.93079,250,250,8.70467,8.69668,8.69268,8.72067,8.72067,8.72067,8.73267,8.73667,8.76467,8.76267,8.76467,5.01924,5.01524,5.02723,5.02723,5.03123,5.05523,5.04323,5.07123,5.07123,5.07123,5.09722,5.01924,4.92325,4.68529,4.64729,4.66929,4.66529,4.68929,4.68729,4.70128,4.71728,4.70928,4.73928,4.74928,4.75128,4.76927,4.76327,4.79527,4.79527,4.80927,4.82926,4.83526,4.85326,4.86926,4.86726,4.88926,4.90725,4.92325,4.94325,4.95125,4.97124,5.00524,3.13952,3.11753,3.06953,3.03754,2.99154,2.96555,2.91156,2.87756,2.83757,2.22366,2.18967,2.16167,2.15367,2.15167,2.14167,2.17367,2.16767,2.16367,2.18567,2.32765,2.37964,2.44363,2.43163,2.41763,2.38564,2.37364,2.34164,2.30165,2.27565,2.26965,2.30565,2.31765,2.33764,2.34364,2.36164,2.37364,2.38364,2.40563,2.41163,2.41963,2.44163,2.45363,2.47362,2.48362,2.49162,2.51162,2.52961,2.55961,2.56561,2.57361,2.5996,2.6136,2.6396,2.6576,2.66959,2.69559,250,2.71959,7.15891,7.17491,7.2469,7.27889,7.37888,9.08662,9.06062,8.99063,8.90664,8.84865,8.76266,8.68668,8.67868,8.66668,8.66268,8.73067,8.81066,8.91064,9.01063,9.11461,9.2106,9.28259,9.43456,9.49056,9.62653,9.73252,9.8625,9.96848,10.0825,10.2244,10.3484,10.4724,10.6244,10.7524,10.9003,11.0623,11.2203,11.4623,11.6362,11.8122,11.9922,3.79342,3.78142,3.77343,3.76743,3.75143,3.73943,3.73743,3.71743,3.71143,3.70544,3.69144,3.68544,3.67344,3.67544,3.66144,3.63945,3.64145,3.62945,3.63345,3.61745,3.61345,3.61145,3.60345,3.60345,3.58545,3.58545,3.57746,3.55746,3.56946,3.56946,3.54746,3.55546,3.53346,3.54146,3.53346,3.52746,3.52146,3.50947,3.51547,3.50547,3.48947,3.50347,3.49947,3.50147,3.48547,3.47747,3.49147,3.47947,1.90571,1.90571,2.38764,2.37764,2.36764,3.47747,3.47747,3.45947,3.46147,3.46547,3.47747,3.45747,3.46747,3.48147,3.46747,3.47947,3.48147,3.46147,3.47947,3.47347,3.48147,3.47347,3.47147,2.38764,2.36364,1.90171,1.88771,1.88771,3.48547,3.59145,5.19121,4.08338,3.88341,3.69544,3.58345,4.18136,4.17536,3.2915,2.89156,2.83157,2.77958,2.79157,2.82357,2.81957,2.82557,2.84357,250,250,1.9677,2.0037,250,250,2.87956,2.86156,2.85956,2.87756,2.87356,2.89956,2.89956,2.89356,2.91356,2.90756,2.92955,4.41933,4.42333,4.44332,4.44332,4.47132,4.47532,1.43178,1.42778,1.37579,1.35979,1.3418,1.3298,1.3098,1.2858,1.2938,1.2818,1.25581,1.34779,1.44978,1.46778,1.47178,1.48177,1.49377,1.49177,1.51777,1.51377,1.49977,1.51977,1.52377,1.54376,1.54976,1.54376,1.55976,1.55976,1.57176,1.57576,1.57576,1.59176,1.59376,1.60975,1.61575,1.61975,1.62775,1.63775,1.65375,1.65175,1.66575,1.68174,1.68174,1.69774,1.70574,1.70574,1.73174,1.72774,1.75173,1.74973,1.76573,1.77573,1.78573,1.80573,1.81372,1.81372,1.83372,1.84572,1.86772,1.87571,1.88771,1.91171,1.92571,1.93371,1.9597,1.9637,1.9917,1.9857,2.01969,2.03169,2.03569,2.05969,2.07168,2.09368,2.11768,2.13168,2.15967,2.16967,2.19967,2.21566,2.23166,2.25966,2.28365,2.27765,2.25766,2.23566,2.23166,2.21166,2.21966,2.20766,2.19767,2.21566,2.24766,4.41933,4.39133,4.38333,4.37533,4.35134,4.32334,4.30334,4.29935,4.28335,4.29135,4.25935,4.24335,4.21536,4.21136,4.19536,4.19536,4.17137,4.15137,4.15137,4.11537,4.12737,4.09538,3.79542,3.79142,3.87541,3.9314,4.03139,4.45332,4.44332,3.04754,3.03554,3.05353,3.08753,2.98755,2.98755,3.21951,3.2715,3.07753,3.03954,3.03554,2.85357,2.86556,2.91156,2.90956,3.01554,3.03754,3.01754,3.00754,2.89556,2.90156,2.88556,3.02154,3.37349,3.33949,2.95155,2.92156,2.91956,2.91156,2.91156,2.91956,2.95155,2.99954,3.2595,3.65944,4.18736,4.18336,4.19536,4.19536,2.79557,2.93355,2.97955,3.01954,3.03554,3.05554,3.08753,3.08753,3.12552,3.12552,3.12952,3.15352,3.13952,3.17352,3.69944,3.82142,3.89141,250,2.19167,2.17967,2.13567,2.08368,2.04369,2.04369,1.9837,1.9677,1.92371,1.69174,1.64975,1.63375,1.62775,1.62775,1.62375,1.63375,1.63775,1.64175,1.65775,1.67774,1.68774,1.90371,1.9477,1.9837,1.9797,1.9877,1.9757,1.93171,1.9557,1.9437,1.9637,1.89171,1.78173,1.76173,1.75173,1.76573,1.74573,1.73974,1.76373,1.75973,1.77973,1.77373,1.75373,1.79573,4.64729,4.6193,4.63129,4.63929,4.65529,4.67529,4.70328,4.71928,4.70328,4.33934,250,4.11537,4.09938,4.07538,4.01739,3.98539,3.98339,3.9614,3.97539,4.01539,1.70574,1.67774,1.65975,1.66575,1.64775,1.63975,1.62175,1.59976,1.65175,1.68974,1.69974,1.69774,1.63975,1.64575,1.63175,1.63575,1.64975,1.62775,1.63975,1.64575,1.64375,1.65375,1.64975,1.65175,1.65575,1.65775,1.67574,1.67175,1.68974,1.68974,1.69974,1.70574,1.71774,1.72974,1.73574,1.76173,1.76973,1.78373,1.81972,1.89171,250,250,250,250,5.05923,5.16521,5.15122,5.10922,5.09722,5.05923,5.03123,5.00924,4.97724,4.95525,4.92925,4.90325,4.89126,4.85326,4.85126,4.81727,4.52731,4.51531,4.49132,4.48332,4.45932,4.43532,4.42333,4.39533,4.39333,4.37533,4.35734,4.33134,4.31334,4.30534,4.28735,4.32134,4.38733,4.43732,4.47332,4.44732,4.43332,4.42533,4.26935,3.82742,3.81542,3.79942,3.80142,3.77942,3.77543,3.76943,3.74143,3.73943,3.72143,3.71543,3.69544,3.67544,3.65944,3.64345,3.63945,3.65744,3.63945,3.63345,3.61545,3.62345,3.60345,3.60545,3.58145,3.57146,2.75158,2.77758,2.77158,2.74758,2.71159,2.70559,2.68359,2.69559,2.71359,2.73358,2.75958,2.75758,2.81357,2.82957,3.00354,3.18152,3.2755,3.2735,3.2635,3.2615,3.2595,3.23951,3.24951,3.24351,3.23151,3.23951,3.22351,2.99754,2.98355,2.98555,2.99154,2.97555,2.99954,2.99154,2.98155,2.99354,2.97155,2.97755,2.97355,2.97755,2.98355,2.98155,2.98954,2.98755,2.97555,2.98555,2.96555,2.99154,4.01739,4.02139,4.02339,3.72943,3.57946,3.44348,3.2695,3.2555,3.2695,3.2755,3.2695,3.2735,3.2915,3.2815,3.3015,3.3075,3.3095,3.31949,3.3155,3.33749,3.32749,3.32749,4.04538,4.04738,4.12537,3.9674,3.82942,3.69744,3.51147,3.46547,3.48147,3.41148,3.46947,4.21136,4.21936,4.21936,4.23136,4.24935,4.25135,4.26135,3.56346,3.50347,3.45147,3.40548,3.40348,3.38348,3.38748,3.38149,3.37749,3.39948,3.39948,3.43748,3.47347,3.49947,3.59145,3.85141,3.84541,4.47132,4.52331,4.54331,3.9334,4.5813,4.17736,4.5953,4.62929,2.25366,2.19966,2.18567,2.17967,2.19967,2.23166,250,3.41948,2.06369,2.09168,2.07168,2.06369,2.07168,1.9917,1.9917};
+// std::vector<float> reference_scan_1 = {3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144};
+
+std::vector<float> reference_scan_1 ={3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144};
+std::vector<float> target_scan_1 ={1.93771,1.93171,1.9437,1.9597,1.9537,1.9737,1.9777,1.9757,1.9937,1.9617,1.89771,1.87371,1.85772,1.86372,1.85572,1.86372,1.86172,1.86372,1.88371,1.89171,1.91971,4.73128,4.74128,4.76927,4.76927,4.78927,4.80327,4.81927,4.83526,4.85126,4.31534,4.23136,4.19536,4.16337,4.14137,4.13537,1.82572,1.79173,1.78373,1.75373,1.76173,1.74773,1.73974,1.72774,1.71574,1.71574,1.70974,1.70374,1.70974,1.70374,1.71174,1.70774,1.70374,1.71174,1.69974,1.71574,1.71374,1.70774,1.71774,1.71374,1.72374,1.72574,1.74773,1.75773,1.75373,1.78173,1.77773,1.77173,1.79573,1.79773,1.82972,1.83772,1.85572,1.87571,1.92571,1.9737,250,250,250,250,250,250,250,5.14722,5.2752,5.2692,5.2252,5.18921,5.17521,5.13322,5.12122,5.08523,5.04923,5.03123,5.00524,4.98324,4.95525,4.92525,4.91325,4.86926,4.86126,4.83926,4.81727,4.80527,4.77527,4.75528,4.67729,4.53931,4.54331,4.65929,4.65529,4.63529,4.6153,4.6033,4.5773,4.5633,4.55131,4.52131,4.52131,4.48732,3.9194,3.89541,3.88741,3.87941,3.85941,3.85941,3.83542,3.81542,3.81142,3.78742,3.78142,3.76343,3.74943,3.73343,3.70744,3.72343,3.72143,3.70344,3.70544,3.68344,3.68344,3.66744,3.65144,3.65544,2.85157,2.83557,2.81357,2.78358,2.77158,2.76358,2.76358,2.76358,2.79357,2.82157,2.79557,2.85557,2.89356,3.14552,3.2775,3.32349,3.32949,3.3075,3.2995,3.3055,3.2975,3.3055,3.2855,3.2795,3.2855,3.2675,3.04954,3.02354,3.01954,3.03354,3.01754,3.02554,3.01354,3.00354,3.02154,3.00954,3.01354,3.00354,3.00354,3.01954,2.99954,3.01154,3.00754,3.01354,3.00754,2.99154,3.01354,4.03339,4.02939,4.03539,3.82342,3.62345,3.74343,3.2695,3.2815,3.2735,3.2875,3.2895,3.2875,3.2935,3.2955,3.3115,3.3135,3.3135,3.32949,3.32149,3.34349,3.33749,4.12537,4.10737,4.07138,4.05538,4.03738,3.85941,3.44947,3.42548,3.53146,3.44348,3.43548,3.48547,4.19136,4.20336,4.21336,3.59145,3.59145,3.59145,3.59145,3.59145,3.47147,3.45147,3.42348,3.45147,3.42348,3.34749,3.34749,3.44348,250,250,250,2.95955,2.93355,2.93555,2.92955,2.91556,2.88756,2.88756,2.92355,4.50331,2.19167,2.16767,2.13368,2.04369,1.91971,1.85972,1.83172,1.85972,1.79173,1.76773,1.73174,1.79173,1.84772,1.79173,1.79173,1.86372,1.79173,1.86372,1.89171,1.84772,1.89171,1.87571,1.89171,1.84372,1.85572,1.66775,1.51577,1.45978,1.47977,1.47178,1.42778,1.43178,1.41179,1.40779,1.37979,1.39579,1.39579,1.37579,1.35579,1.35979,1.3078,1.3398,1.3278,1.39579,1.39579,1.39579,250,1.39579,1.43578,1.44778,1.39579,1.45978,1.41578,1.42778,1.43978,1.41179,1.39579,1.36779,1.37979,1.37179,1.3158,1.3318,1.21182,1.17582,1.15582,1.14783,1.11183,1.12383,1.11183,1.12183,1.09783,1.10783,1.10183,1.08783,1.08783,1.08384,1.08384,1.07384,1.06784,1.06984,1.05784,1.06184,1.05584,1.04784,1.05784,1.03784,1.05184,1.04184,1.03584,1.05584,1.01984,1.02384,1.03584,1.01585,1.00785,1.00785,1.09583,250,250,1.19582,250,1.19582,1.17982,1.19582,1.17782,1.17582,1.18782,1.17582,1.17182,1.17382,1.16582,1.17982,1.16782,1.13583,1.12783,1.09583,1.12783,1.08384,1.09583,1.08384,1.07584,1.06384,1.05184,1.03384,1.05384,1.05184,1.04784,1.04184,1.05584,1.05584,1.05984,1.06784,1.12383,1.84772,1.83972,1.85172,1.85972,1.84772,1.85572,1.85972,1.89171,250,4.36734,4.36334,4.37333,4.35534,4.36534,4.36933,4.35934,4.36733,4.35134,4.34734,4.36134,4.33734,4.34934,4.33734,4.35334,4.34134,4.77927,4.85926,4.90325,6.45502,6.45902,6.44702,6.45102,8.5347,250,7.61484,7.59484,8.5287,8.5347,8.5427,8.5447,8.5527,8.5627,8.5627,8.5727,8.57869,8.58269,8.60069,8.59469,8.61669,8.62469,4.86726,4.88326,4.88926,4.89725,4.89925,4.90725,4.92325,4.91525,4.94125,4.93525,4.94725,4.90125,4.79127,4.54931,4.52131,4.51731,4.54331,4.53731,4.55331,4.5633,4.5693,4.6033,4.6073,4.6193,4.6233,4.64729,4.65329,4.66729,4.68129,4.69528,4.72328,4.72128,4.73528,4.75728,4.75728,4.79327,4.79927,4.80727,4.82527,4.83126,4.85726,4.87526,4.89126,3.02154,2.99954,2.98155,2.91556,2.87756,2.85357,2.80757,2.78958,2.75158,2.70359,2.67559,2.6176,2.06569,2.04769,2.03569,2.04369,2.03369,2.03969,2.04969,2.06169,2.10168,2.22166,2.30765,2.32165,2.31165,2.29765,2.25766,2.24566,2.21766,2.17367,2.17767,2.16567,2.19967,2.21966,2.22566,2.24366,2.23966,2.27165,2.27965,2.29365,2.31965,2.32765,2.35564,2.35764,2.36564,2.39164,2.40163,2.41763,2.43363,2.44563,2.47162,2.47962,2.51762,2.52162,2.54161,2.56561,2.57961,2.6056,2.59161,2.6076,7.2149,7.2469,7.30689,9.00663,8.91864,8.85865,8.77066,8.71467,8.63868,8.57469,8.69468,8.5327,8.62469,8.70867,8.80066,8.90464,8.99663,9.10061,9.2046,9.29259,9.43456,9.51655,9.66053,9.75451,9.88649,10.0145,10.1425,10.2784,10.4224,10.5424,10.7024,10.8423,11.0103,11.1783,11.4323,11.6122,11.7702,11.9642,12.0702,12.0462,3.74743,3.70744,3.70944,3.70744,3.69144,3.68144,3.66544,3.66944,3.64944,3.64544,3.64744,3.62345,3.62745,3.61145,3.60545,3.60545,3.58945,3.59345,3.57346,3.57346,3.56346,3.54146,3.55546,3.53746,3.53746,3.54146,3.53346,3.52746,3.51746,3.50947,3.51147,3.49547,3.50347,3.49547,3.48747,3.48947,3.47547,3.47947,3.47347,3.46147,3.46947,3.46147,3.46947,3.46347,3.45147,3.46347,3.44548,3.45347,3.45347,3.44748,3.45147,250,1.86972,1.87172,1.86772,3.43548,3.43348,3.46547,3.44548,3.43548,3.44947,3.44348,3.46747,3.44947,3.43748,3.44747,3.44947,3.45547,3.45547,3.45547,3.46347,3.45747,3.47147,2.36764,2.37164,2.35964,3.47547,1.89771,1.88971,1.87971,1.91171,3.68144,3.59345,4.16936,4.18736,4.20936,3.2695,250,2.79158,2.77758,2.78958,2.81557,2.82357,2.81957,2.82357,2.85357,2.84357,250,250,250,2.09168,2.04369,250,250,2.89356,2.89956,2.89956,2.91556,2.91956,2.92156,2.93755,2.93555,4.46932,4.47532,4.48332,4.50331,4.51331,4.53331,4.54331,4.55131,4.5813,4.54931,4.52131,1.49577,1.45978,1.43978,1.40379,1.38379,1.37979,1.35579,1.3318,1.3218,1.3318,1.3138,1.3018,1.3118,1.49977,1.52577,1.52377,1.52777,1.53977,1.54376,1.56376,1.57176,1.56376,1.58776,1.58376,1.60376,1.60376,1.60975,1.62575,1.62975,1.64575,1.64575,1.65375,1.66775,1.66575,1.68774,1.70174,1.70374,1.71374,1.72574,1.74373,1.74573,1.75573,1.78573,1.78973,1.80173,1.81772,1.81772,1.84172,1.84972,1.86772,1.87371,1.88971,1.90971,1.91371,1.9457,1.9617,1.9737,1.9897,1.9997,2.02369,2.03169,2.04569,2.07968,2.08768,2.12168,2.13368,2.13967,2.17967,2.18767,2.21966,2.23966,2.24366,2.27965,2.29965,2.34364,2.36164,2.37564,2.34364,2.32765,2.33564,2.31165,2.29165,2.29165,2.29565,2.29165,2.29165,4.49132,4.46332,4.43532,4.43133,4.41533,4.39933,4.38333,4.36334,4.33534,4.34734,4.30334,4.29535,4.27935,4.28335,4.29135,4.24735,4.26735,4.22736,4.19936,4.08738,3.87141,3.9594,4.03139,4.23136,4.55131,4.54531,4.52931,4.51731,4.52131,4.50531,4.50131,4.48932,4.47532,4.46932,3.2755,3.21951,3.21151,3.20751,3.24351,3.24151,3.11153,3.11553,3.12552,3.13152,3.09153,3.01954,2.94755,2.96355,3.03554,3.04354,3.09753,3.09953,3.09953,3.07753,3.08953,3.07553,3.01954,3.09353,3.14352,3.09153,3.46547,3.13152,3.09953,3.09153,3.09353,3.13152,3.17352,3.76143,4.31734,4.31934,4.30934,4.31734,4.30934,4.32534,4.31334,4.30934,4.30934,3.9354,3.9174,3.61945,3.9314,3.9394,3.9374,3.9494,3.9454,3.9394,3.9654,3.9454,3.99139,2.09168,2.17167,2.19167,2.08768,2.08768,2.03169,1.88771,1.87971,1.85372,1.83372,1.78973,1.74973,1.74773,1.73774,1.75173,1.74573,1.74773,1.76173,1.75773,1.78173,1.79173};
+
+std::vector<float> reference_scan_2 ={4.86126,4.82527,4.84126,4.83326,4.85326,250,250,4.43932,4.43332,5.68313,5.04523,4.74728,4.80527,5.59715,5.01724,5.44117,5.54915,5.54516,5.51716,4.96524,5.16921,5.47317,4.28935,4.27335,5.46517,4.5893,5.43117,5.42317,5.41318,5.40318,4.68529,4.67329,5.36718,5.36518,5.36118,5.30319,5.33719,5.31519,5.38718,5.39518,5.45717,5.43317,5.44117,5.41718,5.39318,5.42117,5.39318,5.36118,5.34919,5.33719,5.35718,5.32919,5.33719,5.30519,5.32919,5.30519,5.32119,5.29319,5.30519,5.32119,5.30119,5.32119,5.2772,5.30519,5.30519,5.29319,5.2812,5.2732,5.29319,13.6039,13.5939,13.6079,13.6119,13.6159,13.6299,13.6099,13.6299,13.6179,13.6259,13.6419,11.5742,11.5642,6.51301,6.549,6.553,6.52101,6.45702,250,250,250,250,250,250,250,250,5.43317,5.38718,5.37118,5.30519,5.32519,5.31519,3.2655,3.11153,2.98155,2.78558,2.81157,2.73358,2.71359,2.6276,2.55361,2.49762,2.44163,2.37364,2.33564,2.27365,2.22766,2.17967,2.12768,2.08768,2.05369,2.01969,1.9757,1.93371,1.87971,1.86772,1.84372,1.81572,1.77573,1.76373,1.72574,1.70174,1.67575,1.65975,1.62975,1.61775,1.59176,1.57176,1.53977,1.52777,1.49577,1.48777,1.44778,1.43578,1.41578,1.39779,1.38779,1.36579,1.37579,1.35979,1.35579,1.35579,1.38179,1.39379,1.38779,1.39379,1.40779,1.40579,1.42178,1.42178,1.42178,1.43178,1.43578,1.44378,1.45178,1.45578,1.47178,1.48377,1.48977,1.48977,1.50377,1.52777,1.52577,1.55176,1.54976,1.55176,1.58176,1.57976,1.61175,1.59976,1.62775,1.62575,1.63775,1.63775,1.62975,1.64775,1.67575,1.67974,1.91571,1.82372,1.72174,1.68974,1.67774,1.69974,1.70774,1.74773,1.78373,1.79173,1.82372,1.84772,1.89371,1.91771,1.9477,1.9897,2.01769,2.05969,2.09568,2.11968,2.17167,2.20966,2.25966,2.30365,2.33564,2.40563,2.77558,2.13767,2.17167,2.23366,2.27365,2.31765,2.37764,2.78158,2.80957,2.84157,2.86556,2.90956,2.93755,2.98954,3.01754,3.04754,3.10353,3.14352,3.18352,3.19351,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,3.07353,2.97755,2.96755,3.03754,3.11353,3.18951,3.2615,250,1.21781,1.21981,1.21382,1.22581,1.22381,1.22381,1.21182,1.20382,1.21581,1.20382,1.20582,1.20582,1.19982,1.20582,1.20382,1.21382,1.20582,1.18982,1.18982,1.18982,1.20182,1.19782,1.18782,1.19582,1.18182,1.19982,1.19582,1.18182,1.19582,1.18182,1.20382,1.18982,1.18182,1.19582,1.19382,1.19382,1.18582,1.18382,1.19182,1.19182,1.19382,1.18782,1.18382,1.18982,1.19382,1.19382,1.16782,1.10983,0.751886,0.761884,0.761884,0.735888,0.731889,0.735888,0.731889,0.743887,0.743887,0.735888,0.751886,0.739887,0.747886,0.749886,0.743887,0.751886,0.743887,0.759884,0.753885,0.743887,0.751886,0.751886,0.749886,0.759884,0.757885,0.763884,0.763884,0.773882,0.759884,0.773882,0.763884,0.763884,0.777882,0.763884,0.777882,0.779881,0.775882,0.78788,0.775882,0.791879,0.78988,0.791879,0.791879,0.791879,0.797879,0.791879,0.795879,0.807877,0.803878,0.811876,0.811876,0.811876,0.823875,0.815876,0.831873,0.811876,0.823875,0.835873,0.831873,0.835873,0.839872,0.839872,0.85587,0.85187,0.863868,0.85587,0.85587,0.865868,0.85587,0.871867,0.867868,0.871867,0.883865,0.879866,0.897863,0.893864,0.897863,0.909861,0.901863,0.91986,0.91786,0.923859,0.931858,0.931858,0.939857,0.943856,0.953855,0.959854,0.959854,0.977851,0.971852,0.999848,0.991849,0.995848,1.00785,1.00585,1.02784,1.01984,1.02984,1.03984,1.04584,1.06384,1.06584,1.06784,1.08384,1.07984,1.10383,1.10783,1.10783,1.12383,1.12783,1.14383,1.15582,1.15582,1.17982,1.17982,1.20382,1.20982,1.21981,1.23181,1.23381,1.24981,1.26381,1.27581,1.3098,1.3078,1.3358,1.3358,1.35179,1.36579,1.37379,1.40179,1.41378,1.41778,1.44178,1.45378,1.48977,1.49177,1.50977,1.53377,1.54976,1.58176,1.59776,1.60975,1.63775,1.65375,1.68974,1.71374,1.73374,1.77573,1.79173,1.82772,1.83972,1.88771,1.90771,1.9577,1.9877,2.01569,2.05569,2.08768,2.12368,2.16567,2.25766,2.39364,2.32365,2.33764,2.39364,2.39364,250,2.46762,2.55361,250,250,250,3.40748,3.39748,3.39748,3.39548,3.40348,3.47347,3.58145,4.28535,4.45532,4.5973,4.72128,250,250,250,250,7.8888,250,250,9.11661,9.8685,10.0405,9.98848,250,12.3841,13.8739,13.8639,13.8519,17.6173,19.1251,25.3661,25.7701,25.7561,25.8161,13.8179,13.8239,13.8299,13.8359,250,9.93649,9.94449,7.56485,9.95848,9.96648,9.94849,9.98448,8.29874,8.30474,8.31273,250,250,250,250,250,250,250,250,10.1125,10.0445,250,8.18875,8.22875,250,7.34888,7.32488,7.34088,7.34888,7.37288,7.37688,7.38488,7.40887,10.2564,10.2604,250,250,10.3024,10.3844,250,10.3544,7.63684,250,7.18891,250,5.2652,5.2212,5.2332,5.19321,5.19721,5.08123,5.20521,5.19321,7.58884,4.13337,4.10138,4.08738,4.08738,4.11137,4.12137,3.9734,3.9374,3.9094,3.9294,5.12122,4.69329,4.66929,4.5933,4.54131,4.46932,4.84526,4.85726,4.86926,4.89325,4.89325,4.92125,4.98124,5.00524,4.15937,4.12537,4.04538,4.01339,3.9694,3.9214,3.88541,3.83342,3.79542,3.75143,3.70944,3.67144,3.63545,3.61745,3.57346,3.55146,3.52146,3.48347,3.45547,3.41948,3.40948,3.37749,3.32549,3.3175,3.2815,3.2695,3.24151,3.24151,3.22151,3.19351,3.21751,3.22751,3.24151,1.3118,1.2978,1.3038,1.2918,1.2958,1.2978,1.27381,1.26781,1.26181,1.24381,1.24781,1.23781,1.24181,1.21382,1.21981,1.20582,1.19582,1.19582,1.18582,1.17982,1.16582,1.15582,1.15982,1.14783,1.15382,1.14583,1.12783,1.13183,1.11983,1.12583,1.11383,1.10183,1.10183,1.09183,1.10183,1.09183,1.07784,1.07984,1.06384,1.07784,1.06384,1.05384,1.06384,1.03784,1.05384,1.04384,1.03184,1.04984,1.02784,1.03784,1.03384,1.01585,1.01984,1.00185,1.01585,1.00985,0.999848,1.00785,0.993849,1.00185,0.995848,0.993849,0.989849,0.98585,0.98585,0.977851,0.98385,0.961854,0.971852,0.963853,0.939857,0.975851,0.965853,0.953855,1.00385,1.02984,1.07784,1.10583,1.11783,1.16582,1.20182,1.23781,1.2978,1.36979,1.44978,1.50977,1.58176,1.60975,1.64375,1.70174,1.70574,1.77773,1.86572,1.73774,1.70374,1.67774,1.68774,1.68974,1.67574,1.68974,1.68974,1.70574,1.70374,1.75773,1.84172,1.82172,1.81772,1.79573,1.79573,1.80573,1.79773,1.80173,1.79373,1.81972,1.81972,1.82972,1.88171,7.93879,7.95679,7.95879,7.17291,6.68498,6.70498,6.74097,6.69698,6.73897,6.71498,6.70098,6.60899,6.64099,8.02078,8.02478,8.02078,8.04877,8.04278,7.71683,7.67083,7.40487,7.69083,7.69683,7.70083,7.72282,250,8.14876,8.15476,6.10507,6.04908,5.84111,5.73313,5.62714,5.52916,5.50916,5.53716,5.85911,5.82511,5.78112,5.73313,5.65314,5.65314,5.63714,8.45471,8.46071,8.48071,8.5087,8.5207,8.5567,5.54516,5.50916,5.48317,5.45517,5.46717,5.46317,5.46717,5.47117,5.47317,5.47717,5.50916,5.58115,5.64914,8.95664,8.99063,8.97663,250,250,2.89356,2.86556,2.85157,2.85756,2.79357,2.79357,2.79357,2.69359,2.74158,2.59361,2.59361,2.59361,2.59361,2.53361,2.49362,2.43363,2.47762,2.43363,2.46562,2.41763,2.32165,2.21366,2.18567,2.19767,2.17967,2.16967,2.22166,2.24566,2.39364,2.35764,2.39364,2.39364,2.49362,2.49362,2.59361,250,2.59361,2.59361,250,250,250,250,250,250,250,250,2.89356,2.79357,2.89356,250,250,2.91956,2.90356,2.92156,250,2.99354,3.07353,3.00554,3.01354,3.03754,3.41748,3.47747,3.43748,250,3.50947,3.42348,3.44148,3.66544,3.79342,250,3.56146,3.50147,3.59345,3.56346,5.42117,4.42133,4.38733,4.37133,4.38933,4.99324};
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 0cadea4ffe640770290f82ea4b5e17542468d404..c73292612597de2fb22c677ffd5bedf92c62daa4 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -1,4 +1,25 @@
-#include "utils_gtest.h"
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <core/utils/utils_gtest.h>
TEST(TestTest, DummyTestExample)
{
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
index daa6a21eb3d332adfeb0600aed096387fd7161d2..d84c18d1ac15c24aac47545cae3513561b823aca 100644
--- a/test/gtest_landmark_polyline.cpp
+++ b/test/gtest_landmark_polyline.cpp
@@ -1,5 +1,26 @@
-#include "utils_gtest.h"
-#include "laser/landmark/landmark_polyline_2D.h"
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <core/utils/utils_gtest.h>
+#include "laser/landmark/landmark_polyline_2d.h"
using namespace wolf;
using namespace Eigen;
@@ -8,44 +29,44 @@ class LandmarkPolylineTest : public testing::Test
{
public:
- LandmarkPolyline2DPtr lmk_ptr;
- std::vector<LandmarkPolyline2DPtr> lmks_class_list;
- std::vector<VectorXs> lmks_groundtruth;
- std::vector<MatrixXs> points_groundtruth;
+ LandmarkPolyline2dPtr lmk_ptr;
+ std::vector<LandmarkPolyline2dPtr> lmks_class_list;
+ std::vector<VectorXd> lmks_groundtruth;
+ std::vector<MatrixXd> points_groundtruth;
std::vector<PolylineRectangularClass> classification_groundtruth;
std::vector<PolylineRectangularClass> classification_candidates;
- virtual void SetUp()
+ void SetUp() override
{
classification_candidates.push_back(PolylineClassContainer());
classification_candidates.push_back(PolylineClassSmallContainer());
classification_candidates.push_back(PolylineClassPallet());
classification_candidates.push_back(PolylineRectangularClass(OTHER,45.6, 3.1));
- //std::vector<Scalar> object_L({12, 5.9, 1.2});
- //std::vector<Scalar> object_W({2.345, 2.345, 0.9});
+ //std::vector<double> object_L({12, 5.9, 1.2});
+ //std::vector<double> object_W({2.345, 2.345, 0.9});
//std::vector<PolylineClassType> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
- StateBlockPtr lmk_p_ptr = std::make_shared<StateBlock>(Vector2s::Zero());
- StateBlockPtr lmk_o_ptr = std::make_shared<StateBlock>(Vector1s::Zero());
+ StateBlockPtr lmk_p_ptr = std::make_shared<StateBlock>(Vector2d::Zero());
+ StateBlockPtr lmk_o_ptr = std::make_shared<StateBlock>(Vector1d::Zero());
- MatrixXs points = MatrixXs::Zero(2,5);
+ MatrixXd points = MatrixXd::Zero(2,5);
points << 1, 1,-1,-1, 1,
2,-2,-2, 2, 2;
- lmk_ptr = std::make_shared<LandmarkPolyline2D>(lmk_p_ptr, lmk_o_ptr, points, true, true);
+ lmk_ptr = std::make_shared<LandmarkPolyline2d>(lmk_p_ptr, lmk_o_ptr, points, true, true);
// classification test
- MatrixXs class_points = MatrixXs::Zero(2,4);
+ MatrixXd class_points = MatrixXd::Zero(2,4);
for (auto configuration = 1; configuration <= 2; configuration++)
for (auto candidate_class : classification_candidates)
//for (unsigned int classification = 0; classification < object_L.size(); classification++)
{
- const Scalar& L = candidate_class.L;
- const Scalar& W = candidate_class.W;
- //const Scalar& L = object_L[classification];
- //const Scalar& W = object_W[classification];
+ const double& L = candidate_class.L;
+ const double& W = candidate_class.W;
+ //const double& L = object_L[classification];
+ //const double& W = object_W[classification];
// create trivial points in corresponding configuration
if (configuration == 1)
@@ -57,10 +78,10 @@ class LandmarkPolylineTest : public testing::Test
// rotate and translate "randomly"
- Vector3s center_pose; center_pose << 4, -9, M_PI/5;
- Matrix2s R = Rotation2D<Scalar>(center_pose(2)).matrix();
+ Vector3d center_pose; center_pose << 4, -9, M_PI/5;
+ Matrix2d R = Rotation2D<double>(center_pose(2)).matrix();
class_points = R * class_points;
- class_points += center_pose.head<2>() * VectorXs::Ones(4).transpose();
+ class_points += center_pose.head<2>() * VectorXd::Ones(4).transpose();
// store object and points groundtruth
lmks_groundtruth.push_back(center_pose);
@@ -72,7 +93,7 @@ class LandmarkPolylineTest : public testing::Test
// create a landmark polyline using the points
StateBlockPtr p_ptr = std::make_shared<StateBlock>(center_pose.head<2>());
StateBlockPtr o_ptr = std::make_shared<StateBlock>(center_pose.tail<1>());
- lmks_class_list.push_back(std::make_shared<LandmarkPolyline2D>(p_ptr, o_ptr, class_points, true, true));
+ lmks_class_list.push_back(std::make_shared<LandmarkPolyline2d>(p_ptr, o_ptr, class_points, true, true));
}
}
};
@@ -102,7 +123,7 @@ TEST_F(LandmarkPolylineTest, Classify)
{
for (unsigned int i = 0; i < lmks_class_list.size(); i++)
{
- LandmarkPolyline2DPtr lmk_ptr = lmks_class_list[i];
+ LandmarkPolyline2dPtr lmk_ptr = lmks_class_list[i];
// test classify
ASSERT_TRUE(lmk_ptr->tryClassify(0.1, classification_candidates));
@@ -114,7 +135,7 @@ TEST_F(LandmarkPolylineTest, Classify)
ASSERT_EQ(lmk_ptr->getClassification().D, classification_groundtruth[i].D);
// test center pose
- ASSERT_MATRIX_APPROX(lmk_ptr->getState(), lmks_groundtruth[i], 1e-9);
+ ASSERT_MATRIX_APPROX(lmk_ptr->getState().vector("PO"), lmks_groundtruth[i], 1e-9);
// test closed
ASSERT_TRUE(lmk_ptr->isClosed());
@@ -126,8 +147,8 @@ TEST_F(LandmarkPolylineTest, Classify)
ASSERT_TRUE(st_pair.second->isFixed());
// test points are in the same place (compoping with object pose)
- Matrix2s R = Rotation2D<Scalar>(lmk_ptr->getO()->getState()(0)).matrix();
- VectorXs t = lmk_ptr->getP()->getState();
+ Matrix2d R = Rotation2D<double>(lmk_ptr->getO()->getState()(0)).matrix();
+ VectorXd t = lmk_ptr->getP()->getState();
for (unsigned int point_i = 0; point_i < 4; point_i++)
ASSERT_MATRIX_APPROX(R*lmk_ptr->getPointVector(point_i)+t, points_groundtruth[i].col(point_i), 1e-9);
}
diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp
deleted file mode 100644
index 2318ad97bc74063361a15ef4e2437bee1f56d687..0000000000000000000000000000000000000000
--- a/test/gtest_polyline_2D.cpp
+++ /dev/null
@@ -1,207 +0,0 @@
-#include <stdlib.h>
-#include "utils_gtest.h"
-#include "laser/processor/polyline_2D_utils.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(Polyline2DTest, Transformations)
-{
- ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector2s::Zero(),0), 1e-12);
- ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector3s::Zero()), 1e-12);
- ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-12);
-}
-
-TEST(Polyline2DTest, RigidTransformation8points)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,8);
- points_last.bottomRows(1) = MatrixXs::Ones(1,8);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // compute rigit transformation
- Vector3s v = computeRigidTrans(points_incoming, points_last);
-
- ASSERT_MATRIX_APPROX(v.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation2points2defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,2);
- points_last.bottomRows(1) = MatrixXs::Ones(1,2);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation3points2defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,3);
- points_last.bottomRows(1) = MatrixXs::Ones(1,3);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // change not defined point (first)
- points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1));
- points_incoming(2,0) = 1;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation3points1defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,3);
- points_last.bottomRows(1) = MatrixXs::Ones(1,3);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // change 2 not defined points
- points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(1) - points_incoming.col(0));
- points_incoming(2,0) = 1;
- points_incoming.col(2) = points_incoming.col(2) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(2) - points_incoming.col(1));
- points_incoming(2,2) = 1;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, false);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-TEST(Polyline2DTest, RigidTransformation2points1defined)
-{
- // Random points around random position
- MatrixXs points_last = MatrixXs::Random(3,2);
- points_last.bottomRows(1) = MatrixXs::Ones(1,2);
- MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
- points_last = T_random * points_last;
-
- // For different orientation changes between last and incomming
- for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
- {
- // movement from last to incoming (random translation)
- Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
- Vector2s t_last_incoming(Vector2s::Random());
-
- // inverse transformation
- Scalar theta_incoming_last = -theta_last_incoming;
- Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
- Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
-
- // rotate points
- MatrixXs points_incoming = T_incoming_last * points_last;
-
- // change not defined point
- points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1));
- points_incoming(2,0) = 1;
-
- // compute rigit transformation
- Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true);
- Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
-
- ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12);
- ASSERT_MATRIX_APPROX(v_incoming_last_computed .head(2), t_incoming_last, 1e-12);
- ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
- }
-
-// PRINTF("All good at TestTest::DummyTestExample !\n");
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_polyline_2d.cpp b/test/gtest_polyline_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d332da1dee3f59d4d4df231fcfe855dfe8531df4
--- /dev/null
+++ b/test/gtest_polyline_2d.cpp
@@ -0,0 +1,392 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <stdlib.h>
+#include <core/utils/utils_gtest.h>
+#include "laser/processor/polyline_2d_utils.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+class Polyline2dTest : public testing::Test
+{
+ public:
+
+ Matrix3d T_map_sens1, T_map_sens2, T_map_polyline_center, T_sens1_sens2;
+ MatrixXd points_map, points_sens1, points_sens2;
+
+ void SetUp() override
+ {
+ }
+
+ void SetUpPoints(int n, bool first_defined=true, bool last_defined=true)
+ {
+ // points
+ MatrixXd points = MatrixXd::Random(3,n);
+ points.bottomRows(1) = MatrixXd::Ones(1,n);
+ T_map_polyline_center = pose2T2d(10*Vector2d::Random(), (rand() % 2000*M_PI)*0.001);
+ points_map = T_map_polyline_center * points;
+
+ //std::cout << "points_map: \n" << points_map << std::endl;
+
+ // sens1
+ T_map_sens1 = pose2T2d(5*Vector2d::Random(), (rand() % 2000*M_PI)*0.001);
+ points_sens1 = T_map_sens1.inverse() * points_map;
+ // move first point along the segment from the first to the second
+ if (!first_defined)
+ {
+ double rand_scale = (rand() % 2000 + 1)*0.001;
+ Eigen::Vector2d new_first_point = points_sens1.col(1).head(2) + (points_sens1.col(0).head(2) - points_sens1.col(1).head(2)) * rand_scale;
+
+ std::cout << "old first point: " << points_sens1.col(0).head(2).transpose() << std::endl;
+ std::cout << "new first point: " << new_first_point.transpose() << std::endl;
+ std::cout << "scale: " << rand_scale << std::endl;
+ std::cout << "div vector: " << ((new_first_point - points_sens1.col(1).head(2)).array() / (points_sens1.col(0).head(2) - points_sens1.col(1).head(2)).array()).transpose() << std::endl;
+
+ points_sens1.col(0).head(2) = new_first_point;
+ }
+ // move last point along the segment from the last to the second last
+ if (!last_defined)
+ points_sens1.col(n-1).head(2) = points_sens1.col(n-2).head(2) + (points_sens1.col(n-1).head(2)-points_sens1.col(n-2).head(2)) * (rand() % 2000 + 1)*0.001;
+ //std::cout << "T_map_sens1: \n" << T_map_sens1 << std::endl;
+ //std::cout << "points_sens1: \n" << points_sens1 << std::endl;
+
+ // sens2
+ T_map_sens2 = pose2T2d(5*Vector2d::Random(), (rand() % 2000*M_PI)*0.001);
+ points_sens2 = T_map_sens2.inverse() * points_map;
+ T_sens1_sens2 = T_map_sens1.inverse() * T_map_sens2;
+
+ //std::cout << "T_map_sens2: \n" << T_map_sens2 << std::endl;
+ //std::cout << "points_sens2: \n" << points_sens2 << std::endl;
+ }
+};
+
+TEST_F(Polyline2dTest, Transformations)
+{
+ ASSERT_MATRIX_APPROX(Matrix3d::Identity(), pose2T2d(Vector2d::Zero(),0), 1e-6);
+ ASSERT_MATRIX_APPROX(Matrix3d::Identity(), pose2T2d(Vector3d::Zero()), 1e-6);
+ ASSERT_MATRIX_APPROX(Vector3d::Zero(), T2pose2d(Matrix3d::Identity()), 1e-6);
+
+ Vector3d v_rand, v_subs;
+ v_rand << Vector2d::Random(), pi2pi((rand() % 2000*M_PI)*0.001);
+ v_subs = v_rand - T2pose2d(pose2T2d(v_rand));
+ v_subs(2) = pi2pi(v_subs(2));
+ ASSERT_MATRIX_APPROX(v_subs, Vector3d::Zero(), 1e-6);
+
+ Matrix3d T_rand(Matrix3d::Identity());
+ T_rand.topLeftCorner(2,2) = Rotation2D<double>(v_rand(2)).matrix();
+ T_rand.topRightCorner(2,1) = v_rand.head(2);
+ ASSERT_MATRIX_APPROX(T_rand, pose2T2d(v_rand), 1e-6);
+}
+
+TEST_F(Polyline2dTest, computeRigidTrans8points)
+{
+ bool first_defined = true;
+ bool last_defined = true;
+
+ // Random points
+ for (auto i = 0; i < 1e3; i++)
+ {
+ SetUpPoints(8, first_defined, last_defined);
+
+ // compute rigid transformation
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_sens2), T_sens1_sens2, 1e-6);
+ auto sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_sens2)*points_sens2,
+ 0,
+ points_sens2.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+
+ // compute rigid transformation
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_map), T_map_sens1.inverse(), 1e-6);
+ sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_map)*points_map,
+ 0,
+ points_map.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+ }
+}
+
+TEST_F(Polyline2dTest, computeRigidTrans2points2defined)
+{
+ bool first_defined = true;
+ bool last_defined = true;
+
+ // Random points
+ for (auto i = 0; i < 1e3; i++)
+ {
+ SetUpPoints(2, first_defined, last_defined);
+
+ // compute rigid transformation
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_sens2), T_sens1_sens2, 1e-6);
+ auto sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_sens2)*points_sens2,
+ 0,
+ points_sens2.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+
+ // compute rigid transformation
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_map), T_map_sens1.inverse(), 1e-6);
+ sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_map)*points_map,
+ 0,
+ points_map.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+ }
+}
+
+TEST_F(Polyline2dTest, computeRigidTrans3points2defined)
+{
+ bool first_defined = false;
+ bool last_defined = true;
+
+ // Random points
+ for (auto i = 0; i < 1e3; i++)
+ {
+ SetUpPoints(3, first_defined, last_defined);
+
+ // compute rigid transformation
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_sens2), T_sens1_sens2, 1e-6);
+ auto sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_sens2)*points_sens2,
+ 0,
+ points_sens2.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+
+ // compute rigid transformation
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_map), T_map_sens1.inverse(), 1e-6);
+ sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_map)*points_map,
+ 0,
+ points_map.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+ }
+}
+
+TEST_F(Polyline2dTest, computeRigidTrans3points1defined)
+{
+ bool first_defined = false;
+ bool last_defined = false;
+
+ // Random points
+ for (auto i = 0; i < 1e3; i++)
+ {
+ SetUpPoints(3, first_defined, last_defined);
+
+ // compute rigid transformation
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_sens2), T_sens1_sens2, 1e-6);
+ auto sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_sens2)*points_sens2,
+ 0,
+ points_sens2.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+
+ // compute rigid transformation
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_map), T_map_sens1.inverse(), 1e-6);
+ sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_map)*points_map,
+ 0,
+ points_map.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+ }
+}
+
+TEST_F(Polyline2dTest, computeRigidTrans2points1defined)
+{
+ bool first_defined = false;
+ bool last_defined = true;
+
+ // Random points
+ for (auto i = 0; i < 1e3; i++)
+ {
+ SetUpPoints(2, first_defined, last_defined);
+
+ // compute rigid transformation
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_sens2), T_sens1_sens2, 1e-6);
+ auto sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_sens2)*points_sens2,
+ 0,
+ points_sens2.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+
+ // compute rigid transformation
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_map), T_map_sens1.inverse(), 1e-6);
+ sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_map)*points_map,
+ 0,
+ points_map.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+ }
+}
+
+TEST_F(Polyline2dTest, computeRigidTrans2points1definedBack)
+{
+ bool first_defined = true;
+ bool last_defined = false;
+
+ // Random points
+ for (auto i = 0; i < 1e3; i++)
+ {
+ SetUpPoints(2, first_defined, last_defined);
+
+ // compute rigid transformation
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_sens2), T_sens1_sens2, 1e-6);
+ auto sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_sens2)*points_sens2,
+ 0,
+ points_sens2.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+
+ // compute rigid transformation
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+
+ ASSERT_MATRIX_APPROX(pose2T2d(v_sens1_map), T_map_sens1.inverse(), 1e-6);
+ sq_error = computeSqDist(points_sens1,
+ 0,
+ points_sens1.cols()-1,
+ first_defined,
+ last_defined,
+ pose2T2d(v_sens1_map)*points_map,
+ 0,
+ points_map.cols()-1,
+ first_defined,
+ last_defined,
+ points_sens1.cols()-1,
+ points_sens1.cols());
+ ASSERT_TRUE((sq_error < 1e-6).all());
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loop_closure_falko.cpp b/test/gtest_processor_loop_closure_falko.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..87d7868972e2be0f9e4c88e5a9ab869a0907408b
--- /dev/null
+++ b/test/gtest_processor_loop_closure_falko.cpp
@@ -0,0 +1,719 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/internal/config.h"
+
+#include "core/capture/capture_void.h"
+#include "core/problem/problem.h"
+#include "core/utils/utils_gtest.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
+
+#include "laser/processor/processor_loop_closure_falko.h"
+#include "laser/sensor/sensor_laser_2d.h"
+
+#include "data/scan_data.h"
+
+// STL
+#include <iostream>
+#include <iterator>
+
+using namespace wolf;
+using namespace Eigen;
+using namespace laserscanutils;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoPublic);
+class ProcessorLoopClosureFalkoPublic : public ProcessorLoopClosureFalkoAhtBsc
+{
+ public:
+ ProcessorLoopClosureFalkoPublic(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalkoAhtBsc(_params_falko)
+ {
+ }
+
+ ~ProcessorLoopClosureFalkoPublic() = default;
+
+ std::shared_ptr<LoopClosureFalko<bsc, bscExtractor, aht_matcher>> getLoopClosure()
+ {
+ return loop_closure_;
+ }
+
+ void emplaceFeatures(CaptureBasePtr cap)
+ {
+ ProcessorLoopClosureFalkoAhtBsc::emplaceFeatures(cap);
+ }
+
+ std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr cap)
+ {
+ return ProcessorLoopClosureFalkoAhtBsc::findLoopClosures(cap);
+ }
+};
+
+class ProcessorLoopClosureFalkoTest : public testing::Test
+{
+ public:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ ProcessorLoopClosureFalkoPublicPtr processor;
+
+ virtual void SetUp()
+ {
+
+ // Emplace sensor
+ LaserScanParams _params;
+ _params.angle_min_ = 0;
+ _params.angle_max_ = 2.0 * M_PI;
+ _params.angle_step_ = 0.00701248;
+ sensor =
+ SensorBase::emplace<SensorLaser2d>(problem->getHardware(),
+ std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()),
+ _params);
+
+ // Emplace processor
+
+ ParserYaml parser = ParserYaml("test/yaml/params_processor_loop_closure_falko.yaml", laser_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ParamsProcessorLoopClosureFalkoPtr params =
+ std::make_shared<ParamsProcessorLoopClosureFalko>("ProcessorLoopClosureFalko", server);
+ processor = ProcessorBase::emplace<ProcessorLoopClosureFalkoPublic>(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);
+ }
+
+ CaptureLaser2dPtr emplaceCaptureLaser2d(FrameBasePtr frame, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureLaser2d>(frame, frame->getTimeStamp(), sensor, _ranges);
+ }
+
+ CaptureLaser2dPtr createCaptureLaser2d(TimeStamp ts, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return std::make_shared<CaptureLaser2d>(ts, sensor, _ranges);
+ }
+};
+
+TEST_F(ProcessorLoopClosureFalkoTest, installProcessor)
+{
+ // Stored Frames
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ // Stored Captures
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, frame_stored)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, capture_stored)
+{
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, captureCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ ASSERT_TRUE(frm1 != nullptr);
+
+ std::vector<float> ranges;
+ auto cap1 = emplaceCaptureLaser2d(frm1, ranges);
+
+ // captureCallback
+ std::cout << __LINE__ << std::endl;
+ processor->captureCallback(cap1);
+ std::cout << __LINE__ << std::endl;
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, captureCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // new capture
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(1, ranges);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, captureCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(2, ranges);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ // 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(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase1)
+{
+ // emplace frame and capture
+ std::cout << __LINE__ << std::endl;
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ std::cout << __LINE__ << std::endl;
+ std::vector<float> ranges;
+ std::cout << __LINE__ << std::endl;
+ auto cap1 = emplaceCaptureLaser2d(frm1, ranges);
+
+ std::cout << __LINE__ << std::endl;
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+ std::cout << __LINE__ << std::endl;
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the
+ std::cout << __LINE__ << std::endl;
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ std::cout << __LINE__ << std::endl;
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+ std::cout << __LINE__ << std::endl;
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(1, ranges);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(2, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase4)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, 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
+ std::vector<float> ranges;
+ auto cap4 = createCaptureLaser2d(4, ranges);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+ problem->keyFrameCallback(frm2, nullptr);
+ problem->keyFrameCallback(frm3, nullptr);
+ problem->keyFrameCallback(frm4, nullptr);
+ problem->keyFrameCallback(frm5, nullptr);
+
+ // 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(ProcessorLoopClosureFalkoTest, keyFrameCallbackMatch)
+{
+ // new frame
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ // new captures
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(1, ranges);
+ auto cap2 = createCaptureLaser2d(2, ranges);
+ auto cap3 = createCaptureLaser2d(3, ranges);
+ auto cap4 = createCaptureLaser2d(4, ranges);
+ auto cap5 = createCaptureLaser2d(5, ranges);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ processor->captureCallback(cap2);
+ processor->captureCallback(cap3);
+ processor->captureCallback(cap4);
+ processor->captureCallback(cap5);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm2, nullptr);
+
+ 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(ProcessorLoopClosureFalkoTest, emplaceFeatures)
+{
+ std::vector<float> ranges;
+
+ // add scan to capture:
+ auto cap1 = createCaptureLaser2d(1, ranges);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, findLoopClosures)
+{
+ Vector3d pos1 = {0.5, 0.3, 0};
+ auto cap1 = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), testRanges1);
+ auto cap2 = emplaceCaptureLaser2d(emplaceFrame(2, pos1), testRanges2);
+ auto cap3 = emplaceCaptureLaser2d(emplaceFrame(3, Vector3d::Zero()), testRanges1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ EXPECT_EQ(processor->match_list_.size(), 0);
+
+ processor->captureCallback(cap2);
+ EXPECT_EQ(processor->match_list_.size(), 1);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap2);
+
+ processor->captureCallback(cap3);
+ EXPECT_EQ(processor->match_list_.size(), 3);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap3);
+
+ // Verify factors
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 1);
+ EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 2);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestExtractScene)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_1);
+ auto cap_target = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), target_scan_1);
+
+ processor->captureCallback(cap_ref);
+ processor->captureCallback(cap_target);
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+
+ auto cap_laser_1 = std::dynamic_pointer_cast<CaptureLaser2d>(cap_target);
+ auto scan_1 = cap_laser_1->getScan();
+
+ for (auto feat : cap_target->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_target = feat_falko->getScene()->keypoints_list_;
+ }
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front());
+
+ auto associations = match_falko->match_falko_->associations;
+
+ std::cout<< "transform : " << match_falko->match_falko_->transform_vector.transpose() << std::endl;
+
+ for (auto asso : associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestExtractSceneReference)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_2);
+
+ processor->captureCallback(cap_ref);
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+ std::cout << "keypoints size : " << key_ref.size() << std::endl;
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestExtractScene2)
+{
+ // Initialization
+ LaserScan scan_target, scan_ref;
+ LaserScanParams laser_params;
+
+ laser_params.angle_min_ = 0;
+ laser_params.angle_max_ = 2.0 * M_PI;
+
+ // ** TEST WITH TARGET AND REFERENCE SCENE
+ for (int i = 0; i < target_scan_1.size(); i++)
+ {
+ scan_target.ranges_raw_.push_back(target_scan_1[i]);
+ scan_ref.ranges_raw_.push_back(reference_scan_1[i]);
+ }
+ laser_params.angle_step_ = 0.00701248;
+
+ auto loop_cl_falko_2 = processor->getLoopClosure();
+
+ auto new_scene_target = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2->extractScene(scan_target, laser_params));
+ auto new_scene_reference = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2->extractScene(scan_ref, laser_params));
+
+ // std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl;
+ // std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl;
+
+ auto match_r_t = loop_cl_falko_2->matchScene(new_scene_reference, new_scene_target);
+ for (int i = 0; i < match_r_t->associations.size(); i++)
+ if (match_r_t->associations[i].second != -1)
+ {
+ std::cout << "id first : " << match_r_t->associations[i].first << std::endl;
+ std::cout << "id second : " << match_r_t->associations[i].second << std::endl;
+ }
+
+ auto key_ref = new_scene_reference->keypoints_list_;
+ auto key_target = new_scene_target->keypoints_list_;
+
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+ for (auto asso : match_r_t->associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestEmplaceFeatures)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_1);
+ auto cap_target = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), target_scan_1);
+
+ processor->emplaceFeatures(cap_target);
+ processor->emplaceFeatures(cap_ref);
+
+ auto cap_laser_1 = std::dynamic_pointer_cast<CaptureLaser2d>(cap_target);
+ auto scan_1 = cap_laser_1->getScan();
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_target->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_target = feat_falko->getScene()->keypoints_list_;
+ }
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+ // std::cout << "reference keypoints size : " << key_ref.size() << std::endl;
+ // std::cout << "target keypoints size : " << key_target.size() << std::endl;
+
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ auto matches = processor->findLoopClosures(cap_target);
+
+ ASSERT_TRUE(matches.size() > 0);
+
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(matches.begin()->second);
+
+ auto associations = match_falko->match_falko_->associations;
+
+ // std::cout << "associations.size() :" << associations.size() << std::endl;
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+
+ for (auto asso : associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestProcessCapture)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_1);
+ auto cap_target = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), target_scan_1);
+
+ processor->captureCallback(cap_ref);
+ auto cap_laser_1 = std::dynamic_pointer_cast<CaptureLaser2d>(cap_target);
+ auto scan_1 = cap_laser_1->getScan();
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+ cap_target->process();
+ //processor->process(cap_target);
+
+ for (auto feat : cap_target->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_target = feat_falko->getScene()->keypoints_list_;
+ }
+
+ // std::cout << "reference keypoints size : " << key_ref.size() << std::endl;
+ // std::cout << "target keypoints size : " << key_target.size() << std::endl;
+ // std::cout << "processor->match_list_.size() : " << processor->match_list_.size() << std::endl;
+
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front());
+ auto associations = match_falko->match_falko_->associations;
+
+ for (auto asso : associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loop_closure_falko_icp.cpp b/test/gtest_processor_loop_closure_falko_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a12e6ecd45e89bc7f6e92021e28a5800d14e97b0
--- /dev/null
+++ b/test/gtest_processor_loop_closure_falko_icp.cpp
@@ -0,0 +1,121 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/internal/config.h"
+#include "core/capture/capture_void.h"
+#include "core/problem/problem.h"
+#include "core/utils/utils_gtest.h"
+
+#include "laser/processor/processor_loop_closure_falko_icp.h"
+#include "laser/sensor/sensor_laser_2d.h"
+
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
+
+#include "data/scan_data.h"
+
+// STL
+#include <iostream>
+#include <iterator>
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+class ProcessorLoopClosureFalkoIcpTest : public testing::Test
+{
+ public:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ // std::shared_ptr<ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, nn_matcher>> processor;
+ ProcessorLoopClosureFalkoIcpNnBscPtr processor;
+
+ virtual void SetUp()
+ {
+
+ // Emplace sensor
+ laserscanutils::LaserScanParams _params;
+ _params.angle_min_ = 0;
+ _params.angle_max_ = 2.0 * M_PI;
+ _params.angle_step_ = 2.0 * M_PI / (testRanges1.size() - 1);
+ _params.range_min_ = 0;
+ _params.range_max_ = 200;
+ sensor =
+ SensorBase::emplace<SensorLaser2d>(problem->getHardware(), std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()), _params);
+
+ // Emplace processor
+ ParserYaml parser = ParserYaml("test/yaml/params_processor_loop_closure_falko.yaml", laser_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto params = std::make_shared<ParamsProcessorLoopClosureFalkoIcp>("ProcessorLoopClosureFalko",
+ server);
+ processor = ProcessorBase::emplace<ProcessorLoopClosureFalkoIcpNnBsc>(sensor, params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d &x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureLaser2dPtr emplaceCaptureLaser2d(FrameBasePtr frame, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureLaser2d>(frame, frame->getTimeStamp(), sensor, _ranges);
+ }
+
+ CaptureLaser2dPtr createCaptureLaser2d(TimeStamp ts, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return std::make_shared<CaptureLaser2d>(ts, sensor, _ranges);
+ }
+};
+
+TEST_F(ProcessorLoopClosureFalkoIcpTest, validatefindLoopClosures)
+{
+
+ auto cap1 = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), testRanges1);
+ auto cap2 = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), testRanges2);
+ auto cap3 = emplaceCaptureLaser2d(emplaceFrame(3, Vector3d::Zero()), testRanges1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ EXPECT_EQ(processor->match_list_.size(), 0);
+
+ processor->captureCallback(cap2);
+ EXPECT_EQ(processor->match_list_.size(), 1);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap2);
+
+ processor->captureCallback(cap3);
+ EXPECT_EQ(processor->match_list_.size(), 3);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap3);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loop_closure_icp.cpp b/test/gtest_processor_loop_closure_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..491e22aafa5e22ebab10eaf7d436fc615a366889
--- /dev/null
+++ b/test/gtest_processor_loop_closure_icp.cpp
@@ -0,0 +1,240 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/internal/config.h"
+
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "laser/processor/processor_loop_closure_icp.h"
+#include <stdlib.h>
+#include <random>
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+class ProcessorLoopClosureIcp_Test : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorLaser2dPtr lidar;
+ ProcessorLoopClosureIcpPtr processor;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ lidar = std::static_pointer_cast<SensorLaser2d>(problem->installSensor("SensorLaser2d",
+ "lidar",
+ Eigen::Vector3d(0,0,0),
+ laser_root_dir + "/test/yaml/sensor_laser_2d.yaml"));
+ auto params = std::make_shared<ParamsProcessorLoopClosureIcp>();
+
+ params->recent_frames_ignored = 0;
+ params->frames_ignored_after_loop = 0;
+ params->max_error_threshold = 0.1;
+ params->min_points_percent = 50;
+ params->max_candidates = 50;
+ params->max_attempts = 50;
+ params->candidate_generation = "random";
+
+ params->icp_params.use_point_to_line_distance = true;
+ params->icp_params.max_correspondence_dist = 1e9;
+ params->icp_params.max_iterations = 1e3;
+ params->icp_params.use_corr_tricks = true;
+ params->icp_params.outliers_maxPerc = 0.8;
+ params->icp_params.outliers_adaptive_order = 0.7;
+ params->icp_params.outliers_adaptive_mult = 2;
+ params->icp_params.do_compute_covariance = true;
+ params->icp_params.cov_factor = 1;
+ params->icp_params.max_angular_correction_deg = 1.5;
+ params->icp_params.max_linear_correction = 10;
+ params->icp_params.epsilon_xy = 0.01;
+ params->icp_params.epsilon_theta = 0.01;
+ params->icp_params.sigma = 0.1;
+ params->icp_params.restart = false;
+ params->icp_params.restart_threshold_mean_error = 0;
+ params->icp_params.restart_dt = 0;
+ params->icp_params.restart_dtheta = 0;
+ params->icp_params.clustering_threshold = 0.2;
+ params->icp_params.orientation_neighbourhood = 5;
+ params->icp_params.do_alpha_test = false;
+ params->icp_params.do_alpha_test_thresholdDeg = 0;
+ params->icp_params.do_visibility_test = false;
+ params->icp_params.outliers_remove_doubles = false;
+ params->icp_params.debug_verify_tricks = false;
+ params->icp_params.min_reading = 0;
+ params->icp_params.max_reading = 100;
+ params->icp_params.use_ml_weights = false;
+ params->icp_params.use_sigma_weights = false;
+
+ processor = std::static_pointer_cast<ProcessorLoopClosureIcp>(problem->installProcessor("ProcessorLoopClosureIcp",
+ "prc icp",
+ lidar,
+ params));
+ }
+
+ std::vector<float> generateRanges(const int& frame_idx)
+ {
+ // generating synthetic 360º scan, half with range_A and half with range_B depending on the frame_idx. Randomly rotated.
+ // loop closures should occur (exact ranges but rotated) between pairs of frame_idx separated by 4 (0-4, 1-5, ...)
+
+ int n_ranges = std::round((lidar->getScanParams().angle_max_-lidar->getScanParams().angle_min_) / lidar->getScanParams().angle_step_);
+
+ // shape: 0-2 half circles, 1-square, 2-star-like
+ int shape = frame_idx % 3;
+ WOLF_DEBUG_COND(shape == 0, "Generating ranges: HALF CIRCLES");
+ WOLF_DEBUG_COND(shape == 1, "Generating ranges: SQUARE");
+ WOLF_DEBUG_COND(shape == 2, "Generating ranges: STAR-LIKE");
+
+ std::vector<float> ranges(n_ranges);
+ for (auto i = 0; i < n_ranges; i++)
+ {
+ // HALF CIRCLES (10m and 5m)
+ if (shape == 0)
+ ranges[i] = (i < n_ranges / 2 ? 10.0 : 5.0);
+
+ // SQUARE (2m)
+ else if (shape == 1)
+ ranges[i] = 2.0 * sqrt(std::pow(cos(i * lidar->getScanParams().angle_step_),2) +
+ std::pow(sin(i * lidar->getScanParams().angle_step_),2));
+
+ // STAR-LIKE (3m to 10m)
+ else if (shape == 2)
+ ranges[i] = 3.0 + (i % (n_ranges / 6)) * 8.0 / (n_ranges / 6);
+ }
+
+ // rotate randomly
+ std::random_device rd; // obtain a random number from hardware
+ std::mt19937 gen(rd()); // seed the generator
+ std::uniform_int_distribution<> distr(0, 5); // define the range of random
+ int init = distr(gen); // generate random number
+ std::rotate(ranges.begin(),ranges.begin()+init,ranges.end());
+ WOLF_DEBUG("Rotated ranges in ", init, " beams");
+
+ return ranges;
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d &x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureLaser2dPtr emplaceCaptureLaser2d(FrameBasePtr frame, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureLaser2d>(frame, frame->getTimeStamp(), lidar, _ranges);
+ }
+
+ void test()
+ {
+ TimeStamp t(0.0);
+
+ auto i_loop = 3;
+ auto N = 6;
+ while (i_loop < processor->getParams()->recent_frames_ignored)
+ {
+ i_loop += 3;
+ N += 3;
+ }
+
+ for (int i = 0; i < N; i++)
+ {
+ auto frm = emplaceFrame(t, Vector3d::Zero());
+ frm->perturb();
+ auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+
+ processor->keyFrameCallback(frm);
+
+ WOLF_INFO_COND(i == i_loop, "Loop should have been detected:");
+ problem->print(4,1,1,1);
+
+ if (i == i_loop)
+ {
+ ASSERT_EQ(cap->getFeatureList().size(), 1);
+ ASSERT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
+ ASSERT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
+
+ i_loop += 1 + processor->getParams()->frames_ignored_after_loop;
+ }
+ else
+ {
+ EXPECT_TRUE(cap->getFeatureList().empty());
+ EXPECT_TRUE(frm->getConstrainedByList().empty());
+ }
+ t += 1;
+ }
+ }
+};
+
+
+TEST_F(ProcessorLoopClosureIcp_Test, setup)
+{
+ ASSERT_TRUE(problem->check());
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_random)
+{
+ test();
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_ignore_previous_random)
+{
+ processor->getParams()->recent_frames_ignored = 4;
+ test();
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_frames_ignored_after_loop_random)
+{
+ processor->getParams()->frames_ignored_after_loop = 2;
+ test();
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_tree)
+{
+ processor->getParams()->candidate_generation = "tree";
+ test();
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_ignore_previous_tree)
+{
+ processor->getParams()->candidate_generation = "tree";
+ processor->getParams()->recent_frames_ignored = 4;
+ test();
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_frames_ignored_after_loop_tree)
+{
+ processor->getParams()->candidate_generation = "tree";
+ processor->getParams()->frames_ignored_after_loop = 2;
+ test();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4b09cad83ef2e1200bd9c14503b1f6d093fa8f58
--- /dev/null
+++ b/test/gtest_processor_odom_icp.cpp
@@ -0,0 +1,219 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file gtest_processor_odom_icp.cpp
+ *
+ * Created on: Aug 6, 2019
+ * \author: jsola
+ */
+
+
+#include "laser/internal/config.h"
+
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "laser/processor/processor_odom_icp.h" // THIS AT THE END OTHERWISE IT FAILS COMPILING
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+class ProcessorOdomIcpPublic : public ProcessorOdomIcp
+{
+ public:
+ ProcessorOdomIcpPublic(const ParamsProcessorOdomIcpPtr& _params) :
+ ProcessorOdomIcp(_params)
+ {
+
+ }
+ ~ProcessorOdomIcpPublic() override{}
+
+ // we can add public methods here to access protected stuff in ProcessorOdomIcp
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomIcpPublic);
+
+class ProcessorOdomIcp_Test : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ SensorLaser2dPtr lidar;
+ ProcessorOdomIcpPublicPtr processor;
+
+ std::vector<float> ranges;
+
+ TimeStamp t0;
+ VectorComposite x0; // prior state
+ VectorComposite s0; // prior sigmas
+ FrameBasePtr F0, F; // keyframes
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ solver = std::make_shared<SolverCeres>(problem);
+ auto sen = problem->installSensor("SensorLaser2d", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2d.yaml");
+
+ lidar = std::static_pointer_cast<SensorLaser2d>(sen);
+
+ auto prc = problem->installProcessor("ProcessorOdomIcp", "prc icp", "lidar", laser_root_dir + "/test/yaml/processor_odom_icp.yaml");
+ processor = std::static_pointer_cast<ProcessorOdomIcpPublic>(prc);
+
+ ranges = std::vector<float>({49.97591781616211, 49.996429443359375, 49.999759674072266, 50.0, 50.0, 50.0, 49.998634338378906, 50.0, 50.0, 49.99236297607422, 49.99384307861328, 50.0, 49.9869270324707, 50.0, 49.99005889892578, 49.99773406982422, 50.0, 49.98741912841797, 50.0, 49.99842071533203, 50.0, 49.99243927001953, 49.997291564941406, 50.0, 50.0, 49.98580551147461, 49.991844177246094, 49.98896026611328, 50.0, 50.0, 49.9897346496582, 49.998111724853516, 49.99882125854492, 50.0, 50.0, 50.0, 50.0, 49.999698638916016, 50.0, 50.0, 50.0, 50.0, 49.991397857666016, 49.99360275268555, 49.999027252197266, 49.99750900268555, 49.99100112915039, 49.998714447021484, 49.98794174194336, 50.0, 50.0, 50.0, 50.0, 50.0, 49.98186492919922, 50.0, 50.0, 50.0, 49.99155807495117, 49.997196197509766, 49.98872375488281, 49.99138259887695, 50.0, 49.99021530151367, 49.99164581298828, 50.0, 49.991390228271484, 50.0, 50.0, 50.0, 49.997249603271484, 50.0, 49.991851806640625, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.97983169555664, 49.98630142211914, 50.0, 20.6888370513916, 49.9797477722168, 49.98846435546875, 49.99418640136719, 50.0, 50.0, 50.0, 49.99342346191406, 50.0, 49.9906005859375, 50.0, 49.99853515625, 49.989444732666016, 38.552852630615234, 38.28703689575195, 38.04865264892578, 37.78112030029297, 37.54747772216797, 37.28171157836914, 37.206565856933594, 36.835411071777344, 36.61864471435547, 36.39655685424805, 36.1579475402832, 35.95964431762695, 35.75605773925781, 35.552188873291016, 35.75926208496094, 36.27781677246094, 35.16993713378906, 34.99699401855469, 34.82910919189453, 34.6483039855957, 34.48637390136719, 34.32539749145508, 34.16202163696289, 34.232051849365234, 33.86043167114258, 33.71691131591797, 33.566650390625, 33.42384338378906, 33.2882080078125, 33.16693115234375, 33.041419982910156, 32.906009674072266, 33.008323669433594, 33.706356048583984, 34.43825149536133, 35.25088119506836, 36.05652618408203, 36.930118560791016, 37.83384704589844, 33.12321472167969, 33.02351760864258, 32.9192008972168, 32.82925796508789, 32.74382781982422, 32.64959716796875, 32.580204010009766, 32.49120330810547, 33.05714797973633, 32.343536376953125, 32.26381301879883, 32.21063232421875, 32.12488555908203, 32.06965255737305, 32.0222282409668, 31.954957962036133, 31.903532028198242, 31.83578872680664, 32.51456832885742, 34.189456939697266, 31.12668228149414, 31.076339721679688, 31.047151565551758, 30.967018127441406, 30.956220626831055, 30.924589157104492, 30.893285751342773, 30.869199752807617, 30.843050003051758, 32.791847229003906, 30.809431076049805, 30.79128074645996, 30.779237747192383, 30.776460647583008, 30.74305534362793, 30.74994468688965, 30.7137393951416, 30.734609603881836, 30.719928741455078, 30.71673011779785, 49.99970626831055, 50.0, 49.987911224365234, 33.68583679199219, 31.76846694946289, 31.8026123046875, 31.802202224731445, 31.818490982055664, 31.85223960876465, 31.86141014099121, 31.906801223754883, 31.93423843383789, 31.964210510253906, 33.567230224609375, 32.055015563964844, 32.07001876831055, 32.13076400756836, 32.16000747680664, 32.22781753540039, 32.26890563964844, 32.323944091796875, 32.36326217651367, 32.430908203125, 49.980655670166016, 34.32135772705078, 33.09465789794922, 32.27247619628906, 32.33710479736328, 32.41763687133789, 32.498661041259766, 32.57213592529297, 32.67158126831055, 32.74591827392578, 32.814476013183594, 32.93477249145508, 33.04751968383789, 33.136863708496094, 33.23999786376953, 33.34675979614258, 33.42970657348633, 33.53573226928711, 33.66716003417969, 33.78378677368164, 33.905670166015625, 34.02836608886719, 34.151817321777344, 34.2794189453125, 34.41516876220703, 34.551273345947266, 34.702728271484375, 34.84151840209961, 34.986881256103516, 35.162757873535156, 35.332794189453125, 35.47941970825195, 35.65633010864258, 35.82624435424805, 36.00060272216797, 36.17729187011719, 36.36515808105469, 36.55763626098633, 36.744773864746094, 38.46407699584961, 37.869808197021484, 37.767921447753906, 37.958900451660156, 38.20857620239258, 38.38622283935547, 38.68323516845703, 38.871334075927734, 39.151519775390625, 39.377750396728516, 39.68268966674805, 39.89873123168945, 40.197330474853516, 40.47549819946289, 40.73743438720703, 41.04566955566406, 42.33650207519531, 41.92591094970703, 41.43912124633789, 41.045528411865234, 41.32114028930664, 41.581878662109375, 41.944580078125, 42.318912506103516, 42.6725959777832, 43.07264709472656, 43.443477630615234, 43.83216094970703, 44.19996643066406, 44.63225555419922, 45.06049346923828, 45.468536376953125, 45.89896774291992, 46.330604553222656, 46.778343200683594, 47.31666946411133, 47.789310455322266, 48.26376724243164, 48.826602935791016, 49.33188247680664, 49.990909576416016, 50.0, 50.0, 50.0, 50.0, 49.995697021484375, 49.99568176269531, 49.98163986206055, 50.0, 50.0, 49.9764289855957, 50.0, 50.0, 49.98639678955078, 49.99431228637695, 50.0, 50.0, 50.0, 50.0, 49.9874267578125, 50.0, 49.98714828491211, 50.0, 49.99470901489258, 49.99113464355469, 50.0, 50.0, 50.0, 49.985504150390625, 49.99067306518555, 50.0, 49.997161865234375, 50.0, 50.0, 50.0, 49.995513916015625, 49.993038177490234, 50.0, 49.99763107299805, 50.0, 49.98752975463867, 50.0, 49.988037109375, 50.0, 50.0, 50.0, 49.9975700378418, 50.0, 49.998756408691406, 49.97819519042969, 49.99104690551758, 49.99087905883789, 49.94268798828125, 49.85968017578125, 49.786617279052734, 49.70594787597656, 49.62562561035156, 49.56686782836914, 49.50475311279297, 49.416934967041016, 49.35699462890625, 49.308589935302734, 49.990482330322266, 50.0, 49.998836517333984, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.980472564697266, 49.99903869628906, 50.0, 50.0, 49.989845275878906, 49.98395919799805, 50.0, 49.99302673339844, 49.99530792236328, 49.99745559692383, 50.0, 49.99560546875, 21.569303512573242});
+
+ t0 = 0.0;
+ x0 = VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
+ s0 = VectorComposite("PO", {Vector2d(1,1), Vector1d(1)});
+ F0 = problem->setPriorFactor(x0, s0, t0);
+ }
+ void TearDown() override{}
+};
+
+TEST(ParamsProcessorOdomIcp, default_construct_and_print)
+{
+ auto params = std::make_shared<ParamsProcessorOdomIcp>();
+
+ ASSERT_TRUE(params); // not nullptr
+
+ params->icp_params.use_corr_tricks = false;
+
+ ASSERT_EQ(params->icp_params.use_corr_tricks, false);
+
+ WOLF_INFO("params: ", params->print());
+}
+
+TEST(ParamsProcessorOdomIcp, factory_and_print)
+{
+ auto params = std::static_pointer_cast<ParamsProcessorOdomIcp>(FactoryParamsProcessor::create("ProcessorOdomIcp", laser_root_dir + "/test/yaml/processor_odom_icp.yaml"));
+ ASSERT_TRUE(params); // not nullptr
+
+ // check a couple of entries.
+ ASSERT_EQ (params->icp_params.use_corr_tricks , 1 );
+ ASSERT_DOUBLE_EQ(params->icp_params.outliers_maxPerc , 5.0 );
+
+ WOLF_INFO("params: ", params->print());
+}
+
+TEST(ProcessorOdomIcp, Constructor)
+{
+ auto params = std::make_shared<ParamsProcessorOdomIcp>();
+ auto prc = std::make_shared<ProcessorOdomIcp>(params);
+
+ ASSERT_TRUE(prc); // not nullptr
+ ASSERT_EQ(prc->getType(), "ProcessorOdomIcp");
+}
+
+TEST(ProcessorOdomIcp, creator_yaml)
+{
+ auto params = std::static_pointer_cast<ParamsProcessorOdomIcp>(FactoryParamsProcessor::create("ProcessorOdomIcp", laser_root_dir + "/test/yaml/processor_odom_icp.yaml"));
+ auto prc = std::make_shared<ProcessorOdomIcp>(params);
+
+ ASSERT_TRUE(prc); // not nullptr
+ ASSERT_EQ(prc->getType(), "ProcessorOdomIcp");
+}
+
+TEST_F(ProcessorOdomIcp_Test, full)
+{
+
+ TimeStamp t(t0);
+
+
+ for (int i = 0; i < 7; i++)
+ {
+ std::cout << "\n========== Process " << t.getSeconds() << " sec. ==========" << std::endl;
+
+ CaptureLaser2dPtr scan = std::make_shared<CaptureLaser2d>(t, lidar, ranges);
+ scan->process();
+
+ // F = problem->getLastFrame();
+ F = processor->getLast()->getFrame();
+
+ WOLF_TRACE("last capture ", processor->getOrigin()->id() ,
+ " - features: ", processor->getOrigin()->getFeatureList().size());
+
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO"), x0.vector("PO"), 1e-6);
+
+ // check that feature and factor has been emplaced
+ ASSERT_TRUE(i <= 1 or processor->getOrigin()->getFeatureList().size() > 0);
+
+ FactorBasePtrList factor_list;
+ processor->getOrigin()->getFactorList(factor_list);
+ ASSERT_TRUE(i <= 1 or factor_list.size() > 0);
+
+ WOLF_TRACE("F", F->id() , " ", F->getState().vector("PO").transpose());
+
+ if (i >= 2 && i <= 4)
+ {
+ // perturb F
+ F->perturb(0.1);
+ WOLF_TRACE("F", F->id() , " ", F->getState().vector("PO").transpose(), " perturbed!");
+ }
+
+ t += 1.0;
+ }
+
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO"), x0.vector("PO"), 1e-6);
+}
+
+TEST_F(ProcessorOdomIcp_Test, solve)
+{
+ TimeStamp t(t0);
+
+ for (int i = 0; i < 6; i++)
+ {
+ std::cout << "\n========== Process " << t.getSeconds() << " sec. ==========" << std::endl;
+
+ CaptureLaser2dPtr scan = std::make_shared<CaptureLaser2d>(t, lidar, ranges);
+ scan->process();
+
+ // check that feature and factor has been emplaced
+ ASSERT_TRUE(i <= 1 or processor->getOrigin()->getFeatureList().size() > 0);
+
+ FactorBasePtrList factor_list;
+ processor->getOrigin()->getFactorList(factor_list);
+ ASSERT_TRUE(i <= 1 or factor_list.size() > 0);
+
+ t += 1.0;
+ }
+
+ for (auto F : *problem->getTrajectory())
+ F->perturb(0.5);
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_TRACE(report);
+
+ for (auto F : *problem->getTrajectory())
+ {
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
+ }
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "ProcessorOdomIcp_Test.solve";
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_tracker_feature_polyline_2D.cpp b/test/gtest_processor_tracker_feature_polyline_2D.cpp
deleted file mode 100644
index fdda00d566df60a04e27f8b1cc60f157f5943900..0000000000000000000000000000000000000000
--- a/test/gtest_processor_tracker_feature_polyline_2D.cpp
+++ /dev/null
@@ -1,1183 +0,0 @@
-/**
- * \file gtest_processor_tracker_feature_polyline_2D.cpp
- *
- * Created on: Apr 3, 2019
- * \author: jvallve
- */
-
-#include "laser/processor/processor_tracker_feature_polyline_2D.h"
-#include "core/common/wolf.h"
-
-#include "utils_gtest.h"
-#include "core/utils/logging.h"
-
-#include "core/ceres_wrapper/ceres_manager.h"
-
-#include <cmath>
-#include <iostream>
-
-using namespace Eigen;
-using namespace wolf;
-WOLF_PTR_TYPEDEFS(ProcessorPolylinePublicMethods);
-class ProcessorPolylinePublicMethods : public ProcessorTrackerFeaturePolyline2D
-{
- public:
- ProcessorPolylinePublicMethods(ProcessorParamsTrackerFeaturePolyline2DPtr _params) :
- ProcessorTrackerFeaturePolyline2D(_params)
- {
-
- }
-
- unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
- {
- return ProcessorTrackerFeaturePolyline2D::trackFeatures(_features_last_in,_features_incoming_out,_feature_correspondences);
- }
-
- bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
- {
- return ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(_origin_feature,
- _last_feature,
- _incoming_feature);
- }
-
- bool voteForKeyFrame()
- {
- return ProcessorTrackerFeaturePolyline2D::voteForKeyFrame();
- }
-
- unsigned int processNew(const unsigned int& _max_features)
- {
- return ProcessorTrackerFeaturePolyline2D::processNew(_max_features);
- }
-
- unsigned int detectNewFeatures(const int& _max_new_features,
- FeatureBasePtrList& _features_last_out)
- {
- return ProcessorTrackerFeaturePolyline2D::detectNewFeatures(_max_new_features, _features_last_out);
- }
-
- void establishConstraints()
- {
- ProcessorTrackerFeaturePolyline2D::establishFactors();
- }
-
- void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id)
- {
- ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(_polyline_feature,
- _polyline_landmark,
- _ftr_point_id,
- _lmk_point_id,
- _lmk_prev_point_id);
- }
-
- void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
- LandmarkPolyline2DPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id)
- {
- ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(_polyline_feature,
- _polyline_landmark,
- _ftr_point_id,
- _lmk_point_id);
- }
-
- LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr)
- {
- return ProcessorTrackerFeaturePolyline2D::createLandmark(_feature_ptr);
- }
-
- bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
- {
- return ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch( lmk_match, pl_ftr);
- }
-
- void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
- const FeaturePolyline2DPtr& _ftr_last,
- const FeaturePolyline2DPtr& _ftr_incoming,
- const Eigen::Matrix3s& _T_last_incoming_prior) const
- {
- ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature( ftr_matches,
- _ftr_last,
- _ftr_incoming,
- _T_last_incoming_prior);
- }
-
- void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const
- {
- ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(lmk_matches,
- _lmk_ptr,
- _feat_ptr,
- _T_feature_landmark_prior);
- }
-
- bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const
- {
- return ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(_lmk_match, _lmk_ptr, _feat_ptr);
- }
-
- void computeTransformations()
- {
- ProcessorTrackerFeaturePolyline2D::computeTransformations();
- }
-
- LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
- FeatureMatchPolyline2DPtr ftr_match_incoming_last,
- LandmarkMatchPolyline2DPtr lmk_match_last) const
- {
- return ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(pl_incoming,
- ftr_match_incoming_last,
- lmk_match_last);
- }
-};
-
-class ProcessorPolyline2Dt : public testing::Test
-{
- public: //These can be accessed in fixtures
- ProcessorPolylinePublicMethodsPtr processor_pl;
- wolf::SensorBasePtr sensor_ptr;
- wolf::TimeStamp t;
- wolf::Scalar mti_clock, tmp;
- wolf::Scalar dt;
- Vector6s data;
- Matrix6s data_cov;
- VectorXs x0;
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
-
- //a new of this will be created for each new test
- virtual void SetUp()
- {
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
- sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
-
- // Time and data variables
- data = Vector6s::Zero();
- data_cov = Matrix6s::Identity();
-
- // Set the origin
- x0.resize(10);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-TEST(ProcessorIMU_constructors, ALL)
-{
- using namespace wolf;
-
- //constructor with ProcessorIMUParamsPtr argument only
- ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
- param_ptr->max_time_span = 2.0;
- param_ptr->max_buff_length = 20000;
- param_ptr->dist_traveled = 2.0;
- param_ptr->angle_turned = 2.0;
-
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
- ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
- ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
- ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
- ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
-
- //Factory constructor without yaml
- std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
- ProcessorParamsIMU params_default;
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default.angle_turned);
-
- //Factory constructor with yaml
- processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
-}
-
-TEST(ProcessorIMU, voteForKeyFrame)
-{
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 1;
- prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_imu_params->dist_traveled = 1000000000;
- prc_imu_params->angle_turned = 1000000000;
- prc_imu_params->voting_active = true;
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
-
- //setting origin
- VectorXs x0(10);
- TimeStamp t(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- //data variable and covariance matrix
- // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- Vector6s data;
- data << 1,0,0, 0,0,0;
- Matrix6s data_cov(Matrix6s::Identity());
- data_cov(0,0) = 0.5;
-
- // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
-
- // Time
- // we want more than one data to integrate otherwise covariance will be 0
- Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- /*There should be 3 frames :
- - 1 KeyFrame at origin
- - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
- - 1 frame that would be used by future data
- */
- ASSERT_EQ(problem->getTrajectoryPtr()->getFrameList().size(),(unsigned int) 3);
-
- /* if max_time_span == 2, Wolf tree should be
-
- Hardware
- S1
- pm5
- o: C2 - F2
- l: C4 - F3
- Trajectory
- KF1
- Estim, ts=0, x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0)
- C1 -> S1
- KF2
- Estim, ts=2.1, x = ( 2.2 0 -22 0 0 0 1 2.1 0 -21 0 0 0 0 0 0 )
- C2 -> S1
- f1
- m = ( 2.21 0 0 0 0 0 1 2.1 0 0 )
- c1 --> F1
- F3
- Estim, ts=2.1, x = ( . . .)
- C4 -> S1
- */
- //TODO : ASSERT TESTS to make sure the constraints are correctly set + check the tree above
-
-}
-
-//replace TEST by TEST_F if SetUp() needed
-TEST_F(ProcessorIMUt, interpolate)
-{
- using namespace wolf;
-
- t.set(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 0, 0, 0, 0; // only acc_x
- cap_imu_ptr->setData(data);
-
- // make one step to depart from origin
- cap_imu_ptr->setTimeStamp(0.05);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
-
- // make two steps, then pretend it's just one
- cap_imu_ptr->setTimeStamp(0.10);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
-
- cap_imu_ptr->setTimeStamp(0.15);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
- mot_final.delta_ = mot_final.delta_integr_;
- Motion mot_sec = mot_final;
-
-// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
-
-class P : public wolf::ProcessorIMU
-{
- public:
- P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
- Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
- {
- return ProcessorIMU::interpolate(ref, sec, ts);
- }
-} imu;
-
-Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
-
-ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
-//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
-ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
-
-}
-
-TEST_F(ProcessorIMUt, acc_x)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- Vector6s b; b << 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-}
-
-TEST_F(ProcessorIMUt, acc_y)
-{
- // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
- // difference hier is that we integrate over 1ms periods
-
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
- x << 0,10,0, 0,0,0,1, 0,20,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, acc_z)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 50; //how 0.1s
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
- x << 0,0,25, 0,0,0,1, 0,0,10;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, check_Covariance)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
-
- ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
-// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
-}
-
-TEST_F(ProcessorIMUt, gyro_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
-{
- // time
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
-
- // bias
- wolf::Scalar abx = 0.002;
- Vector6s bias; bias << abx,0,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
- // state
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
-
- // init things
- problem->setPrior(x0, P0, t, 0.01);
-
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
-// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
-
- // data
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x_true(10);
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- VectorXs x_est(10);
- x_est = problem->getCurrentState().head(10);
-
- ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- wolf::Scalar abx(0.002), aby(0.01);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector6s bias; bias << abx,aby,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
-
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
-// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
- data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
-// << "\n estimated state : " << x.transpose();
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
-// "\n expected state is : \n" << x.transpose() << std::endl;
-}
-
-TEST_F(ProcessorIMUt, gyro_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity and bias parts
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- //rotation + translation due to initial velocity
- x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_x_acc_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
- // v = a*dt = 0.001
- x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
- // v = a*dt = 1
- x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_acc_y)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
- // v = a*dt = 0.001
- x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
- // v = a*dt = 1
- x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_acc_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
- // v = a*dt = 0.001
- x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
- // v = a*dt = 1
- x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/utils_gtest.h b/test/utils_gtest.h
deleted file mode 100644
index 9cb2166e8429cd236f77c502891c14880e57ec30..0000000000000000000000000000000000000000
--- a/test/utils_gtest.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/**
- * \file utils_gtest.h
- * \brief Some utils for gtest
- * \author Jeremie Deray
- * Created on: 26/09/2016
- * Eigen macros extension by: Joan Sola on 26/04/2017
- */
-
-#ifndef WOLF_UTILS_GTEST_H
-#define WOLF_UTILS_GTEST_H
-
-#include <gtest/gtest.h>
-
-// Macros for testing equalities and inequalities.
-//
-// * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
-// * {ASSERT|EXPECT}_NE(v1, v2): Tests that v1 != v2
-// * {ASSERT|EXPECT}_LT(v1, v2): Tests that v1 < v2
-// * {ASSERT|EXPECT}_LE(v1, v2): Tests that v1 <= v2
-// * {ASSERT|EXPECT}_GT(v1, v2): Tests that v1 > v2
-// * {ASSERT|EXPECT}_GE(v1, v2): Tests that v1 >= v2
-//
-// C String Comparisons. All tests treat NULL and any non-NULL string
-// as different. Two NULLs are equal.
-//
-// * {ASSERT|EXPECT}_STREQ(s1, s2): Tests that s1 == s2
-// * {ASSERT|EXPECT}_STRNE(s1, s2): Tests that s1 != s2
-// * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case
-// * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case
-//
-// Macros for comparing floating-point numbers.
-//
-// * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual):
-// Tests that two float values are almost equal.
-// * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual):
-// Tests that two double values are almost equal.
-// * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error):
-// Tests that v1 and v2 are within the given distance to each other.
-//
-// These predicate format functions work on floating-point values, and
-// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g.
-//
-// EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0);
-//
-// Macros that execute statement and check that it doesn't generate new fatal
-// failures in the current thread.
-//
-// * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
-
-// http://stackoverflow.com/a/29155677
-
-namespace testing
-{
-namespace internal
-{
-enum GTestColor
-{
- COLOR_DEFAULT,
- COLOR_RED,
- COLOR_GREEN,
- COLOR_YELLOW
-};
-
-extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
-
-#define PRINTF(...) \
- do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\
- "[ ] "); \
- testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \
- while(0)
-
-#define PRINT_TEST_FINISHED \
-{ \
- const ::testing::TestInfo* const test_info = \
- ::testing::UnitTest::GetInstance()->current_test_info(); \
- PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\
- append(" of test ").append(test_info->name()).append(".\n").c_str()); \
-}
-
-// C++ stream interface
-class TestCout : public std::stringstream
-{
-public:
- ~TestCout()
- {
- PRINTF("%s\n", str().c_str());
- }
-};
-
-/* Usage :
-
-TEST(Test, Foo)
-{
- // the following works but prints default stream
- EXPECT_TRUE(false) << "Testing Stream.";
-
- // or you can play with AINSI color code
- EXPECT_TRUE(false) << "\033[1;31m" << "Testing Stream.";
-
- // or use the above defined macros
-
- PRINTF("Hello world");
-
- // or
-
- TEST_COUT << "Hello world";
-}
-
-*/
-#define TEST_COUT testing::internal::TestCout()
-
-} // namespace internal
-
-/* 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) { \
- 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) { \
- return (lhs - rhs).isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-
-#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)); \
- 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)); \
- return er.isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-} // namespace testing
-
-#endif /* WOLF_UTILS_GTEST_H */
diff --git a/test/yaml/params_processor_loop_closure_falko.yaml b/test/yaml/params_processor_loop_closure_falko.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a936e3029a9681df069492f873091f53385c097b
--- /dev/null
+++ b/test/yaml/params_processor_loop_closure_falko.yaml
@@ -0,0 +1,81 @@
+ config:
+ problem:
+ dim: 2
+ processor:
+ ProcessorLoopClosureFalko:
+ time_tolerance : 0.1
+ apply_loss_function : false
+ min_keypoints : 4
+ max_loops : 3
+ distance_between_ref_frm : 0.5
+ near_matches_th : 1
+ score_th : 5
+ th_kp_area : 0.66
+ th_kp_perimeter : 0.66
+ th_kp_maxd : 0.66
+ th_kp_eig : 0
+ th_scan_area : 0
+ th_scan_eig : 0
+ logging : false
+ logging_filepath : ~/Desktop
+
+# Parameters for matcher
+ falko:
+ matcher_distance_th : 100
+ use_descriptors : true
+ circularSectorNumber : 16
+ radialRingNumber : 8
+ matcher_ddesc_th : 20
+ keypoints_number_th : 5
+ enable_subbeam : false
+ nms_radius : 0.1
+ neigh_b : 0.1
+ b_ratio : 4
+ xRes : 0.15
+ yRes : 0.15
+ thetaRes : 0.1
+ xAbsMax : 3
+ yAbsMax : 3
+ thetaAbsMax : 3
+
+# from processor base
+ keyframe_vote:
+ time_tolerance : 0.1
+ voting_active : true
+
+# from processor odom ICP
+ max_error_threshold : 0.5
+ min_points_percent : 0.5
+# from CSM
+ icp:
+ verbose: false
+ max_iterations: 50
+ max_correspondence_dist: 1
+ use_corr_tricks: true
+ outliers_maxPerc: 0.9
+ use_point_to_line_distance: true
+ outliers_adaptive_order: 0.7
+ outliers_adaptive_mult: 1.5
+ do_compute_covariance: true
+ cov_factor: 1
+
+ max_angular_correction_deg: 4
+ max_linear_correction: 10
+ epsilon_xy: 1e-3
+ epsilon_theta: 1e-2
+ sigma: 0.2
+ restart: false
+ restart_threshold_mean_error: 0
+ restart_dt: 0
+ restart_dtheta: 0
+ clustering_threshold: 0
+ orientation_neighbourhood: 0
+ do_alpha_test: false
+ do_alpha_test_thresholdDeg: 0
+ do_visibility_test: false
+ outliers_remove_doubles: false
+ debug_verify_tricks: false
+ min_reading: 0.023
+ max_reading: 60
+ use_ml_weights: false
+ use_sigma_weights: false
\ No newline at end of file
diff --git a/test/yaml/processor_loop_closure_icp.yaml b/test/yaml/processor_loop_closure_icp.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c5125820901c97e44dc0066bdbe662bbec732eb7
--- /dev/null
+++ b/test/yaml/processor_loop_closure_icp.yaml
@@ -0,0 +1,23 @@
+# from processor base
+time_tolerance : 0.1
+voting_active : true
+
+# from processor tracker
+min_features_for_keyframe : 0
+max_new_features : 0
+
+# from processor loop closure ICP
+candidate_generation : random
+max_candidates : 2
+max_attempts : 10
+cov_factor : 1
+use_point_to_line_distance : true
+max_correspondence_dist : 2
+max_iterations : 3
+use_corr_tricks : true
+outliers_maxPerc : 5
+outliers_adaptive_order : 6
+outliers_adaptive_mult : 7
+max_reading : 100
+min_reading : 0
+do_compute_covariance : true
\ No newline at end of file
diff --git a/test/yaml/processor_odom_icp.yaml b/test/yaml/processor_odom_icp.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..601de1ad83f470e1a9999aeedd7521909c247f6f
--- /dev/null
+++ b/test/yaml/processor_odom_icp.yaml
@@ -0,0 +1,28 @@
+type : "ProcessorOdomIcp"
+
+# from processor base
+time_tolerance : 0.1
+voting_active : true
+
+# from processor tracker
+min_features_for_keyframe : 0
+max_new_features : 0
+
+# from processor odom ICP
+cov_factor : 1
+use_point_to_line_distance : true
+max_correspondence_dist : 2
+max_iterations : 3
+use_corr_tricks : true
+outliers_maxPerc : 5
+outliers_adaptive_order : 6
+outliers_adaptive_mult : 7
+max_reading : 100
+min_reading : 0
+do_compute_covariance : true
+vfk_min_dist : 0
+vfk_min_angle : 0
+vfk_min_time : 0
+vfk_min_error : 0
+vfk_max_points : 0
+initial_guess : "zero"
diff --git a/test/yaml/sensor_laser_2d.yaml b/test/yaml/sensor_laser_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2c742dc85412de2bc2a1e7a68ffc428a98f3c0b3
--- /dev/null
+++ b/test/yaml/sensor_laser_2d.yaml
@@ -0,0 +1,15 @@
+type : "SensorLaser2d"
+
+# from sensor base
+ # no params so far
+
+# from sensor laser 2d
+LaserScanParams:
+ angle_min : -1.57079994678
+ angle_max : 1.57079994678
+ angle_step : 0.00875097513199
+ scan_time : 0.0
+ range_min : 0.1
+ range_max : 50.0
+ range_std_dev : 0.01
+ angle_std_dev : 0.001
diff --git a/wolf_scripts/create_plugin.sh b/wolf_scripts/create_plugin.sh
new file mode 100755
index 0000000000000000000000000000000000000000..b1ca42f945f7f559a70e43377412faa8e6d4ec9c
--- /dev/null
+++ b/wolf_scripts/create_plugin.sh
@@ -0,0 +1,126 @@
+#!/bin/bash
+# $1 path to the root of the plugin
+# $2 name of the plugin
+# $3 files to be moved
+
+#Generate the necessary dirs
+# if [ ! -d $1 ];
+# then
+# mkdir $1
+# fi
+
+# if [ ! -d $1/include/$2 ];
+# then
+# # mkdir $1/include
+# mkdir $1/include/$2
+# fi
+# if [ ! -d $1/src ];
+# then
+# mkdir $1/src
+# fi
+root_dir=$(echo $1 | rev | cut -d '/' -f 2- | rev)
+if [ ! -d $root_dir/$2 ];
+then
+ cp -a ../Skeleton $root_dir
+ mv $root_dir/Skeleton $root_dir/$2
+ mv $root_dir/$2/include/skeleton $root_dir/$2/include/$2
+fi
+
+for folder in capture factor feature landmark processor sensor yaml ; do
+ if [ ! -d $1/include/$2/$folder ];
+ then
+ mkdir $1/include/$2/$folder
+ fi
+ if [ ! -d $1/src/$folder ];
+ then
+ mkdir $1/src/$folder
+ fi
+done
+for file in $(cat $3); do
+ head=$(echo $file | cut -d '/' -f 1)
+ if [ "$head" = "include" ];
+ then
+ folder=$(echo $file | cut -d '/' -f 3)
+ suffix=$(echo $file | cut -d '/' -f 4-)
+ line=$(ag "HDRS_"${folder^^} $1/CMakeLists.txt | cut -d ':' -f 1 | head -1)
+ line=$(($line + 1))
+ echo $line " && " $file " && " $folder " && " $suffix
+ if [ "$suffix" = "" ];
+ then
+ line=$(ag "HDRS_BASE" $1/CMakeLists.txt | cut -d ':' -f 1 | head -1)
+ line=$(($line + 1))
+ suffix=$folder
+ sed -i ""$line"i $head/$2/$suffix" $1/CMakeLists.txt
+ cp $file $1/$head/$2/$suffix
+ else
+ sed -i ""$line"i $head/$2/$folder/$suffix" $1/CMakeLists.txt
+ cp $file $1/$head/$2/$folder/$suffix
+ fi
+ elif [ "$head" = "src" ];
+ then
+ folder=$(echo $file | cut -d '/' -f 2)
+ suffix=$(echo $file | cut -d '/' -f 3-)
+ # ag "SRCS_"$folder $1/CMakeLists.txt
+ line=$(ag "SRCS_"${folder^^} $1/CMakeLists.txt | cut -d ':' -f 1 | head -1)
+ line=$(($line + 1))
+ echo $line " && " $file " && " $folder " && " $suffix
+ if [ "$suffix" = "" ];
+ then
+ line=$(ag "SRCS_BASE" $1/CMakeLists.txt | cut -d ':' -f 1 | head -1)
+ line=$(($line + 1))
+ suffix=$folder
+ sed -i ""$line"i $file" $1/CMakeLists.txt
+ cp $file $1/$head/$suffix
+ else
+ sed -i ""$line"i $file" $1/CMakeLists.txt
+ cp $file $1/$head/$folder/$suffix
+ fi
+ else
+ cp $file $1/$file
+ fi
+done
+for f in $(cat $3); do
+ hhead=$(echo $f | cut -d '/' -f 1)
+ if [ "$hhead" = "include" ];
+ then
+ ffolder=$(echo $f | cut -d '/' -f 3)
+ ssuffix=$(echo $f | cut -d '/' -f 4-)
+ inc=$ffolder/$ssuffix
+ else
+ continue
+ fi
+ for ff in $(cat $3); do
+ head=$(echo $ff | cut -d '/' -f 1)
+ if [ "$head" = "include" ];
+ then
+ folder=$(echo $ff | cut -d '/' -f 3)
+ suffix=$(echo $ff | cut -d '/' -f 4-)
+ if [ "$suffix" = "" ];
+ then
+ new_path=$1/$head/$2/$folder
+ sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path
+ else
+ new_path=$1/$head/$2/$folder/$suffix
+ sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path
+ fi
+ # sed -n -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@pg" $new_path
+ # sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path
+ elif [ "$head" = "src" ];
+ then
+ folder=$(echo $ff | cut -d '/' -f 2)
+ suffix=$(echo $ff | cut -d '/' -f 3-)
+ # sed -n -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@pg" $new_path
+ if [ "$suffix" = "" ];
+ then
+ new_path=$1/$head/$folder
+ sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path
+ else
+ new_path=$1/$head/$folder/$suffix
+ sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path
+ fi
+ else
+ # sed -n -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@pg" $new_path
+ sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $1/$ff
+ fi
+ done
+done
\ No newline at end of file