diff --git a/CPU_version/svo_msgs/CMakeLists.txt b/CPU_version/svo_msgs/CMakeLists.txt index 06bb680..6d1a371 100644 --- a/CPU_version/svo_msgs/CMakeLists.txt +++ b/CPU_version/svo_msgs/CMakeLists.txt @@ -8,14 +8,12 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs ) -# search for all msg files -FILE(GLOB messages_to_build RELATIVE "msg" - "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg") + # notify catkin to look at the previously found msg files add_message_files( FILES - ${messages_to_build} + Info.msg ) # build the header files from the msg files, and notify catkin about the dependencies diff --git a/CPU_version/svo_ros/.idea/svo_ros.iml b/CPU_version/svo_ros/.idea/svo_ros.iml deleted file mode 100644 index bc2cd87..0000000 --- a/CPU_version/svo_ros/.idea/svo_ros.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/CPU_version/svo_ros/.idea/workspace.xml b/CPU_version/svo_ros/.idea/workspace.xml deleted file mode 100644 index 09c1504..0000000 --- a/CPU_version/svo_ros/.idea/workspace.xml +++ /dev/null @@ -1,627 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1615974669888 - - - 1617193017745 - - - 1617640897291 - - - - - - - - - - - - \ No newline at end of file diff --git a/CPU_version/vio_svo/CMakeLists.txt b/CPU_version/vio_svo/CMakeLists.txt index b57bcfc..70101db 100644 --- a/CPU_version/vio_svo/CMakeLists.txt +++ b/CPU_version/vio_svo/CMakeLists.txt @@ -32,7 +32,6 @@ FIND_PACKAGE(catkin REQUIRED COMPONENTS cv_bridge tf svo - svo_msgs vikit_common vikit_ros ) @@ -44,10 +43,22 @@ FIND_PACKAGE(Sophus REQUIRED) FIND_PACKAGE(Boost REQUIRED COMPONENTS thread) FIND_PACKAGE(GTSAM REQUIRED) +# notify catkin to look at the previously found msg files +add_message_files( + FILES + Info.msg +) + +# build the header files from the msg files, and notify catkin about the dependencies +generate_messages( + DEPENDENCIES + geometry_msgs + sensor_msgs +) # Describe catkin Project catkin_package( DEPENDS Eigen OpenCV Sophus Boost - CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo svo_msgs vikit_common vikit_ros geometry_msgs + CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo vikit_common vikit_ros geometry_msgs INCLUDE_DIRS include LIBRARIES svo_visualizer ) diff --git a/CPU_version/vio_svo/msg/Info.msg b/CPU_version/vio_svo/msg/Info.msg new file mode 100644 index 0000000..b962e1b --- /dev/null +++ b/CPU_version/vio_svo/msg/Info.msg @@ -0,0 +1,6 @@ +Header header +float32 processing_time +int32[] keyframes +int32 num_matches +int32 tracking_quality +int32 stage \ No newline at end of file diff --git a/CPU_version/vio_svo/package.xml b/CPU_version/vio_svo/package.xml index bb5b0f2..c63372b 100644 --- a/CPU_version/vio_svo/package.xml +++ b/CPU_version/vio_svo/package.xml @@ -6,7 +6,7 @@ Monocular Visual Odometry - ROS Nodes Christian Forster - Christian Forster + Majid Geravand GPLv3 @@ -23,7 +23,6 @@ cv_bridge tf svo - svo_msgs vikit_common vikit_ros geometry_msgs @@ -38,7 +37,6 @@ cv_bridge tf svo - svo_msgs vikit_common vikit_ros geometry_msgs diff --git a/CPU_version/vio_svo/src/visualizer.cpp b/CPU_version/vio_svo/src/visualizer.cpp index 4d3a631..901f9ef 100644 --- a/CPU_version/vio_svo/src/visualizer.cpp +++ b/CPU_version/vio_svo/src/visualizer.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include @@ -37,7 +37,7 @@ Visualizer() : pub_frames_ = pnh_.advertise("keyframes", 10); pub_points_ = pnh_.advertise("points", 1000); pub_pose_ = pnh_.advertise("pose",10); - pub_info_ = pnh_.advertise("info", 10); + pub_info_ = pnh_.advertise("info", 10); } void Visualizer::publishMinimal( @@ -55,7 +55,7 @@ void Visualizer::publishMinimal( // publish info msg. if(pub_info_.getNumSubscribers() > 0) { - svo_msgs::Info msg_info; + vio_svo::Info msg_info; msg_info.header = header_msg; msg_info.processing_time = slam.lastProcessingTime(); msg_info.keyframes.reserve(slam.map().keyframes_.size()); diff --git a/GPU_version/README.md b/GPU_version/README.md new file mode 100755 index 0000000..00b6d87 --- /dev/null +++ b/GPU_version/README.md @@ -0,0 +1,45 @@ +SVO +=== + +This code implements a semi-direct monocular visual odometry pipeline which has been integrated with GTSAM to infused IMU as well. + + +Paper svo: http://rpg.ifi.uzh.ch/docs/ICRA14_Forster.pdf +Paper Svo+GTSAM: https://arxiv.org/abs/1512.02363 + + +#### Disclaimer + +SVO has been tested under ROS Groovy, Hydro and Indigo with Ubuntu 12.04, 13.04 and 14.04. This is research code, any fitness for a particular purpose is disclaimed. + + +#### Licence + +The source code is released under a GPLv3 licence. A closed-source professional edition is available for commercial purposes. In this case, please contact the authors for further info. + + +#### Citing + +If you use SVO in an academic context, please cite the following publication: + + @inproceedings{Forster2014ICRA, + author = {Forster, Christian and Pizzoli, Matia and Scaramuzza, Davide}, + title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry}, + booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, + year = {2014} + } + + +#### Documentation + +The API is documented here: http://uzh-rpg.github.io/rpg_svo/doc/ +The GTSAM documentation https://gtsam.org/tutorials/ + +#### Instructions + +See the Wiki for more instructions. https://github.com/uzh-rpg/rpg_svo/wiki and https://gtsam.org/get_started/ + +#### Contributing + +You are very welcome to contribute to SVO by opening a pull request via Github. +I try to follow the ROS C++ style guide http://wiki.ros.org/CppStyleGuide diff --git a/GPU_version/svo/.idea/.gitignore b/GPU_version/svo/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/GPU_version/svo/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/GPU_version/svo/.idea/misc.xml b/GPU_version/svo/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/GPU_version/svo/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/CPU_version/svo_ros/.idea/modules.xml b/GPU_version/svo/.idea/modules.xml similarity index 59% rename from CPU_version/svo_ros/.idea/modules.xml rename to GPU_version/svo/.idea/modules.xml index eb3cacf..4c8cf2b 100644 --- a/CPU_version/svo_ros/.idea/modules.xml +++ b/GPU_version/svo/.idea/modules.xml @@ -2,7 +2,7 @@ - + \ No newline at end of file diff --git a/CPU_version/svo_ros/.idea/vcs.xml b/GPU_version/svo/.idea/vcs.xml similarity index 100% rename from CPU_version/svo_ros/.idea/vcs.xml rename to GPU_version/svo/.idea/vcs.xml diff --git a/GPU_version/svo/CMakeLists.txt b/GPU_version/svo/CMakeLists.txt new file mode 100644 index 0000000..ed4b593 --- /dev/null +++ b/GPU_version/svo/CMakeLists.txt @@ -0,0 +1,132 @@ +################################################################################ +# user build settings + +SET(TRACE TRUE) +SET(HAVE_G2O FALSE) +SET(USE_ROS TRUE) # Set FALSE if you want to build this package without Catkin +SET(DEBUG_OUTPUT FALSE) # Only relevant if build without ROS + +################################################################################ + +SET(PROJECT_NAME gpu_svo) +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +SET(CMAKE_VERBOSE_MAKEFILE OFF) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") +set(CMAKE_C_STANDARD 14) +set(CMAKE_CXX_STANDARD 14) + +# Set definitions +IF(TRACE) + ADD_DEFINITIONS(-DSVO_TRACE) +ENDIF() +IF(HAVE_G2O) + ADD_DEFINITIONS(-DUSE_BUNDLE_ADJUSTMENT) +ENDIF() +IF(USE_ROS) + ADD_DEFINITIONS(-DSVO_USE_ROS) +ENDIF() +IF(DEBUG_OUTPUT) + ADD_DEFINITIONS(-DSVO_DEBUG_OUTPUT) +ENDIF() + +# Set build flags, set ARM_ARCHITECTURE environment variable on Odroid +# Set build flags. Set IS_ARM on odroid board as environment variable +SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") +IF(DEFINED ENV{ARM_ARCHITECTURE}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +ENDIF() +IF(CMAKE_COMPILER_IS_GNUCC) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +ENDIF() +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") + +# Add plain cmake packages +FIND_PACKAGE(OpenCV REQUIRED) +FIND_PACKAGE(Eigen REQUIRED) +FIND_PACKAGE(Sophus REQUIRED) +FIND_PACKAGE(fast REQUIRED) +FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system) +FIND_PACKAGE(GTSAM REQUIRED) + +set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}") +IF(USE_ROS) + FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules vikit_common vikit_ros) + catkin_package( + DEPENDS Eigen OpenCV Sophus Boost fast GTSAM + CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + ) +ELSE() + FIND_PACKAGE(vikit_common REQUIRED) + SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +ENDIF() + +# Include dirs +INCLUDE_DIRECTORIES( + include + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${fast_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} +) + +# Set link libraries +LIST(APPEND LINK_LIBS + ${OpenCV_LIBS} + ${Sophus_LIBRARIES} + ${Boost_LIBRARIES} + ${fast_LIBRARIES} +) +IF(USE_ROS) + INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS}) + LIST(APPEND LINK_LIBS ${catkin_LIBRARIES}) +ELSE() + INCLUDE_DIRECTORIES(${vikit_common_INCLUDE_DIRS}) + LIST(APPEND LINK_LIBS ${vikit_common_LIBRARIES}) +ENDIF() + +# Set sourcefiles +LIST(APPEND SOURCEFILES + src/frame_handler_mono.cpp + src/frame_handler_base.cpp + src/frame.cpp + src/point.cpp + src/map.cpp + src/pose_optimizer.cpp + src/initialization.cpp + src/matcher.cpp + src/reprojector.cpp + src/feature_alignment.cpp + src/feature_detection.cpp + src/depth_filter.cpp + src/config.cpp + src/sparse_img_align.cpp src/imu_integration.cpp) + +# Add g2o if available +IF(HAVE_G2O) + FIND_PACKAGE(G2O REQUIRED) + INCLUDE_DIRECTORIES( + ${G2O_INCLUDE_DIR} + /usr/include/suitesparse # for cholmod + ) + LIST(APPEND LINK_LIBS + ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE} + ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse + ) + LIST(APPEND SOURCEFILES src/bundle_adjustment.cpp) +ENDIF(HAVE_G2O) + +# Create svo library +ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCEFILES}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS} gtsam) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) diff --git a/GPU_version/svo/CMakeModules/FindEigen.cmake b/GPU_version/svo/CMakeModules/FindEigen.cmake new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/GPU_version/svo/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/GPU_version/svo/CMakeModules/FindG2O.cmake b/GPU_version/svo/CMakeModules/FindG2O.cmake new file mode 100644 index 0000000..655600f --- /dev/null +++ b/GPU_version/svo/CMakeModules/FindG2O.cmake @@ -0,0 +1,113 @@ +# Find the header files + +FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h + ${G2O_ROOT}/include + $ENV{G2O_ROOT}/include + $ENV{G2O_ROOT} + /usr/local/include + /usr/include + /opt/local/include + /sw/local/include + /sw/include + NO_DEFAULT_PATH + ) + +# Macro to unify finding both the debug and release versions of the +# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY +# macro. + +MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ${G2O_ROOT}/lib/Debug + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Debug + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ${G2O_ROOT}/lib/Release + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Release + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + IF(NOT ${MYLIBRARY}_DEBUG) + IF(MYLIBRARY) + SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) + ENDIF(MYLIBRARY) + ENDIF( NOT ${MYLIBRARY}_DEBUG) + +ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) + +# Find the core elements +FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) +FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) + +# Find the CLI library +FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) + +# Find the pluggable solvers +FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) +FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) +FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) +FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) +FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) +FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) + +# Find the predefined types +FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) +FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) +FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) +FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) + +# G2O solvers declared found if we found at least one solver +SET(G2O_SOLVERS_FOUND "NO") +IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + SET(G2O_SOLVERS_FOUND "YES") +ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + +# G2O itself declared found if we found the core libraries and at least one solver +SET(G2O_FOUND "NO") +IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) + SET(G2O_FOUND "YES") +ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) diff --git a/GPU_version/svo/Doxyfile b/GPU_version/svo/Doxyfile new file mode 100644 index 0000000..b1b4256 --- /dev/null +++ b/GPU_version/svo/Doxyfile @@ -0,0 +1,1782 @@ +# Doxyfile 1.7.6.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" "). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or sequence of words) that should +# identify the project. Note that if you do not use Doxywizard you need +# to put quotes around the project name if it contains spaces. + +PROJECT_NAME = "svo" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer +# a quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = "Semi-Direct Visual Odometry" + +# With the PROJECT_LOGO tag one can specify an logo or icon that is +# included in the documentation. The maximum height of the logo should not +# exceed 55 pixels and the maximum width should not exceed 200 pixels. +# Doxygen will copy the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = "doc" + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful if your file system +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = YES + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 8 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding +# "class=itcl::class" will allow you to use the command class in the +# itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given extension. +# Doxygen has a built-in mapping, but you can override or extend it using this +# tag. The format is ext=language, where ext is a file extension, and language +# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, +# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions +# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also makes the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and +# unions are shown inside the group in which they are included (e.g. using +# @ingroup) instead of on a separate page (for HTML and Man pages) or +# section (for LaTeX and RTF). + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and +# unions with only public data fields will be shown inline in the documentation +# of the scope in which they are defined (i.e. file, namespace, or group +# documentation), provided this scope is documented. If set to NO (the default), +# structs, classes, and unions are shown on a separate page (for HTML and Man +# pages) or section (for LaTeX and RTF). + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penalty. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will roughly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +SYMBOL_CACHE_SIZE = 0 + +# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be +# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given +# their name and scope. Since this can be an expensive process and often the +# same symbol appear multiple times in the code, doxygen keeps a cache of +# pre-resolved symbols. If the cache is too small doxygen will become slower. +# If the cache is too large, memory is wasted. The cache size is given by this +# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespaces are hidden. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to +# do proper type resolution of all parameters of a function it will reject a +# match between the prototype and the implementation of a member function even +# if there is only one candidate or it is obvious which candidate to choose +# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen +# will still accept a match between prototype and implementation in such cases. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or macro consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and macros in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. +# This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. The create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files +# containing the references data. This must be a list of .bib files. The +# .bib extension is automatically appended if omitted. Using this command +# requires the bibtex tool to be installed. See also +# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style +# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this +# feature you need bibtex and perl available in the search path. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# The WARN_NO_PARAMDOC option can be enabled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = "src", "include/svo" + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh +# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py +# *.f90 *.f *.for *.vhd *.vhdl + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = g2o +EXCLUDE_SYMBOLS += vk + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. +# If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. +# Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. +# The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty or if +# non of the patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) +# and it is also possible to disable source filtering for a specific pattern +# using *.ext= (so without naming a filter). This option only has effect when +# FILTER_SOURCE_FILES is enabled. + +FILTER_SOURCE_PATTERNS = + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. +# Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. Note that when using a custom header you are responsible +# for the proper inclusion of any scripts and style sheets that doxygen +# needs, which is dependent on the configuration options used. +# It is advised to generate a default header using "doxygen -w html +# header.html footer.html stylesheet.css YourConfigFile" and then modify +# that header. Note that the header is subject to change so you typically +# have to redo this when upgrading to a newer version of doxygen or when +# changing the value of configuration settings such as GENERATE_TREEVIEW! + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# style sheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that +# the files will be copied as-is; there are no commands or markers available. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the style sheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = NO + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +# +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) +# at top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. Since the tabs have the same information as the +# navigation tree you can set this option to NO if you already set +# GENERATE_TREEVIEW to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. +# Since the tree basically has the same information as the tab index you +# could consider to set DISABLE_INDEX to NO when enabling this option. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values +# (range [0,1..20]) that doxygen will group on one line in the generated HTML +# documentation. Note that a value of 0 will completely suppress the enum +# values from appearing in the overview section. + +ENUM_VALUES_PER_LINE = 4 + +# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, +# and Class Hierarchy pages using a tree view instead of an ordered list. + +USE_INLINE_TREES = NO + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax +# (see http://www.mathjax.org) which uses client side Javascript for the +# rendering instead of using prerendered bitmaps. Use this if you do not +# have LaTeX installed or if you want to formulas look prettier in the HTML +# output. When enabled you also need to install MathJax separately and +# configure the path to it using the MATHJAX_RELPATH option. + +USE_MATHJAX = NO + +# When MathJax is enabled you need to specify the location relative to the +# HTML output directory using the MATHJAX_RELPATH option. The destination +# directory should contain the MathJax.js script. For instance, if the mathjax +# directory is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the +# mathjax.org site, so you can quickly see the result without installing +# MathJax, but it is strongly recommended to install a local copy of MathJax +# before deployment. + +MATHJAX_RELPATH = http://www.mathjax.org/mathjax + +# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension +# names that should be enabled during MathJax rendering. + +MATHJAX_EXTENSIONS = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a PHP enabled web server instead of at the web client +# using Javascript. Doxygen will generate the search PHP script and index +# file to put on the web server. The advantage of the server +# based approach is that it scales better to large projects and allows +# full text search. The disadvantages are that it is more difficult to setup +# and does not have live searching capabilities. + +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = YES + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4 + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for +# the generated latex document. The footer should contain everything after +# the last chapter. If it is left blank doxygen will generate a +# standard footer. Notice: only use this tag if you know what you are doing! + +LATEX_FOOTER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See +# http://en.wikipedia.org/wiki/BibTeX for more info. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load style sheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. +# This is useful +# if you want to understand what is going on. +# On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# pointed to by INCLUDE_PATH will be searched when a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition that +# overrules the definition found in the source code. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all references to function-like macros +# that are alone on a line, have an all uppercase name, and do not end with a +# semicolon, because these will confuse the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option also works with HAVE_DOT disabled, but it is recommended to +# install and use dot, since it yields more powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will use the Helvetica font for all dot files that +# doxygen generates. When you want a differently looking font you can specify +# the font name using DOT_FONTNAME. You need to make sure dot is able to find +# the font, which can be done by putting it in a standard location or by setting +# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. + +DOT_FONTNAME = Helvetica + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the Helvetica font. +# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to +# set the path where dot can find it. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will generate a graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are svg, png, jpg, or gif. +# If left blank png will be used. If you choose svg you need to set +# HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible in IE 9+ (other browsers do not have this requirement). + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# Note that this requires a modern browser other than Internet Explorer. +# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you +# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible. Older versions of IE do not have SVG support. + +INTERACTIVE_SVG = NO + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the +# \mscfile command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = YES + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES diff --git a/GPU_version/svo/doc/.gitignore b/GPU_version/svo/doc/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/GPU_version/svo/doc/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/GPU_version/svo/doc/notation.ipe b/GPU_version/svo/doc/notation.ipe new file mode 100644 index 0000000..f8c1efe --- /dev/null +++ b/GPU_version/svo/doc/notation.ipe @@ -0,0 +1,312 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +$_w\mathtt{p}$ +$\overrightarrow{\mathcal{F}}_w$ + +64 576 m +96 576 l + + +64 576 m +64 608 l + + +64 576 m +80 592 l + + +144 688 m +176 688 l + + +144 688 m +144 720 l + + +144 688 m +160 704 l + + +285.262 759.615 m +162.017 735.456 +129.232 628.801 s + + +$\mathtt{T}_{f,w}$ + +128.441 626.745 m +320.625 647.224 l + + +331.681 587.399 m +331.681 587.399 l + +$x$ +$z$ +$y$ + +256 736 m +224 704 l +320 704 l +352 736 l +h + + +285.262 759.265 m +322.176 651.119 l + +$\mathtt{px}$ + +285.612 758.915 m +315.723 669.632 l + +$\mathtt{f}$ +$\overrightarrow{\mathcal{F}}_f$ + +255.668 719.918 m +274.728 719.957 l + + +256.107 720.028 m +246.655 710.527 l + +$v$ +$u$ + +$\mathbf{p}$ +$_f\mathtt{p}$ + + diff --git a/GPU_version/svo/doc/notation.pdf b/GPU_version/svo/doc/notation.pdf new file mode 100644 index 0000000..3bc3fcd Binary files /dev/null and b/GPU_version/svo/doc/notation.pdf differ diff --git a/GPU_version/svo/doc/notation.png b/GPU_version/svo/doc/notation.png new file mode 100644 index 0000000..d613b70 Binary files /dev/null and b/GPU_version/svo/doc/notation.png differ diff --git a/GPU_version/svo/include/svo/bundle_adjustment.h b/GPU_version/svo/include/svo/bundle_adjustment.h new file mode 100644 index 0000000..f3c3501 --- /dev/null +++ b/GPU_version/svo/include/svo/bundle_adjustment.h @@ -0,0 +1,109 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_BUNDLE_ADJUSTMENT_H_ +#define SVO_BUNDLE_ADJUSTMENT_H_ + +#include + +namespace g2o { +class EdgeProjectXYZ2UV; +class SparseOptimizer; +class VertexSE3Expmap; +class VertexSBAPointXYZ; +} + +namespace svo { + +typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3; +typedef g2o::VertexSE3Expmap g2oFrameSE3; +typedef g2o::VertexSBAPointXYZ g2oPoint; + +class Frame; +class Point; +class Feature; +class Map; + +/// Local, global and 2-view bundle adjustment with g2o +namespace ba { + +/// Temporary container to hold the g2o edge with reference to frame and point. +struct EdgeContainerSE3 +{ + g2oEdgeSE3* edge; + Frame* frame; + Feature* feature; + bool is_deleted; + EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) : + edge(e), frame(frame), feature(feature), is_deleted(false) + {} +}; + +/// Optimize two camera frames and their observed 3D points. +/// Is used after initialization. +void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map); + +/// Local bundle adjustment. +/// Optimizes core_kfs and their observed map points while keeping the +/// neighbourhood fixed. +void localBA( + Frame* center_kf, + set* core_kfs, + Map* map, + size_t& n_incorrect_edges_1, + size_t& n_incorrect_edges_2, + double& init_error, + double& final_error); + +/// Global bundle adjustment. +/// Optimizes the whole map. Is currently not used in SVO. +void globalBA(Map* map); + +/// Initialize g2o with solver type, optimization strategy and camera model. +void setupG2o(g2o::SparseOptimizer * optimizer); + +/// Run the optimization on the provided graph. +void runSparseBAOptimizer( + g2o::SparseOptimizer* optimizer, + unsigned int num_iter, + double& init_error, + double& final_error); + +/// Create a g2o vertice from a keyframe object. +g2oFrameSE3* createG2oFrameSE3( + Frame* kf, + size_t id, + bool fixed); + +/// Creates a g2o vertice from a mappoint object. +g2oPoint* createG2oPoint( + Vector3d pos, + size_t id, + bool fixed); + +/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. +g2oEdgeSE3* createG2oEdgeSE3( + g2oFrameSE3* v_kf, + g2oPoint* v_mp, + const Vector2d& f_up, + bool robust_kernel, + double huber_width, + double weight = 1); + +} // namespace ba +} // namespace svo + +#endif // SVO_BUNDLE_ADJUSTMENT_H_ diff --git a/GPU_version/svo/include/svo/config.h b/GPU_version/svo/include/svo/config.h new file mode 100644 index 0000000..e1c97b1 --- /dev/null +++ b/GPU_version/svo/include/svo/config.h @@ -0,0 +1,177 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_CONFIG_H_ +#define SVO_CONFIG_H_ + +#include +#include +#include + +namespace svo { + +using std::string; + +/// Global configuration file of SVO. +/// Implements the Singleton design pattern to allow global access and to ensure +/// that only one instance exists. +class Config +{ +public: + static Config& getInstance(); + + /// Base-name of the tracefiles. + static string& traceName() { return getInstance().trace_name; } + + /// Directory where the tracefiles are saved. + static string& traceDir() { return getInstance().trace_dir; } + + /// Number of pyramid levels used for features. + static size_t& nPyrLevels() { return getInstance().n_pyr_levels; } + + /// Use the IMU to get relative rotations. + static bool& useImu() { return getInstance().use_imu; } + + /// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment. + static size_t& coreNKfs() { return getInstance().core_n_kfs; } + + /// Initial scale of the map. Depends on the distance the camera is moved for the initialization. + static double& mapScale() { return getInstance().map_scale; } + + /// Feature grid size of a cell in [px]. + static size_t& gridSize() { return getInstance().grid_size; } + + /// Initialization: Minimum required disparity between the first two frames. + static double& initMinDisparity() { return getInstance().init_min_disparity; } + + /// Initialization: Minimum number of tracked features. + static size_t& initMinTracked() { return getInstance().init_min_tracked; } + + /// Initialization: Minimum number of inliers after RANSAC. + static size_t& initMinInliers() { return getInstance().init_min_inliers; } + + /// Maximum level of the Lucas Kanade tracker. + static size_t& kltMaxLevel() { return getInstance().klt_max_level; } + + /// Minimum level of the Lucas Kanade tracker. + static size_t& kltMinLevel() { return getInstance().klt_min_level; } + + /// Reprojection threshold [px]. + static double& reprojThresh() { return getInstance().reproj_thresh; } + + /// Reprojection threshold after pose optimization. + static double& poseOptimThresh() { return getInstance().poseoptim_thresh; } + + /// Number of iterations in local bundle adjustment. + static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; } + + /// Maximum number of points to optimize at every iteration. + static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; } + + /// Number of iterations in structure optimization. + static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; } + + /// Reprojection threshold after bundle adjustment. + static double& lobaThresh() { return getInstance().loba_thresh; } + + /// Threshold for the robust Huber kernel of the local bundle adjustment. + static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; } + + /// Number of iterations in the local bundle adjustment. + static size_t& lobaNumIter() { return getInstance().loba_num_iter; } + + /// Minimum distance between two keyframes. Relative to the average height in the map. + static double& kfSelectMinDist() { return getInstance().kfselect_mindist; } + + /// Select only features with a minimum Harris corner score for triangulation. + static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; } + + /// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required! + static size_t& subpixNIter() { return getInstance().subpix_n_iter; } + + /// Limit the number of keyframes in the map. This makes nslam essentially. + /// a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. + /// Minimum number of keyframes is 3. + static size_t& maxNKfs() { return getInstance().max_n_kfs; } + + /// How much (in milliseconds) is the camera delayed with respect to the imu. + static double& imgImuDelay() { return getInstance().img_imu_delay; } + + /// Maximum number of features that should be tracked. + static size_t& maxFts() { return getInstance().max_fts; } + + /// If the number of tracked features drops below this threshold. Tracking quality is bad. + static size_t& qualityMinFts() { return getInstance().quality_min_fts; } + + /// If within one frame, this amount of features are dropped. Tracking quality is bad. + static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; } + + /// acc white noise in continuous. + static double& ACC_Noise() { return getInstance().ACC_noise; } + + /// acc white noise in continuous. + static double& GYO_Noise() { return getInstance().GYO_noise; } + /// integration uncertainty continuous + static double& IUC() { return getInstance().iuc; } + // acc bias in continuous + static double& ABC() { return getInstance().abc; } + // gyro bias in continuous + static double& GBC() { return getInstance().gbc; } + // error in the bias used for preintegration + static double& EBP() { return getInstance().ebp; } + // the noise used for projection + static double& NRP() { return getInstance().nrp; } + + +private: + Config(); + Config(Config const&); + void operator=(Config const&); + string trace_name; + string trace_dir; + size_t n_pyr_levels; + bool use_imu; + size_t core_n_kfs; + double map_scale; + size_t grid_size; + double init_min_disparity; + size_t init_min_tracked; + size_t init_min_inliers; + size_t klt_max_level; + size_t klt_min_level; + double reproj_thresh; + double poseoptim_thresh; + size_t poseoptim_num_iter; + size_t structureoptim_max_pts; + size_t structureoptim_num_iter; + double loba_thresh; + double loba_robust_huber_width; + size_t loba_num_iter; + double kfselect_mindist; + double triang_min_corner_score; + size_t triang_half_patch_size; + size_t subpix_n_iter; + size_t max_n_kfs; + double img_imu_delay; + size_t max_fts; + size_t quality_min_fts; + int quality_max_drop_fts; + double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise; +}; + +} // namespace svo + +#endif // SVO_CONFIG_H_ diff --git a/GPU_version/svo/include/svo/depth_filter.h b/GPU_version/svo/include/svo/depth_filter.h new file mode 100644 index 0000000..1612d0e --- /dev/null +++ b/GPU_version/svo/include/svo/depth_filter.h @@ -0,0 +1,171 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_DEPTH_FILTER_H_ +#define SVO_DEPTH_FILTER_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +class Frame; +class Feature; +class Point; + +/// A seed is a probabilistic depth estimate for a single pixel. +struct Seed +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int batch_counter; + static int seed_counter; + int batch_id; //!< Batch id is the id of the keyframe for which the seed was created. + int id; //!< Seed ID, only used for visualization. + Feature* ftr; //!< Feature in the keyframe for which the depth should be computed. + float a; //!< a of Beta distribution: When high, probability of inlier is large. + float b; //!< b of Beta distribution: When high, probability of outlier is large. + float mu; //!< Mean of normal distribution. + float z_range; //!< Max range of the possible depth. + float sigma2; //!< Variance of normal distribution. + Matrix2d patch_cov; //!< Patch covariance in reference image. + Seed(Feature* ftr, float depth_mean, float depth_min); +}; + +/// Depth filter implements the Bayesian Update proposed in: +/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández. +/// In Image and Vision Computing, 29(7):434-441, 2011. +/// +/// The class uses a callback mechanism such that it can be used also by other +/// algorithms than nslam and for simplified testing. +class DepthFilter +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef boost::unique_lock lock_t; + typedef boost::function callback_t; + + /// Depth-filter config parameters + struct Options + { + bool check_ftr_angle; //!< gradient features are only updated if the epipolar line is orthogonal to the gradient. + bool epi_search_1d; //!< restrict Gauss Newton in the epipolar search to the epipolar line. + bool verbose; //!< display output. + bool use_photometric_disparity_error; //!< use photometric disparity error instead of 1px error in tau computation. + int max_n_kfs; //!< maximum number of keyframes for which we maintain seeds. + double sigma_i_sq; //!< image noise. + double seed_convergence_sigma2_thresh; //!< threshold on depth uncertainty for convergence. + Options() + : check_ftr_angle(false), + epi_search_1d(false), + verbose(false), + use_photometric_disparity_error(false), + max_n_kfs(3), + sigma_i_sq(5e-4), + seed_convergence_sigma2_thresh(200.0) + {} + } options_; + + DepthFilter( + feature_detection::DetectorPtr feature_detector, + callback_t seed_converged_cb); + + virtual ~DepthFilter(); + + /// Start this thread when seed updating should be in a parallel thread. + void startThread(); + + /// Stop the parallel thread that is running. + void stopThread(); + + /// Add frame to the queue to be processed. + void addFrame(FramePtr frame); + + /// Add new keyframe to the queue + void addKeyframe(FramePtr frame, double depth_mean, double depth_min); + + /// Remove all seeds which are initialized from the specified keyframe. This + /// function is used to make sure that no seeds points to a non-existent frame + /// when a frame is removed from the map. + void removeKeyframe(FramePtr frame); + + /// If the map is reset, call this function such that we don't have pointers + /// to old frames. + void reset(); + + /// Returns a copy of the seeds belonging to frame. Thread-safe. + /// Can be used to compute the Next-Best-View in parallel. + /// IMPORTANT! Make sure you hold a valid reference counting pointer to frame + /// so it is not being deleted while you use it. + void getSeedsCopy(const FramePtr& frame, std::list& seeds); + + /// Return a reference to the seeds. This is NOT THREAD SAFE! + //std::list& getSeeds() { return seeds_; } Bug 238 + std::list>& getSeeds() { return seeds_; } + + /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty + static void updateSeed( + const float x, + const float tau2, + Seed* seed); + + /// Compute the uncertainty of the measurement. + static double computeTau( + const SE3& T_ref_cur, + const Vector3d& f, + const double z, + const double px_error_angle); + +protected: + feature_detection::DetectorPtr feature_detector_; + callback_t seed_converged_cb_; + //std::list seeds_; + std::list> seeds_; + boost::mutex seeds_mut_; + bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted. + boost::thread* thread_; + std::queue frame_queue_; + boost::mutex frame_queue_mut_; + boost::condition_variable frame_queue_cond_; + FramePtr new_keyframe_; //!< Next keyframe to extract new seeds. + bool new_keyframe_set_; //!< Do we have a new keyframe to process?. + double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. + double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. + vk::PerformanceMonitor permon_; //!< Separate performance monitor since the DepthFilter runs in a parallel thread. + Matcher matcher_; + + /// Initialize new seeds from a frame. + void initializeSeeds(FramePtr frame); + + /// Update all seeds with a new measurement frame. + virtual void updateSeeds(FramePtr frame); + + /// When a new keyframe arrives, the frame queue should be cleared. + void clearFrameQueue(); + + /// A thread that is continuously updating the seeds. + void updateSeedsLoop(); +}; + +} // namespace svo + +#endif // SVO_DEPTH_FILTER_H_ diff --git a/GPU_version/svo/include/svo/feature.h b/GPU_version/svo/include/svo/feature.h new file mode 100644 index 0000000..6f90a9a --- /dev/null +++ b/GPU_version/svo/include/svo/feature.h @@ -0,0 +1,75 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FEATURE_H_ +#define SVO_FEATURE_H_ + +#include + +namespace svo { + +/// A salient image region that is tracked across frames. +struct Feature +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum FeatureType { + CORNER, + EDGELET + }; + + FeatureType type; //!< Type can be corner or edgelet. + Frame* frame; //!< Pointer to frame in which the feature was detected. + Vector2d px; //!< Coordinates in pixels on pyramid level 0. + Vector3d f; //!< Unit-bearing vector of the feature. + int level; //!< Image pyramid level where feature was extracted. + Point* point; //!< Pointer to 3D point which corresponds to the feature. + Vector2d grad; //!< Dominant gradient direction for edglets, normalized. + + Feature(Frame* _frame, const Vector2d& _px, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(frame->cam_->cam2world(px)), + level(_level), + point(NULL), + grad(1.0,0.0) + {} + + Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(_f), + level(_level), + point(NULL), + grad(1.0,0.0) + {} + + Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(_f), + level(_level), + point(_point), + grad(1.0,0.0) + {} +}; + +} // namespace svo + +#endif // SVO_FEATURE_H_ diff --git a/GPU_version/svo/include/svo/feature_alignment.h b/GPU_version/svo/include/svo/feature_alignment.h new file mode 100644 index 0000000..12523e3 --- /dev/null +++ b/GPU_version/svo/include/svo/feature_alignment.h @@ -0,0 +1,63 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FEATURE_ALIGNMENT_H_ +#define SVO_FEATURE_ALIGNMENT_H_ + +#include + +namespace svo { + +/// Subpixel refinement of a reference feature patch with the current image. +/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on" +/// paper by Baker. +namespace feature_alignment { + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv); + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd = false); + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +bool align2D_NEON( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +} // namespace feature_alignment +} // namespace svo + +#endif // SVO_FEATURE_ALIGNMENT_H_ diff --git a/GPU_version/svo/include/svo/feature_detection.h b/GPU_version/svo/include/svo/feature_detection.h new file mode 100644 index 0000000..d0990a8 --- /dev/null +++ b/GPU_version/svo/include/svo/feature_detection.h @@ -0,0 +1,107 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FEATURE_DETECTION_H_ +#define SVO_FEATURE_DETECTION_H_ + +#include +#include + +namespace svo { + +/// Implementation of various feature detectors. +namespace feature_detection { + +/// Temporary container used for corner detection. Features are initialized from these. +struct Corner +{ + int x; //!< x-coordinate of corner in the image. + int y; //!< y-coordinate of corner in the image. + int level; //!< pyramid level of the corner. + float score; //!< shi-tomasi score of the corner. + float angle; //!< for gradient-features: dominant gradient angle. + Corner(int x, int y, float score, int level, float angle) : + x(x), y(y), level(level), score(score), angle(angle) + {} +}; +typedef vector Corners; + +/// All detectors should derive from this abstract class. +class AbstractDetector +{ +public: + AbstractDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels); + + virtual ~AbstractDetector() {}; + + virtual void detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts) = 0; + + /// Flag the grid cell as occupied + void setGridOccpuancy(const Vector2d& px); + + /// Set grid cells of existing features as occupied + void setExistingFeatures(const Features& fts); + +protected: + + static const int border_ = 8; //!< no feature should be within 8px of border. + const int cell_size_; + const int n_pyr_levels_; + const int grid_n_cols_; + const int grid_n_rows_; + vector grid_occupancy_; + + void resetGrid(); + + inline int getCellIndex(int x, int y, int level) + { + const int scale = (1< DetectorPtr; + +/// FAST detector by Edward Rosten. +class FastDetector : public AbstractDetector +{ +public: + FastDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels); + + virtual ~FastDetector() {} + + virtual void detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts); +}; + +} // namespace feature_detection +} // namespace svo + +#endif // SVO_FEATURE_DETECTION_H_ diff --git a/GPU_version/svo/include/svo/frame.h b/GPU_version/svo/include/svo/frame.h new file mode 100644 index 0000000..c926277 --- /dev/null +++ b/GPU_version/svo/include/svo/frame.h @@ -0,0 +1,154 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FRAME_H_ +#define SVO_FRAME_H_ + +#include +#include +#include +#include +#include + +namespace g2o { +class VertexSE3Expmap; +} +typedef g2o::VertexSE3Expmap g2oFrameSE3; + +namespace svo { + +class Point; +struct Feature; + +typedef list Features; +typedef vector ImgPyr; + +/// A frame saves the image, the associated features and the estimated pose. +class Frame : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. + int id_; //!< Unique id of the frame. + double timestamp_; //!< Timestamp of when the image was recorded. + vk::AbstractCamera* cam_; //!< Camera model. + Sophus::SE3 T_f_w_; //!< Transform (f)rame from (w)orld. + Matrix Cov_; //!< Covariance. + ImgPyr img_pyr_; //!< Image Pyramid. + Features fts_; //!< List of features in the image. + vector key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. + bool is_keyframe_; //!< Was this frames selected as keyframe? + g2oFrameSE3* v_kf_; //!< Temporary pointer to the g2o node object of the keyframe. + int last_published_ts_; //!< Timestamp of last publishing. + + Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); + ~Frame(); + + /// Initialize new frame and create image pyramid. + void initFrame(const cv::Mat& img); + + /// Select this frame as keyframe. + void setKeyframe(); + + /// Add a feature to the image + void addFeature(Feature* ftr); + + /// The KeyPoints are those five features which are closest to the 4 image corners + /// and to the center and which have a 3D point assigned. These points are used + /// to quickly check whether two frames have overlapping field of view. + void setKeyPoints(); + + /// Check if we can select five better key-points. + void checkKeyPoints(Feature* ftr); + + /// If a point is deleted, we must remove the corresponding key-point. + void removeKeyPoint(Feature* ftr); + + /// Return number of point observations. + inline size_t nObs() const { return fts_.size(); } + + /// Check if a point in (w)orld coordinate frame is visible in the image. + bool isVisible(const Vector3d& xyz_w) const; + + /// Full resolution image stored in the frame. + inline const cv::Mat& img() const { return img_pyr_[0]; } + + /// Was this frame selected as keyframe? + inline bool isKeyframe() const { return is_keyframe_; } + + /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). + inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); } + + /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). + inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); } + + /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). + inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); } + + /// Transforms point coordinates in world-frame (w) to camera-frams (f). + inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; } + + /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). + inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; } + + /// Projects Point from unit sphere (f) in camera pixels (c). + inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); } + + /// Return the pose of the frame in the (w)orld coordinate frame. + inline Vector3d pos() const { return T_f_w_.inverse().translation(); } + + /// Frame jacobian for projection of 3D point in (f)rame coordinate to + /// unit plane coordinates uv (focal length = 1). + inline static void jacobian_xyz2uv( + const Vector3d& xyz_in_f, + Matrix& J) + { + const double x = xyz_in_f[0]; + const double y = xyz_in_f[1]; + const double z_inv = 1./xyz_in_f[2]; + const double z_inv_2 = z_inv*z_inv; + + J(0,0) = -z_inv; // -1/z + J(0,1) = 0.0; // 0 + J(0,2) = x*z_inv_2; // x/z^2 + J(0,3) = y*J(0,2); // x*y/z^2 + J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2) + J(0,5) = y*z_inv; // y/z + + J(1,0) = 0.0; // 0 + J(1,1) = -z_inv; // -1/z + J(1,2) = y*z_inv_2; // y/z^2 + J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2 + J(1,4) = -J(0,3); // -x*y/z^2 + J(1,5) = -x*z_inv; // x/z + } +}; + + +/// Some helper functions for the frame object. +namespace frame_utils { + +/// Creates an image pyramid of half-sampled images. +void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr); + +/// Get the average depth of the features in the image. +bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min); + +} // namespace frame_utils +} // namespace svo + +#endif // SVO_FRAME_H_ diff --git a/GPU_version/svo/include/svo/frame_handler_base.h b/GPU_version/svo/include/svo/frame_handler_base.h new file mode 100644 index 0000000..9a78639 --- /dev/null +++ b/GPU_version/svo/include/svo/frame_handler_base.h @@ -0,0 +1,123 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FRAME_HANDLER_BASE_H_ +#define SVO_FRAME_HANDLER_BASE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk +{ +class AbstractCamera; +class PerformanceMonitor; +} + +namespace svo +{ +class Point; +class Matcher; +class DepthFilter; + +/// Base class for various VO pipelines. Manages the map and the state machine. +class FrameHandlerBase : boost::noncopyable +{ +public: + enum Stage { + STAGE_PAUSED, + STAGE_FIRST_FRAME, + STAGE_SECOND_FRAME, + STAGE_DEFAULT_FRAME, + STAGE_RELOCALIZING + }; + enum TrackingQuality { + TRACKING_INSUFFICIENT, + TRACKING_BAD, + TRACKING_GOOD + }; + enum UpdateResult { + RESULT_NO_KEYFRAME, + RESULT_IS_KEYFRAME, + RESULT_FAILURE + }; + + FrameHandlerBase(); + + virtual ~FrameHandlerBase(); + + /// Get the current map. + const Map& map() const { return map_; } + + /// Will reset the map as soon as the current frame is finished processing. + void reset() { set_reset_ = true; } + + /// Start processing. + void start() { set_start_ = true; } + + /// Get the current stage of the algorithm. + Stage stage() const { return stage_; } + + /// Get tracking quality. + TrackingQuality trackingQuality() const { return tracking_quality_; } + + /// Get the processing time of the previous iteration. + double lastProcessingTime() const { return timer_.getTime(); } + + /// Get the number of feature observations of the last frame. + size_t lastNumObservations() const { return num_obs_last_; } + +protected: + Stage stage_; //!< Current stage of the algorithm. + bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration. + bool set_start_; //!< Flag the user can set to start the system when the next image is received. + Map map_; //!< Map of keyframes created by the slam system. + vk::Timer timer_; //!< Stopwatch to measure time to process frame. + vk::RingBuffer acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. + vk::RingBuffer acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance. + size_t num_obs_last_; //!< Number of observations in the previous frame. + TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features. + + /// Before a frame is processed, this function is called. + bool startFrameProcessingCommon(const double timestamp); + + /// When a frame is finished processing, this function is called. + int finishFrameProcessingCommon( + const size_t update_id, + const UpdateResult dropout, + const size_t num_observations); + + /// Reset the map and frame handler to start from scratch. + void resetCommon(); + + /// Reset the frame handler. Implement in derived class. + virtual void resetAll() { resetCommon(); } + + /// Set the tracking quality based on the number of tracked features. + virtual void setTrackingQuality(const size_t num_observations); + + /// Optimize some of the observed 3D points. + virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter); +}; + +} // namespace nslam + +#endif // SVO_FRAME_HANDLER_BASE_H_ diff --git a/GPU_version/svo/include/svo/frame_handler_mono.h b/GPU_version/svo/include/svo/frame_handler_mono.h new file mode 100644 index 0000000..58789c8 --- /dev/null +++ b/GPU_version/svo/include/svo/frame_handler_mono.h @@ -0,0 +1,104 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FRAME_HANDLER_H_ +#define SVO_FRAME_HANDLER_H_ + +#include +#include +#include +#include +#include +#include + +namespace svo { + +/// Monocular Visual Odometry Pipeline as described in the SVO paper. +class FrameHandlerMono : public FrameHandlerBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init); + virtual ~FrameHandlerMono(); + + /// Provide an image. + void addImage(const cv::Mat& img, double timestamp); + + /// Set the first frame (used for synthetic datasets in benchmark node) + void setFirstFrame(const FramePtr& first_frame); + + /// Get the last frame that has been processed. + FramePtr lastFrame() { return last_frame_; } + + /// Get the set of spatially closest keyframes of the last frame. + const set& coreKeyframes() { return core_kfs_; } + + /// Return the feature track to visualize the KLT tracking during initialization. + const vector& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } + const vector& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; } + + /// Access the depth filter. + DepthFilter* depthFilter() const { return depth_filter_; } + + /// An external place recognition module may know where to relocalize. + bool relocalizeFrameAtPose( + const int keyframe_id, + const SE3& T_kf_f, + const cv::Mat& img, + const double timestamp); + std::unique_ptr imu_integPtr_; //Integration of IMU + +protected: + vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). + Reprojector reprojector_; //!< Projects points from other keyframes into the current frame + FramePtr new_frame_; //!< Current frame. + FramePtr last_frame_; //!< Last frame, not necessarily a keyframe. + set core_kfs_; //!< Keyframes in the closer neighbourhood. + vector< pair > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!? + initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. + DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. + + + /// Initialize the visual odometry algorithm. + virtual void initialize(); + + /// Processes the first frame and sets it as a keyframe. + virtual UpdateResult processFirstFrame(); + + /// Processes all frames after the first frame until a keyframe is selected. + virtual UpdateResult processSecondFrame(); + + /// Processes all frames after the first two keyframes. + virtual UpdateResult processFrame(); + + /// Try relocalizing the frame at relative position to provided keyframe. + virtual UpdateResult relocalizeFrame( + const SE3& T_cur_ref, + FramePtr ref_keyframe); + + /// Reset the frame handler. Implement in derived class. + virtual void resetAll(); + + /// Keyframe selection criterion. + virtual bool needNewKf(double scene_depth_mean); + + void setCoreKfs(size_t n_closest); +}; + +} // namespace svo + +#endif // SVO_FRAME_HANDLER_H_ diff --git a/GPU_version/svo/include/svo/global.h b/GPU_version/svo/include/svo/global.h new file mode 100644 index 0000000..03aff4e --- /dev/null +++ b/GPU_version/svo/include/svo/global.h @@ -0,0 +1,101 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_GLOBAL_H_ +#define SVO_GLOBAL_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit +#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) +#endif + +#ifdef SVO_USE_ROS + #include + #define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x) + #define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x) + #define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x) + #define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x) + #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x) +#else + #define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "< // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc. + #define SVO_WARN_STREAM_THROTTLE(rate, x) \ + do { \ + static double __log_stream_throttle__last_hit__ = 0.0; \ + std::chrono::time_point __log_stream_throttle__now__ = \ + std::chrono::system_clock::now(); \ + if (__log_stream_throttle__last_hit__ + rate <= \ + std::chrono::duration_cast( \ + __log_stream_throttle__now__.time_since_epoch()).count()) { \ + __log_stream_throttle__last_hit__ = \ + std::chrono::duration_cast( \ + __log_stream_throttle__now__.time_since_epoch()).count(); \ + SVO_WARN_STREAM(x); \ + } \ + } while(0) +#endif + +namespace svo +{ + using namespace Eigen; + using namespace Sophus; + + const double EPS = 0.0000000001; + const double PI = 3.14159265; + +#ifdef SVO_TRACE + extern vk::PerformanceMonitor* g_permon; + #define SVO_LOG(value) g_permon->log(std::string((#value)),(value)) + #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2) + #define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3) + #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4) + #define SVO_START_TIMER(name) g_permon->startTimer((name)) + #define SVO_STOP_TIMER(name) g_permon->stopTimer((name)) +#else + #define SVO_LOG(v) + #define SVO_LOG2(v1, v2) + #define SVO_LOG3(v1, v2, v3) + #define SVO_LOG4(v1, v2, v3, v4) + #define SVO_START_TIMER(name) + #define SVO_STOP_TIMER(name) +#endif + + class Frame; + typedef boost::shared_ptr FramePtr; +} // namespace svo + +#endif // SVO_GLOBAL_H_ diff --git a/GPU_version/svo/include/svo/imu_integration.h b/GPU_version/svo/include/svo/imu_integration.h new file mode 100644 index 0000000..11b9055 --- /dev/null +++ b/GPU_version/svo/include/svo/imu_integration.h @@ -0,0 +1,121 @@ +// +// Created by gufe on 4/9/21. +// + +#ifndef SVO_IMU_INTEGRATION_H +#define SVO_IMU_INTEGRATION_H +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using gtsam::symbol_shorthand::P; // Pose3 (x,y,z,r,p,y) +using gtsam::symbol_shorthand::C; // Camera pose + +//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop +// Wrapper class +template +class enumerate_impl +{ +public: + // The return value of the operator* of the iterator, this + // is what you will get inside of the for loop + struct item + { + size_t index; + typename T::value_type & item; + }; + typedef item value_type; + + // Custom iterator with minimal interface + struct iterator + { + iterator(typename T::iterator _it, size_t counter=0) : + it(_it), counter(counter) + {} + + iterator operator++() + { + return iterator(++it, ++counter); + } + + bool operator!=(iterator other) + { + return it != other.it; + } + + typename T::iterator::value_type item() + { + return *it; + } + + value_type operator*() + { + return value_type{counter, *it}; + } + + size_t index() + { + return counter; + } + + private: + typename T::iterator it; + size_t counter; + }; + + enumerate_impl(T & t) : container(t) {} + + iterator begin() + { + return iterator(container.begin()); + } + + iterator end() + { + return iterator(container.end()); + } + +private: + T & container; +}; + +// A templated free function allows you to create the wrapper class +// conveniently +template +enumerate_impl enumerate(T & t) +{ + return enumerate_impl(t); +} + +class Imu_Integration{ +public: + Imu_Integration(Sophus::SE3& SE_init); + ~Imu_Integration(); + bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame); + bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame); + bool update(double* imu= nullptr); + bool predict(boost::shared_ptr&,std::size_t&,const double reproj_thresh); +private: + std::shared_ptr statePtr; + std::shared_ptr imu_biasPtr; + std::shared_ptr valuesPtr; + std::shared_ptr graphPtr; + boost::shared_ptr parameterPtr; + std::shared_ptr preintegratedPtr; + boost::shared_ptr ProjectNoisePtr; + std::chrono::steady_clock::time_point t_1; + std::uint32_t imu_factor_id=0; + uint imu_n=0; + std::shared_ptr optimizerParamPtr; + bool syn=false; +}; +#endif //SVO_IMU_INTEGRATION_H diff --git a/GPU_version/svo/include/svo/initialization.h b/GPU_version/svo/include/svo/initialization.h new file mode 100644 index 0000000..cb562e5 --- /dev/null +++ b/GPU_version/svo/include/svo/initialization.h @@ -0,0 +1,83 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_INITIALIZATION_H +#define SVO_INITIALIZATION_H + +#include + +namespace svo { + +class FrameHandlerMono; + +/// Bootstrapping the map from the first two views. +namespace initialization { + +enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS }; + +/// Tracks features using Lucas-Kanade tracker and then estimates a homography. +class KltHomographyInit { + friend class svo::FrameHandlerMono; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FramePtr frame_ref_; + KltHomographyInit() {}; + ~KltHomographyInit() {}; + InitResult addFirstFrame(FramePtr frame_ref); + InitResult addSecondFrame(FramePtr frame_ref); + void reset(); + +protected: + vector px_ref_; //!< keypoints to be tracked in reference frame. + vector px_cur_; //!< tracked keypoints in current frame. + vector f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image. + vector f_cur_; //!< bearing vectors corresponding to the keypoints in the current image. + vector disparities_; //!< disparity between first and second frame. + vector inliers_; //!< inliers after the geometric check (e.g., Homography). + vector xyz_in_cur_; //!< 3D points computed during the geometric check. + SE3 T_cur_from_ref_; //!< computed transformation between the first two frames. +}; + +/// Detect Fast corners in the image. +void detectFeatures( + FramePtr frame, + vector& px_vec, + vector& f_vec); + +/// Compute optical flow (Lucas Kanade) for selected keypoints. +void trackKlt( + FramePtr frame_ref, + FramePtr frame_cur, + vector& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities); + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE3& T_cur_from_ref); + +} // namespace initialization +} // namespace svo + +#endif // SVO_INITIALIZATION_H diff --git a/GPU_version/svo/include/svo/map.h b/GPU_version/svo/include/svo/map.h new file mode 100644 index 0000000..458622b --- /dev/null +++ b/GPU_version/svo/include/svo/map.h @@ -0,0 +1,136 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_MAP_H_ +#define SVO_MAP_H_ + +#include +#include +#include +#include + +namespace svo { + +class Point; +class Feature; +class Seed; + +/// Container for converged 3D points that are not already assigned to two keyframes. +class MapPointCandidates +{ +public: + typedef pair PointCandidate; + typedef list PointCandidateList; + + /// The depth-filter is running in a parallel thread and fills the canidate list. + /// This mutex controls concurrent access to point_candidates. + boost::mutex mut_; + + /// Candidate points are created from converged seeds. + /// Until the next keyframe, these points can be used for reprojection and pose optimization. + PointCandidateList candidates_; + list< Point* > trash_points_; + + MapPointCandidates(); + ~MapPointCandidates(); + + /// Add a candidate point. + void newCandidatePoint(Point* point, double depth_sigma2); + + /// Adds the feature to the frame and deletes candidate from list. + void addCandidatePointToFrame(FramePtr frame); + + /// Remove a candidate point from the list of candidates. + bool deleteCandidatePoint(Point* point); + + /// Remove all candidates that belong to a frame. + void removeFrameCandidates(FramePtr frame); + + /// Reset the candidate list, remove and delete all points. + void reset(); + + void deleteCandidate(PointCandidate& c); + + void emptyTrash(); +}; + +/// Map object which saves all keyframes which are in a map. +class Map : boost::noncopyable +{ +public: + list< FramePtr > keyframes_; //!< List of keyframes in the map. + list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also. + MapPointCandidates point_candidates_; + + Map(); + ~Map(); + + /// Reset the map. Delete all keyframes and reset the frame and point counters. + void reset(); + + /// Delete a point in the map and remove all references in keyframes to it. + void safeDeletePoint(Point* pt); + + /// Moves the point to the trash queue which is cleaned now and then. + void deletePoint(Point* pt); + + /// Moves the frame to the trash queue which is cleaned now and then. + bool safeDeleteFrame(FramePtr frame); + + /// Remove the references between a point and a frame. + void removePtFrameRef(Frame* frame, Feature* ftr); + + /// Add a new keyframe to the map. + void addKeyframe(FramePtr new_keyframe); + + /// Given a frame, return all keyframes which have an overlapping field of view. + void getCloseKeyframes(const FramePtr& frame, list< pair >& close_kfs) const; + + /// Return the keyframe which is spatially closest and has overlapping field of view. + FramePtr getClosestKeyframe(const FramePtr& frame) const; + + /// Return the keyframe which is furthest apart from pos. + FramePtr getFurthestKeyframe(const Vector3d& pos) const; + + bool getKeyframeById(const int id, FramePtr& frame) const; + + /// Transform the whole map with rotation R, translation t and scale s. + void transform(const Matrix3d& R, const Vector3d& t, const double& s); + + /// Empty trash bin of deleted keyframes and map points. We don't delete the + /// points immediately to ensure proper cleanup and to provide the visualizer + /// a list of objects which must be removed. + void emptyTrash(); + + /// Return the keyframe which was last inserted in the map. + inline FramePtr lastKeyframe() { return keyframes_.back(); } + + /// Return the number of keyframes in the map + inline size_t size() const { return keyframes_.size(); } +}; + +/// A collection of debug functions to check the data consistency. +namespace map_debug { + +void mapStatistics(Map* map); +void mapValidation(Map* map, int id); +void frameValidation(Frame* frame, int id); +void pointValidation(Point* point, int id); + +} // namespace map_debug +} // namespace svo + +#endif // SVO_MAP_H_ diff --git a/GPU_version/svo/include/svo/matcher.h b/GPU_version/svo/include/svo/matcher.h new file mode 100644 index 0000000..b53340e --- /dev/null +++ b/GPU_version/svo/include/svo/matcher.h @@ -0,0 +1,130 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_MATCHER_H_ +#define SVO_MATCHER_H_ + +#include + +namespace vk { + class AbstractCamera; + namespace patch_score { + template class ZMSSD; + } +} + +namespace svo { + +class Point; +class Frame; +class Feature; + +/// Warp a patch from the reference view to the current view. +namespace warp { + +void getWarpMatrixAffine( + const vk::AbstractCamera& cam_ref, + const vk::AbstractCamera& cam_cur, + const Vector2d& px_ref, + const Vector3d& f_ref, + const double depth_ref, + const SE3& T_cur_ref, + const int level_ref, + Matrix2d& A_cur_ref); + +int getBestSearchLevel( + const Matrix2d& A_cur_ref, + const int max_level); + +void warpAffine( + const Matrix2d& A_cur_ref, + const cv::Mat& img_ref, + const Vector2d& px_ref, + const int level_ref, + const int level_cur, + const int halfpatch_size, + uint8_t* patch); + +} // namespace warp + +/// Patch-matcher for reprojection-matching and epipolar search in triangulation. +class Matcher +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static const int halfpatch_size_ = 4; + static const int patch_size_ = 8; + + typedef vk::patch_score::ZMSSD PatchScore; + + struct Options + { + bool align_1d; //!< in epipolar search: align patch 1D along epipolar line + int align_max_iter; //!< number of iterations for aligning the feature patches in gauss newton + double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align + size_t max_epi_search_steps;//!< max number of evaluations along epipolar line + bool subpix_refinement; //!< do gauss newton feature patch alignment after epipolar search + bool epi_search_edgelet_filtering; + double epi_search_edgelet_max_angle; + Options() : + align_1d(false), + align_max_iter(10), + max_epi_length_optim(2.0), + max_epi_search_steps(1000), + subpix_refinement(true), + epi_search_edgelet_filtering(true), + epi_search_edgelet_max_angle(0.7) + {} + } options_; + + uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16))); + uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16))); + Matrix2d A_cur_ref_; //!< affine warp matrix + Vector2d epi_dir_; + double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search) + double h_inv_; //!< hessian of 1d image alignment along epipolar line + int search_level_; + bool reject_; + Feature* ref_ftr_; + Vector2d px_cur_; + + Matcher() = default; + ~Matcher() = default; + + /// Find a match by directly applying subpix refinement. + /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result! + bool findMatchDirect( + const Point& pt, + const Frame& frame, + Vector2d& px_cur); + + /// Find a match by searching along the epipolar line without using any features. + bool findEpipolarMatchDirect( + const Frame& ref_frame, + const Frame& cur_frame, + const Feature& ref_ftr, + const double d_estimate, + const double d_min, + const double d_max, + double& depth); + + void createPatchFromPatchWithBorder(); +}; + +} // namespace svo + +#endif // SVO_MATCHER_H_ diff --git a/GPU_version/svo/include/svo/point.h b/GPU_version/svo/include/svo/point.h new file mode 100644 index 0000000..e1bb81b --- /dev/null +++ b/GPU_version/svo/include/svo/point.h @@ -0,0 +1,108 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_POINT_H_ +#define SVO_POINT_H_ + +#include +#include + +namespace g2o { + class VertexSBAPointXYZ; +} +typedef g2o::VertexSBAPointXYZ g2oPoint; + +namespace svo { + +class Feature; + +typedef Matrix Matrix23d; + +/// A 3D point on the surface of the scene. +class Point : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum PointType { + TYPE_DELETED, + TYPE_CANDIDATE, + TYPE_UNKNOWN, + TYPE_GOOD + }; + + static int point_counter_; //!< Counts the number of created points. Used to set the unique id. + int id_; //!< Unique ID of the point. + Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. + Vector3d normal_; //!< Surface normal at point. + Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation. + bool normal_set_; //!< Flag whether the surface normal was estimated or not. + list obs_; //!< References to keyframes which observe the point. + size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. + g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. + int last_published_ts_; //!< Timestamp of last publishing. + int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice. + PointType type_; //!< Quality of the point. + int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. + int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point. + int last_structure_optim_; //!< Timestamp of last point optimization + + Point(const Vector3d& pos); + Point(const Vector3d& pos, Feature* ftr); + ~Point(); + + /// Add a reference to a frame. + void addFrameRef(Feature* ftr); + + /// Remove reference to a frame. + bool deleteFrameRef(Frame* frame); + + /// Initialize point normal. The inital estimate will point towards the frame. + void initNormal(); + + /// Check whether mappoint has reference to a frame. + Feature* findFrameRef(Frame* frame); + + /// Get Frame with similar viewpoint. + bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const; + + /// Get number of observations. + inline size_t nRefs() const { return obs_.size(); } + + /// Optimize point position through minimizing the reprojection error. + void optimize(const size_t n_iter); + + /// Jacobian of point projection on unit plane (focal length = 1) in frame (f). + inline static void jacobian_xyz2uv( + const Vector3d& p_in_f, + const Matrix3d& R_f_w, + Matrix23d& point_jac) + { + const double z_inv = 1.0/p_in_f[2]; + const double z_inv_sq = z_inv*z_inv; + point_jac(0, 0) = z_inv; + point_jac(0, 1) = 0.0; + point_jac(0, 2) = -p_in_f[0] * z_inv_sq; + point_jac(1, 0) = 0.0; + point_jac(1, 1) = z_inv; + point_jac(1, 2) = -p_in_f[1] * z_inv_sq; + point_jac = - point_jac * R_f_w; + } +}; + +} // namespace svo + +#endif // SVO_POINT_H_ diff --git a/GPU_version/svo/include/svo/pose_optimizer.h b/GPU_version/svo/include/svo/pose_optimizer.h new file mode 100644 index 0000000..ca048ae --- /dev/null +++ b/GPU_version/svo/include/svo/pose_optimizer.h @@ -0,0 +1,50 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_POSE_OPTIMIZER_H_ +#define SVO_POSE_OPTIMIZER_H_ + +#include + +namespace svo { + +using namespace Eigen; +using namespace Sophus; +using namespace std; + +typedef Matrix Matrix6d; +typedef Matrix Matrix26d; +typedef Matrix Vector6d; + +class Point; + +/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame. +namespace pose_optimizer { + +void optimizeGaussNewton( + const double reproj_thresh, + const size_t n_iter, + const bool verbose, + FramePtr& frame, + double& estimated_scale, + double& error_init, + double& error_final, + size_t& num_obs); + +} // namespace pose_optimizer +} // namespace svo + +#endif // SVO_POSE_OPTIMIZER_H_ diff --git a/GPU_version/svo/include/svo/reprojector.h b/GPU_version/svo/include/svo/reprojector.h new file mode 100644 index 0000000..e345f0d --- /dev/null +++ b/GPU_version/svo/include/svo/reprojector.h @@ -0,0 +1,101 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_REPROJECTION_H_ +#define SVO_REPROJECTION_H_ + +#include +#include + +namespace vk { +class AbstractCamera; +} + +namespace svo { + +class Map; +class Point; + +/// Project points from the map into the image and find the corresponding +/// feature (corner). We don't search a match for every point but only for one +/// point per cell. Thereby, we achieve a homogeneously distributed set of +/// matched features and at the same time we can save processing time by not +/// projecting all points. +class Reprojector +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Reprojector config parameters + struct Options { + size_t max_n_kfs; //!< max number of keyframes to reproject from + bool find_match_direct; + Options() + : max_n_kfs(10), + find_match_direct(true) + {} + } options_; + + size_t n_matches_; + size_t n_trials_; + + Reprojector(vk::AbstractCamera* cam, Map& map); + + ~Reprojector(); + + /// Project points from the map into the image. First finds keyframes with + /// overlapping field of view and projects only those map-points. + void reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs); + +private: + + /// A candidate is a point that projects into the image plane and for which we + /// will search a maching feature in the image. + struct Candidate { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Point* pt; //!< 3D point. + Vector2d px; //!< projected 2D pixel location. + Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {} + }; + typedef std::list Cell; + typedef std::vector CandidateGrid; + + /// The grid stores a set of candidate matches. For every grid cell we try to find one match. + struct Grid + { + CandidateGrid cells; + vector cell_order; + int cell_size; + int grid_n_cols; + int grid_n_rows; + }; + + Grid grid_; + Matcher matcher_; + Map& map_; + + static bool pointQualityComparator(Candidate& lhs, Candidate& rhs); + void initializeGrid(vk::AbstractCamera* cam); + void resetGrid(); + bool reprojectCell(Cell& cell, FramePtr frame); + bool reprojectPoint(FramePtr frame, Point* point); +}; + +} // namespace svo + +#endif // SVO_REPROJECTION_H_ diff --git a/GPU_version/svo/include/svo/sparse_img_align.h b/GPU_version/svo/include/svo/sparse_img_align.h new file mode 100644 index 0000000..1bedfed --- /dev/null +++ b/GPU_version/svo/include/svo/sparse_img_align.h @@ -0,0 +1,83 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_SPARSE_IMG_ALIGN_H_ +#define SVO_SPARSE_IMG_ALIGN_H_ + +#include +#include +#include + +namespace vk { +class AbstractCamera; +} + +namespace svo { + +class Feature; + +/// Optimize the pose of the frame by minimizing the photometric error of feature patches. +class SparseImgAlign : public vk::NLLSSolver<6, SE3> +{ + static const int patch_halfsize_ = 2; + static const int patch_size_ = 2*patch_halfsize_; + static const int patch_area_ = patch_size_*patch_size_; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + cv::Mat resimg_; + + SparseImgAlign( + int n_levels, + int min_level, + int n_iter, + Method method, + bool display, + bool verbose); + + size_t run( + FramePtr ref_frame, + FramePtr cur_frame); + + /// Return fisher information matrix, i.e. the Hessian of the log-likelihood + /// at the converged state. + Matrix getFisherInformation(); + +protected: + FramePtr ref_frame_; //!< reference frame, has depth for gradient pixels. + FramePtr cur_frame_; //!< only the image is known! + int level_; //!< current pyramid level on which the optimization runs. + bool display_; //!< display residual image. + int max_level_; //!< coarsest pyramid level for the alignment. + int min_level_; //!< finest pyramid level for the alignment. + + // cache: + Matrix jacobian_cache_; + bool have_ref_patch_cache_; + cv::Mat ref_patch_cache_; + std::vector visible_fts_; + + void precomputeReferencePatches(); + virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false); + virtual int solve(); + virtual void update (const ModelType& old_model, ModelType& new_model); + virtual void startIteration(); + virtual void finishIteration(); +}; + +} // namespace svo + +#endif // SVO_SPARSE_IMG_ALIGN_H_ diff --git a/GPU_version/svo/include/svo/svoVisionFactor.h b/GPU_version/svo/include/svo/svoVisionFactor.h new file mode 100644 index 0000000..cd00816 --- /dev/null +++ b/GPU_version/svo/include/svo/svoVisionFactor.h @@ -0,0 +1,70 @@ +// +// Created by root on 4/13/21. +// + +#ifndef SVO_SVOVISIONFACTOR_H +#define SVO_SVOVISIONFACTOR_H +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + + class VisionFactor: public gtsam::NoiseModelFactor1 { + + private: + // measurement information + gtsam::Point3 feature; /// body_P_sensor_; ///< TODO The pose of the sensor in the body frame + int level; ///< Image pyramid level where feature was extracted. + + public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param feature Unit-bearing vector of the feature. + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param poseKey is the index of the camera pose + * @param landmark 3D position of the landmark + * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param level Image pyramid level where feature was extracted. + */ + VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp, + gtsam::Key poseKey,boost::optional body_P_sensor = boost::none) : + gtsam::NoiseModelFactor1(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){} + virtual ~VisionFactor(){} + + /// error function + /// @param p the Estimated pose in Pose3 + /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer + gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional Hp = boost::none) const { + // note that use boost optional like a pointer + // only calculate jacobian matrix when non-null pointer exists + gtsam::Matrix26 J; + gtsam::Point3 point(p*landmark); + svo::Frame::jacobian_xyz2uv(point, J); + Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<( + gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); } + + }; + +} // namespace gtsamexamples +#endif //SVO_SVOVISIONFACTOR_H diff --git a/GPU_version/svo/package.xml b/GPU_version/svo/package.xml new file mode 100644 index 0000000..8aea0a7 --- /dev/null +++ b/GPU_version/svo/package.xml @@ -0,0 +1,27 @@ + + + gpu_svo + 0.1.0 + + Monocular Visual Odometry + + Christian Forster + Christian Forster + GPLv3 + + + catkin + + + cmake_modules + roscpp + roslib + vikit_common + vikit_ros + + + roscpp + roslib + vikit_common + vikit_ros + \ No newline at end of file diff --git a/GPU_version/svo/src/bundle_adjustment.cpp b/GPU_version/svo/src/bundle_adjustment.cpp new file mode 100644 index 0000000..eaba2b0 --- /dev/null +++ b/GPU_version/svo/src/bundle_adjustment.cpp @@ -0,0 +1,435 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SCHUR_TRICK 1 + +namespace svo { +namespace ba { + +void twoViewBA( + Frame* frame1, + Frame* frame2, + double reproj_thresh, + Map* map) +{ + // scale reprojection threshold in pixels to unit plane + reproj_thresh /= frame1->cam_->errorMultiplier2(); + + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + size_t v_id = 0; + + // New Keyframe Vertex 1: This Keyframe is set to fixed! + g2oFrameSE3* v_frame1 = createG2oFrameSE3(frame1, v_id++, true); + optimizer.addVertex(v_frame1); + + // New Keyframe Vertex 2 + g2oFrameSE3* v_frame2 = createG2oFrameSE3(frame2, v_id++, false); + optimizer.addVertex(v_frame2); + + // Create Point Vertices + for(Features::iterator it_ftr=frame1->fts_.begin(); it_ftr!=frame1->fts_.end(); ++it_ftr) + { + Point* pt = (*it_ftr)->point; + if(pt == NULL) + continue; + g2oPoint* v_pt = createG2oPoint(pt->pos_, v_id++, false); + optimizer.addVertex(v_pt); + pt->v_pt_ = v_pt; + g2oEdgeSE3* e = createG2oEdgeSE3(v_frame1, v_pt, vk::project2d((*it_ftr)->f), true, reproj_thresh*Config::lobaRobustHuberWidth()); + optimizer.addEdge(e); + edges.push_back(EdgeContainerSE3(e, frame1, *it_ftr)); // TODO feature now links to frame, so we can simplify edge container! + + // find at which index the second frame observes the point + Feature* ftr_frame2 = pt->findFrameRef(frame2); + e = createG2oEdgeSE3(v_frame2, v_pt, vk::project2d(ftr_frame2->f), true, reproj_thresh*Config::lobaRobustHuberWidth()); + optimizer.addEdge(e); + edges.push_back(EdgeContainerSE3(e, frame2, ftr_frame2)); + } + + // Optimization + double init_error, final_error; + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + printf("2-View BA: Error before/after = %f / %f\n", init_error, final_error); + + // Update Keyframe Positions + frame1->T_f_w_.rotation_matrix() = v_frame1->estimate().rotation().toRotationMatrix(); + frame1->T_f_w_.translation() = v_frame1->estimate().translation(); + frame2->T_f_w_.rotation_matrix() = v_frame2->estimate().rotation().toRotationMatrix(); + frame2->T_f_w_.translation() = v_frame2->estimate().translation(); + + // Update Mappoint Positions + for(Features::iterator it=frame1->fts_.begin(); it!=frame1->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + (*it)->point->pos_ = (*it)->point->v_pt_->estimate(); + (*it)->point->v_pt_ = NULL; + } + + // Find Mappoints with too large reprojection error + const double reproj_thresh_squared = reproj_thresh*reproj_thresh; + size_t n_incorrect_edges = 0; + for(list::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e) + if(it_e->edge->chi2() > reproj_thresh_squared) + { + if(it_e->feature->point != NULL) + { + map->safeDeletePoint(it_e->feature->point); + it_e->feature->point = NULL; + } + ++n_incorrect_edges; + } + + printf("2-View BA: Wrong edges = %zu\n", n_incorrect_edges); +} + +void localBA( + Frame* center_kf, + set* core_kfs, + Map* map, + size_t& n_incorrect_edges_1, + size_t& n_incorrect_edges_2, + double& init_error, + double& final_error) +{ + + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + set mps; + list neib_kfs; + size_t v_id = 0; + size_t n_mps = 0; + size_t n_fix_kfs = 0; + size_t n_var_kfs = 1; + size_t n_edges = 0; + n_incorrect_edges_1 = 0; + n_incorrect_edges_2 = 0; + + // Add all core keyframes + for(set::iterator it_kf = core_kfs->begin(); it_kf != core_kfs->end(); ++it_kf) + { + g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false); + (*it_kf)->v_kf_ = v_kf; + ++n_var_kfs; + assert(optimizer.addVertex(v_kf)); + + // all points that the core keyframes observe are also optimized: + for(Features::iterator it_pt=(*it_kf)->fts_.begin(); it_pt!=(*it_kf)->fts_.end(); ++it_pt) + if((*it_pt)->point != NULL) + mps.insert((*it_pt)->point); + } + + // Now go throug all the points and add a measurement. Add a fixed neighbour + // Keyframe if it is not in the set of core kfs + double reproj_thresh_2 = Config::lobaThresh() / center_kf->cam_->errorMultiplier2(); + double reproj_thresh_1 = Config::poseOptimThresh() / center_kf->cam_->errorMultiplier2(); + double reproj_thresh_1_squared = reproj_thresh_1*reproj_thresh_1; + for(set::iterator it_pt = mps.begin(); it_pt!=mps.end(); ++it_pt) + { + // Create point vertex + g2oPoint* v_pt = createG2oPoint((*it_pt)->pos_, v_id++, false); + (*it_pt)->v_pt_ = v_pt; + assert(optimizer.addVertex(v_pt)); + ++n_mps; + + // Add edges + list::iterator it_obs=(*it_pt)->obs_.begin(); + while(it_obs!=(*it_pt)->obs_.end()) + { + Vector2d error = vk::project2d((*it_obs)->f) - vk::project2d((*it_obs)->frame->w2f((*it_pt)->pos_)); + + if((*it_obs)->frame->v_kf_ == NULL) + { + // frame does not have a vertex yet -> it belongs to the neib kfs and + // is fixed. create one: + g2oFrameSE3* v_kf = createG2oFrameSE3((*it_obs)->frame, v_id++, true); + (*it_obs)->frame->v_kf_ = v_kf; + ++n_fix_kfs; + assert(optimizer.addVertex(v_kf)); + neib_kfs.push_back((*it_obs)->frame); + } + + // create edge + g2oEdgeSE3* e = createG2oEdgeSE3((*it_obs)->frame->v_kf_, v_pt, + vk::project2d((*it_obs)->f), + true, + reproj_thresh_2*Config::lobaRobustHuberWidth(), + 1.0 / (1<<(*it_obs)->level)); + assert(optimizer.addEdge(e)); + edges.push_back(EdgeContainerSE3(e, (*it_obs)->frame, *it_obs)); + ++n_edges; + ++it_obs; + } + } + + // structure only + g2o::StructureOnlySolver<3> structure_only_ba; + g2o::OptimizableGraph::VertexContainer points; + for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) + { + g2o::OptimizableGraph::Vertex* v = static_cast(it->second); + if (v->dimension() == 3 && v->edges().size() >= 2) + points.push_back(v); + } + structure_only_ba.calc(points, 10); + + // Optimization + if(Config::lobaNumIter() > 0) + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + + // Update Keyframes + for(set::iterator it = core_kfs->begin(); it != core_kfs->end(); ++it) + { + (*it)->T_f_w_ = SE3( (*it)->v_kf_->estimate().rotation(), + (*it)->v_kf_->estimate().translation()); + (*it)->v_kf_ = NULL; + } + + for(list::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it) + (*it)->v_kf_ = NULL; + + // Update Mappoints + for(set::iterator it = mps.begin(); it != mps.end(); ++it) + { + (*it)->pos_ = (*it)->v_pt_->estimate(); + (*it)->v_pt_ = NULL; + } + + // Remove Measurements with too large reprojection error + double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2; + for(list::iterator it = edges.begin(); it != edges.end(); ++it) + { + if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<feature_->level)) + { + map->removePtFrameRef(it->frame, it->feature); + ++n_incorrect_edges_2; + } + } + + // TODO: delete points and edges! + init_error = sqrt(init_error)*center_kf->cam_->errorMultiplier2(); + final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2(); +} + +void globalBA(Map* map) +{ + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + list< pair > incorrect_edges; + + // Go through all Keyframes + size_t v_id = 0; + double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2(); + double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2(); + reproj_thresh_1_squared *= reproj_thresh_1_squared; + for(list::iterator it_kf = map->keyframes_.begin(); + it_kf != map->keyframes_.end(); ++it_kf) + { + // New Keyframe Vertex + g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false); + (*it_kf)->v_kf_ = v_kf; + optimizer.addVertex(v_kf); + for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) + { + // for each keyframe add edges to all observed mapoints + Point* mp = (*it_ftr)->point; + if(mp == NULL) + continue; + g2oPoint* v_mp = mp->v_pt_; + if(v_mp == NULL) + { + // mappoint-vertex doesn't exist yet. create a new one: + v_mp = createG2oPoint(mp->pos_, v_id++, false); + mp->v_pt_ = v_mp; + optimizer.addVertex(v_mp); + } + + // Due to merging of mappoints it is possible that references in kfs suddenly + // have a very large reprojection error which may result in distorted results. + Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_)); + if(error.squaredNorm() > reproj_thresh_1_squared) + incorrect_edges.push_back(pair(*it_kf, *it_ftr)); + else + { + g2oEdgeSE3* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f), + true, + reproj_thresh_2*Config::lobaRobustHuberWidth()); + + edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr)); + optimizer.addEdge(e); + } + } + } + + // Optimization + double init_error=0.0, final_error=0.0; + if(Config::lobaNumIter() > 0) + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + + // Update Keyframe and MapPoint Positions + for(list::iterator it_kf = map->keyframes_.begin(); + it_kf != map->keyframes_.end(); ++it_kf) + { + (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(), + (*it_kf)->v_kf_->estimate().translation()); + (*it_kf)->v_kf_ = NULL; + for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) + { + Point* mp = (*it_ftr)->point; + if(mp == NULL) + continue; + if(mp->v_pt_ == NULL) + continue; // mp was updated before + mp->pos_ = mp->v_pt_->estimate(); + mp->v_pt_ = NULL; + } + } + + // Remove Measurements with too large reprojection error + for(list< pair >::iterator it=incorrect_edges.begin(); + it!=incorrect_edges.end(); ++it) + map->removePtFrameRef(it->first.get(), it->second); + + double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2; + for(list::iterator it = edges.begin(); it != edges.end(); ++it) + { + if(it->edge->chi2() > reproj_thresh_2_squared) + { + map->removePtFrameRef(it->frame, it->feature); + } + } +} + +void setupG2o(g2o::SparseOptimizer * optimizer) +{ + // TODO: What's happening with all this HEAP stuff? Memory Leak? + optimizer->setVerbose(false); + +#if SCHUR_TRICK + // solver + g2o::BlockSolver_6_3::LinearSolverType* linearSolver; + linearSolver = new g2o::LinearSolverCholmod(); + //linearSolver = new g2o::LinearSolverCSparse(); + + g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); +#else + g2o::BlockSolverX::LinearSolverType * linearSolver; + linearSolver = new g2o::LinearSolverCholmod(); + //linearSolver = new g2o::LinearSolverCSparse(); + g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); +#endif + + solver->setMaxTrialsAfterFailure(5); + optimizer->setAlgorithm(solver); + + // setup camera + g2o::CameraParameters * cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.); + cam_params->setId(0); + if (!optimizer->addParameter(cam_params)) { + assert(false); + } +} + +void +runSparseBAOptimizer(g2o::SparseOptimizer* optimizer, + unsigned int num_iter, + double& init_error, double& final_error) +{ + optimizer->initializeOptimization(); + optimizer->computeActiveErrors(); + init_error = optimizer->activeChi2(); + optimizer->optimize(num_iter); + final_error = optimizer->activeChi2(); +} + +g2oFrameSE3* +createG2oFrameSE3(Frame* frame, size_t id, bool fixed) +{ + g2oFrameSE3* v = new g2oFrameSE3(); + v->setId(id); + v->setFixed(fixed); + v->setEstimate(g2o::SE3Quat(frame->T_f_w_.unit_quaternion(), frame->T_f_w_.translation())); + return v; +} + +g2oPoint* +createG2oPoint(Vector3d pos, + size_t id, + bool fixed) +{ + g2oPoint* v = new g2oPoint(); + v->setId(id); +#if SCHUR_TRICK + v->setMarginalized(true); +#endif + v->setFixed(fixed); + v->setEstimate(pos); + return v; +} + +g2oEdgeSE3* +createG2oEdgeSE3( g2oFrameSE3* v_frame, + g2oPoint* v_point, + const Vector2d& f_up, + bool robust_kernel, + double huber_width, + double weight) +{ + g2oEdgeSE3* e = new g2oEdgeSE3(); + e->setVertex(0, dynamic_cast(v_point)); + e->setVertex(1, dynamic_cast(v_frame)); + e->setMeasurement(f_up); + e->information() = weight * Eigen::Matrix2d::Identity(2,2); + g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber(); // TODO: memory leak + rk->setDelta(huber_width); + e->setRobustKernel(rk); + e->setParameterId(0, 0); //old: e->setId(v_point->id()); + return e; +} + +} // namespace ba +} // namespace svo diff --git a/GPU_version/svo/src/config.cpp b/GPU_version/svo/src/config.cpp new file mode 100644 index 0000000..e8475fe --- /dev/null +++ b/GPU_version/svo/src/config.cpp @@ -0,0 +1,102 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifdef SVO_USE_ROS +#include +#endif +#include + +namespace svo { + +Config::Config() : +#ifdef SVO_USE_ROS + trace_name(vk::getParam("svo/trace_name", "svo")), + trace_dir(vk::getParam("svo/trace_dir", "/tmp")), + n_pyr_levels(vk::getParam("svo/n_pyr_levels", 3)), + use_imu(vk::getParam("svo/use_imu", false)), + core_n_kfs(vk::getParam("svo/core_n_kfs", 3)), + map_scale(vk::getParam("svo/map_scale", 1.0)), + grid_size(vk::getParam("svo/grid_size", 30)), + init_min_disparity(vk::getParam("svo/init_min_disparity", 50.0)), + init_min_tracked(vk::getParam("svo/init_min_tracked", 50)), + init_min_inliers(vk::getParam("svo/init_min_inliers", 40)), + klt_max_level(vk::getParam("svo/klt_max_level", 4)), + klt_min_level(vk::getParam("svo/klt_min_level", 2)), + reproj_thresh(vk::getParam("svo/reproj_thresh", 2.0)), + poseoptim_thresh(vk::getParam("svo/poseoptim_thresh", 2.0)), + poseoptim_num_iter(vk::getParam("svo/poseoptim_num_iter", 10)), + structureoptim_max_pts(vk::getParam("svo/structureoptim_max_pts", 20)), + structureoptim_num_iter(vk::getParam("svo/structureoptim_num_iter", 5)), + loba_thresh(vk::getParam("svo/loba_thresh", 2.0)), + loba_robust_huber_width(vk::getParam("svo/loba_robust_huber_width", 1.0)), + loba_num_iter(vk::getParam("svo/loba_num_iter", 0)), + kfselect_mindist(vk::getParam("svo/kfselect_mindist", 0.12)), + triang_min_corner_score(vk::getParam("svo/triang_min_corner_score", 20.0)), + triang_half_patch_size(vk::getParam("svo/triang_half_patch_size", 4)), + subpix_n_iter(vk::getParam("svo/subpix_n_iter", 10)), + max_n_kfs(vk::getParam("svo/max_n_kfs", 10)), + img_imu_delay(vk::getParam("svo/img_imu_delay", 0.0)), + max_fts(vk::getParam("svo/max_fts", 120)), + quality_min_fts(vk::getParam("svo/quality_min_fts", 50)), + quality_max_drop_fts(vk::getParam("svo/quality_max_drop_fts", 40)), + ACC_noise(vk::getParam("svo/ACC_noise", 1e-4)), + GYO_noise(vk::getParam("svo/GYO_noise", 1e-4)), + iuc(vk::getParam("svo/IUC", 1e-1)), + abc(vk::getParam("svo/ABC", 1e-1)), + gbc(vk::getParam("svo/GBC", 1e-1)), + ebp(vk::getParam("svo/EBP", 1e-2)), + nrp(vk::getParam("svo/NRP", 1e-5)) +#else + trace_name("svo"), + trace_dir("/tmp"), + n_pyr_levels(3), + use_imu(false), + core_n_kfs(3), + map_scale(1.0), + grid_size(25), + init_min_disparity(50.0), + init_min_tracked(50), + init_min_inliers(40), + klt_max_level(4), + klt_min_level(2), + reproj_thresh(2.0), + poseoptim_thresh(2.0), + poseoptim_num_iter(10), + structureoptim_max_pts(20), + structureoptim_num_iter(5), + loba_thresh(2.0), + loba_robust_huber_width(1.0), + loba_num_iter(0), + kfselect_mindist(0.12), + triang_min_corner_score(20.0), + triang_half_patch_size(4), + subpix_n_iter(10), + max_n_kfs(0), + img_imu_delay(0.0), + max_fts(120), + quality_min_fts(50), + quality_max_drop_fts(40) +#endif +{} + +Config& Config::getInstance() +{ + static Config instance; // Instantiated on first use and guaranteed to be destroyed + return instance; +} + +} // namespace svo + diff --git a/GPU_version/svo/src/depth_filter.cpp b/GPU_version/svo/src/depth_filter.cpp new file mode 100644 index 0000000..db24408 --- /dev/null +++ b/GPU_version/svo/src/depth_filter.cpp @@ -0,0 +1,352 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Seed::batch_counter = 0; +int Seed::seed_counter = 0; + +Seed::Seed(Feature* ftr, float depth_mean, float depth_min) : + batch_id(batch_counter), + id(seed_counter++), + ftr(ftr), + a(10), + b(10), + mu(1.0/depth_mean), + z_range(1.0/depth_min), + sigma2(z_range*z_range/36) +{} + +DepthFilter::DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb) : + feature_detector_(feature_detector), + seed_converged_cb_(seed_converged_cb), + seeds_updating_halt_(false), + thread_(NULL), + new_keyframe_set_(false), + new_keyframe_min_depth_(0.0), + new_keyframe_mean_depth_(0.0) +{} + +DepthFilter::~DepthFilter() +{ + stopThread(); + SVO_INFO_STREAM("DepthFilter destructed."); +} + +void DepthFilter::startThread() +{ + thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this); +} + +void DepthFilter::stopThread() +{ + SVO_INFO_STREAM("DepthFilter stop thread invoked."); + if(thread_ != NULL) + { + SVO_INFO_STREAM("DepthFilter interrupt and join thread... "); + seeds_updating_halt_ = true; + thread_->interrupt(); + thread_->join(); + thread_ = NULL; + } +} + +void DepthFilter::addFrame(FramePtr frame) +{ + if(thread_ != NULL) + { + { + lock_t lock(frame_queue_mut_); + if(frame_queue_.size() > 2) + frame_queue_.pop(); + frame_queue_.push(frame); + } + seeds_updating_halt_ = false; + frame_queue_cond_.notify_one(); + } + else + updateSeeds(frame); +} + +void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_min) +{ + new_keyframe_min_depth_ = depth_min; + new_keyframe_mean_depth_ = depth_mean; + if(thread_ != NULL) + { + new_keyframe_ = frame; + new_keyframe_set_ = true; + seeds_updating_halt_ = true; + frame_queue_cond_.notify_one(); + } + else + initializeSeeds(frame); +} + +void DepthFilter::initializeSeeds(FramePtr frame) +{ + Features new_features; + feature_detector_->setExistingFeatures(frame->fts_); + feature_detector_->detect(frame.get(), frame->img_pyr_, + Config::triangMinCornerScore(), new_features); + + // initialize a seed for every new feature + seeds_updating_halt_ = true; + lock_t lock(seeds_mut_); // by locking the updateSeeds function stops + ++Seed::batch_counter; + std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ + seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_)); + }); + + if(options_.verbose) + SVO_INFO_STREAM("DepthFilter: Initialized "<::iterator it=seeds_.begin(); + size_t n_removed = 0; + while(it!=seeds_.end()) + { + if(it->ftr->frame == frame.get()) + { + it = seeds_.erase(it); + ++n_removed; + } + else + ++it; + } + seeds_updating_halt_ = false; +} + +void DepthFilter::reset() +{ + seeds_updating_halt_ = true; + { + lock_t lock(seeds_mut_); + seeds_.clear(); + } + lock_t lock(); + while(!frame_queue_.empty()) + frame_queue_.pop(); + seeds_updating_halt_ = false; + + if(options_.verbose) + SVO_INFO_STREAM("DepthFilter: RESET."); +} + +void DepthFilter::updateSeedsLoop() +{ + while(!boost::this_thread::interruption_requested()) + { + FramePtr frame; + { + lock_t lock(frame_queue_mut_); + while(frame_queue_.empty() && new_keyframe_set_ == false) + frame_queue_cond_.wait(lock); + if(new_keyframe_set_) + { + new_keyframe_set_ = false; + seeds_updating_halt_ = false; + clearFrameQueue(); + frame = new_keyframe_; + } + else + { + frame = frame_queue_.front(); + frame_queue_.pop(); + } + } + updateSeeds(frame); + if(frame->isKeyframe()) + initializeSeeds(frame); + } +} + +void DepthFilter::updateSeeds(FramePtr frame) +{ + // update only a limited number of seeds, because we don't have time to do it + // for all the seeds in every frame! + size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size(); + lock_t lock(seeds_mut_); + std::list::iterator it=seeds_.begin(); + + const double focal_length = frame->cam_->errorMultiplier2(); + double px_noise = 1.0; + double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz) + + while( it!=seeds_.end()) + { + // set this value true when seeds updating should be interrupted + if(seeds_updating_halt_) + return; + + // check if seed is not already too old + if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) { + it = seeds_.erase(it); + continue; + } + + // check if point is visible in the current image + SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse(); + const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) ); + if(xyz_f.z() < 0.0) { + ++it; // behind the camera + continue; + } + if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast())) { + ++it; // point does not project in image + continue; + } + + // we are using inverse depth coordinates + float z_inv_min = it->mu + sqrt(it->sigma2); + float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f); + double z; + if(!matcher_.findEpipolarMatchDirect( + *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z)) + { + it->b++; // increase outlier probability when no match was found + ++it; + ++n_failed_matches; + continue; + } + + // compute tau + double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle); + double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau)); + + // update the estimate + updateSeed(1./z, tau_inverse*tau_inverse, &*it); + ++n_updates; + + if(frame->isKeyframe()) + { + // The feature detector should not initialize new seeds close to this location + feature_detector_->setGridOccpuancy(matcher_.px_cur_); + } + + // if the seed has converged, we initialize a new candidate point and remove the seed + if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh) + { + assert(it->ftr->point == NULL); // TODO this should not happen anymore + Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu))); + Point* point = new Point(xyz_world, it->ftr); + it->ftr->point = point; + /* FIXME it is not threadsafe to add a feature to the frame here. + if(frame->isKeyframe()) + { + Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_); + ftr->point = point; + point->addFrameRef(ftr); + frame->addFeature(ftr); + it->ftr->frame->addFeature(it->ftr); + } + else + */ + { + seed_converged_cb_(point, it->sigma2); // put in candidate list + } + it = seeds_.erase(it); + } + else if(isnan(z_inv_min)) + { + SVO_WARN_STREAM("z_min is NaN"); + it = seeds_.erase(it); + } + else + ++it; + } +} + +void DepthFilter::clearFrameQueue() +{ + while(!frame_queue_.empty()) + frame_queue_.pop(); +} + +void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list& seeds) +{ + lock_t lock(seeds_mut_); + for(std::list::iterator it=seeds_.begin(); it!=seeds_.end(); ++it) + { + if (it->ftr->frame == frame.get()) + seeds.push_back(*it); + } +} + +void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed) +{ + float norm_scale = sqrt(seed->sigma2 + tau2); + if(std::isnan(norm_scale)) + return; + boost::math::normal_distribution nd(seed->mu, norm_scale); + float s2 = 1./(1./seed->sigma2 + 1./tau2); + float m = s2*(seed->mu/seed->sigma2 + x/tau2); + float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x); + float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range; + float normalization_constant = C1 + C2; + C1 /= normalization_constant; + C2 /= normalization_constant; + float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.); + float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.)) + + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f)); + + // update parameters + float mu_new = C1*m+C2*seed->mu; + seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new; + seed->mu = mu_new; + seed->a = (e-f)/(f-e/f); + seed->b = seed->a*(1.0f-f)/f; +} + +double DepthFilter::computeTau( + const SE3& T_ref_cur, + const Vector3d& f, + const double z, + const double px_error_angle) +{ + Vector3d t(T_ref_cur.translation()); + Vector3d a = f*z-t; + double t_norm = t.norm(); + double a_norm = a.norm(); + double alpha = acos(f.dot(t)/t_norm); // dot product + double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product + double beta_plus = beta + px_error_angle; + double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI + double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines + return (z_plus - z); // tau +} + +} // namespace svo diff --git a/GPU_version/svo/src/feature_alignment.cpp b/GPU_version/svo/src/feature_alignment.cpp new file mode 100644 index 0000000..faa4027 --- /dev/null +++ b/GPU_version/svo/src/feature_alignment.cpp @@ -0,0 +1,601 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifdef __SSE2__ +#include +#endif +#ifdef __ARM_NEON__ +#include +#endif +#include + +namespace svo { +namespace feature_alignment { + +#define SUBPIX_VERBOSE 0 + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv) +{ + const int halfpatch_size_ = 4; + const int patch_size = 8; + const int patch_area = 64; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dv[patch_area]; + Matrix2f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size+2; + float* it_dv = ref_patch_dv; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dv = ref_patch_dv; + float new_chi2 = 0.0; + Vector2f Jres; Jres.setZero(); + for(int y=0; y 0 && new_chi2 > chi2) + { +#if SUBPIX_VERBOSE + cout << "error increased." << endl; +#endif + u -= update[0]; + v -= update[1]; + break; + } + + chi2 = new_chi2; + update = Hinv * Jres; + u += update[0]*dir[0]; + v += update[0]*dir[1]; + mean_diff += update[1]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] + << "\t new chi2 = " << new_chi2 << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd) +{ +#ifdef __ARM_NEON__ + if(!no_simd) + return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate); +#endif + + const int halfpatch_size_ = 4; + const int patch_size_ = 8; + const int patch_area_ = 64; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_]; + float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_]; + Matrix3f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size_+2; + float* it_dx = ref_patch_dx; + float* it_dy = ref_patch_dy; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dx = ref_patch_dx; + float* it_ref_dy = ref_patch_dy; +// float new_chi2 = 0.0; + Vector3f Jres; Jres.setZero(); + for(int y=0; y 0 && new_chi2 > chi2) + { +#if SUBPIX_VERBOSE + cout << "error increased." << endl; +#endif + u -= update[0]; + v -= update[1]; + break; + } + chi2 = new_chi2; +*/ + update = Hinv * Jres; + u += update[0]; + v += update[1]; + mean_diff += update[2]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] +// << "\t new chi2 = " << new_chi2 << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +#define DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n)) // rounds to closest integer and descales + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate) +{ + // TODO: This function should not be used as the alignment is not robust to illumination changes! + const int halfpatch_size = 4; + const int patch_size = 8; + const int patch_area = 64; + bool converged=false; + const int W_BITS = 14; + + // compute derivative of template and prepare inverse compositional + int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area]; + int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area]; + + // compute gradient and hessian + const int ref_step = patch_size+2; + int16_t* it_dx = ref_patch_dx; + int16_t* it_dy = ref_patch_dy; + float A11=0, A12=0, A22=0; + for(int y=0; y(it[1]) - it[-1]; + int16_t dy = static_cast(it[ref_step]) - it[-ref_step]; + *it_dx = dx; + *it_dy = dy; // we are missing a factor 1/2 + A11 += static_cast(dx*dx); // we are missing a factor 1/4 + A12 += static_cast(dx*dy); + A22 += static_cast(dy*dy); + } + } + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.03*0.03; + const int cur_step = cur_img.step.p[0]; + const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16 + float chi2 = 0; + float update_u = 0, update_v = 0; + + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + float new_chi2 = 0.0; + +#ifdef __SSE2__ + // compute bilinear interpolation weights + int wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS)); + int wTR = static_cast(subpix_x * (1.0f-subpix_y)*(1 << W_BITS)); + int wBL = static_cast((1.0f-subpix_x)*subpix_y*(1 << W_BITS)); + int wBR = (1 << W_BITS) - wTL - wTR - wBL; + + __m128i qw0 = _mm_set1_epi32(wTL + (wTR << 16)); // Sets the 4 signed 32-bit integer values to [wTL, wTR]. + __m128i qw1 = _mm_set1_epi32(wBL + (wBR << 16)); + __m128i z = _mm_setzero_si128(); + __m128 qb0 = _mm_setzero_ps(); // 4 floats + __m128 qb1 = _mm_setzero_ps(); // 4 floats + __m128i qdelta = _mm_set1_epi32(1 << (W_BITS-1)); + for(int y=0; y(it[1] - it[-1]); + *it_dy = static_cast(it[ref_step] - it[-ref_step]); // divide by 2 missing + Vector3f J(*it_dx, *it_dy, 1.0f); + H += J*J.transpose(); + } + } + Matrix3f Hinv = H.inverse(); + float mean_diff = 0.0; + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.03*0.03; + const int cur_step = cur_img.step.p[0]; + Vector3f update; + Vector3f Jres; + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + +#ifdef __ARM_NEON__ + const int SHIFT_BITS = 7; + const uint16_t wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1<(subpix_x*(1.0f-subpix_y)*(1<((1.0f-subpix_x)*subpix_y*(1<((1 << SHIFT_BITS) - wTL - wTR - wBL); + + // initialize result to zero + int32x4_t vb1 = vdupq_n_s32(0); + int32x4_t vb2 = vdupq_n_s32(0); + int16x8_t vmean_diff = vdupq_n_s16( (int16_t) (mean_diff+0.5) ); + int16x8_t vres_sum = vdupq_n_s16(0); + for(int y=0; y Vr[i] := Va[i] + Vb[i] * Vc[i] + // int32x4_t vmlal_s16(int32x4_t a, int16x4_t b, int16x4_t c); // VMLAL.S16 q0,d0,d0 + int16x8_t grad = vld1q_s16(ref_patch_dx + y*patch_size); + vb1 = vmlal_s16(vb1, vget_low_s16(grad), vget_low_s16(res)); + vb1 = vmlal_s16(vb1, vget_high_s16(grad), vget_high_s16(res)); + + grad = vld1q_s16(ref_patch_dy + y*patch_size); + vb2 = vmlal_s16(vb2, vget_low_s16(grad), vget_low_s16(res)); + vb2 = vmlal_s16(vb2, vget_high_s16(grad), vget_high_s16(res)); + } + + // finally, sum results of vb1, vb2 and vres_sum + int32x2_t tmp; + tmp = vpadd_s32(vget_low_s32(vb1), vget_high_s32(vb1)); + Jres[0] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + tmp = vpadd_s32(vget_low_s32(vb2), vget_high_s32(vb2)); + Jres[1] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + int32x4_t vres_sum1 = vpaddlq_s16(vres_sum); + tmp = vpadd_s32(vget_low_s32(vres_sum1), vget_high_s32(vres_sum1)); + Jres[2] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); +#endif + + update = Hinv * Jres * 2; // * 2 to compensate because above, we did not compute the derivative correctly + u += update[0]; + v += update[1]; + mean_diff += update[2]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +} // namespace feature_alignment +} // namespace svo diff --git a/GPU_version/svo/src/feature_detection.cpp b/GPU_version/svo/src/feature_detection.cpp new file mode 100644 index 0000000..98183f8 --- /dev/null +++ b/GPU_version/svo/src/feature_detection.cpp @@ -0,0 +1,118 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include + +namespace svo { +namespace feature_detection { + +AbstractDetector::AbstractDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels) : + cell_size_(cell_size), + n_pyr_levels_(n_pyr_levels), + grid_n_cols_(ceil(static_cast(img_width)/cell_size_)), + grid_n_rows_(ceil(static_cast(img_height)/cell_size_)), + grid_occupancy_(grid_n_cols_*grid_n_rows_, false) +{} + +void AbstractDetector::resetGrid() +{ + std::fill(grid_occupancy_.begin(), grid_occupancy_.end(), false); +} + +void AbstractDetector::setExistingFeatures(const Features& fts) +{ + std::for_each(fts.begin(), fts.end(), [&](Feature* i){ + grid_occupancy_.at( + static_cast(i->px[1]/cell_size_)*grid_n_cols_ + + static_cast(i->px[0]/cell_size_)) = true; + }); +} + +void AbstractDetector::setGridOccpuancy(const Vector2d& px) +{ + grid_occupancy_.at( + static_cast(px[1]/cell_size_)*grid_n_cols_ + + static_cast(px[0]/cell_size_)) = true; +} + +FastDetector::FastDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels) : + AbstractDetector(img_width, img_height, cell_size, n_pyr_levels) +{} + +void FastDetector::detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts) +{ + Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f)); + for(int L=0; L fast_corners; +#if __SSE2__ + fast::fast_corner_detect_10_sse2( + (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, + img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#elif HAVE_FAST_NEON + fast::fast_corner_detect_9_neon( + (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, + img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#else + fast::fast_corner_detect_10( + (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, + img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#endif + vector scores, nm_corners; + fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores); + fast::fast_nonmax_3x3(fast_corners, scores, nm_corners); + + for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it) + { + fast::fast_xy& xy = fast_corners.at(*it); + const int k = static_cast((xy.y*scale)/cell_size_)*grid_n_cols_ + + static_cast((xy.x*scale)/cell_size_); + if(grid_occupancy_[k]) + continue; + const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y); + if(score > corners.at(k).score) + corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f); + } + } + + // Create feature for every corner that has high enough corner score + std::for_each(corners.begin(), corners.end(), [&](Corner& c) { + if(c.score > detection_threshold) + fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level)); + }); + + resetGrid(); +} + +} // namespace feature_detection +} // namespace svo + diff --git a/GPU_version/svo/src/frame.cpp b/GPU_version/svo/src/frame.cpp new file mode 100644 index 0000000..44ddab6 --- /dev/null +++ b/GPU_version/svo/src/frame.cpp @@ -0,0 +1,190 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Frame::frame_counter_ = 0; + +Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) : + id_(frame_counter_++), + timestamp_(timestamp), + cam_(cam), + key_pts_(5), + is_keyframe_(false), + v_kf_(NULL) +{ + initFrame(img); +} + +Frame::~Frame() +{ + std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;}); +} + +void Frame::initFrame(const cv::Mat& img) +{ + // check image + if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height()) + throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale"); + + // Set keypoints to NULL + std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; }); + + // Build Image Pyramid + frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_); +} + +void Frame::setKeyframe() +{ + is_keyframe_ = true; + setKeyPoints(); +} + +void Frame::addFeature(Feature* ftr) +{ + fts_.push_back(ftr); +} + +void Frame::setKeyPoints() +{ + for(size_t i = 0; i < 5; ++i) + if(key_pts_[i] != NULL) + if(key_pts_[i]->point == NULL) + key_pts_[i] = NULL; + + std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); }); +} + +void Frame::checkKeyPoints(Feature* ftr) +{ + const int cu = cam_->width()/2; + const int cv = cam_->height()/2; + + // center pixel + if(key_pts_[0] == NULL) + key_pts_[0] = ftr; + else if(std::max(std::fabs(ftr->px[0]-cu), std::fabs(ftr->px[1]-cv)) + < std::max(std::fabs(key_pts_[0]->px[0]-cu), std::fabs(key_pts_[0]->px[1]-cv))) + key_pts_[0] = ftr; + + if(ftr->px[0] >= cu && ftr->px[1] >= cv) + { + if(key_pts_[1] == NULL) + key_pts_[1] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[1]->px[0]-cu) * (key_pts_[1]->px[1]-cv)) + key_pts_[1] = ftr; + } + if(ftr->px[0] >= cu && ftr->px[1] < cv) + { + if(key_pts_[2] == NULL) + key_pts_[2] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[2]->px[0]-cu) * (key_pts_[2]->px[1]-cv)) + key_pts_[2] = ftr; + } + if(ftr->px[0] < cv && ftr->px[1] < cv) + { + if(key_pts_[3] == NULL) + key_pts_[3] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[3]->px[0]-cu) * (key_pts_[3]->px[1]-cv)) + key_pts_[3] = ftr; + } + if(ftr->px[0] < cv && ftr->px[1] >= cv) + { + if(key_pts_[4] == NULL) + key_pts_[4] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[4]->px[0]-cu) * (key_pts_[4]->px[1]-cv)) + key_pts_[4] = ftr; + } +} + +void Frame::removeKeyPoint(Feature* ftr) +{ + bool found = false; + std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){ + if(i == ftr) { + i = NULL; + found = true; + } + }); + if(found) + setKeyPoints(); +} + +bool Frame::isVisible(const Vector3d& xyz_w) const +{ + Vector3d xyz_f = T_f_w_*xyz_w; + if(xyz_f.z() < 0.0) + return false; // point is behind the camera + Vector2d px = f2c(xyz_f); + if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height()) + return true; + return false; +} + + +/// Utility functions for the Frame class +namespace frame_utils { + +void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr) +{ + pyr.resize(n_levels); + pyr[0] = img_level_0; + for(int i=1; i depth_vec; + depth_vec.reserve(frame.fts_.size()); + depth_min = std::numeric_limits::max(); + for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it) + { + if((*it)->point != NULL) + { + const double z = frame.w2f((*it)->point->pos_).z(); + depth_vec.push_back(z); + depth_min = fmin(z, depth_min); + } + } + if(depth_vec.empty()) + { + SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!"); + return false; + } + depth_mean = vk::getMedian(depth_vec); + return true; +} + +} // namespace frame_utils +} // namespace svo diff --git a/GPU_version/svo/src/frame_handler_base.cpp b/GPU_version/svo/src/frame_handler_base.cpp new file mode 100644 index 0000000..175ee01 --- /dev/null +++ b/GPU_version/svo/src/frame_handler_base.cpp @@ -0,0 +1,199 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo +{ + +// definition of global and static variables which were declared in the header +#ifdef SVO_TRACE +vk::PerformanceMonitor* g_permon = NULL; +#endif + +FrameHandlerBase::FrameHandlerBase() : + stage_(STAGE_PAUSED), + set_reset_(false), + set_start_(false), + acc_frame_timings_(10), + acc_num_obs_(10), + num_obs_last_(0), + tracking_quality_(TRACKING_INSUFFICIENT) +{ +#ifdef SVO_TRACE + // Initialize Performance Monitor + g_permon = new vk::PerformanceMonitor(); + g_permon->addTimer("pyramid_creation"); + g_permon->addTimer("sparse_img_align"); + g_permon->addTimer("reproject"); + g_permon->addTimer("reproject_kfs"); + g_permon->addTimer("reproject_candidates"); + g_permon->addTimer("feature_align"); + g_permon->addTimer("pose_optimizer"); + g_permon->addTimer("point_optimizer"); + g_permon->addTimer("local_ba"); + g_permon->addTimer("tot_time"); + g_permon->addLog("timestamp"); + g_permon->addLog("img_align_n_tracked"); + g_permon->addLog("repr_n_mps"); + g_permon->addLog("repr_n_new_references"); + g_permon->addLog("sfba_thresh"); + g_permon->addLog("sfba_error_init"); + g_permon->addLog("sfba_error_final"); + g_permon->addLog("sfba_n_edges_final"); + g_permon->addLog("loba_n_erredges_init"); + g_permon->addLog("loba_n_erredges_fin"); + g_permon->addLog("loba_err_init"); + g_permon->addLog("loba_err_fin"); + g_permon->addLog("n_candidates"); + g_permon->addLog("dropout"); + g_permon->init(Config::traceName(), Config::traceDir()); +#endif + + SVO_INFO_STREAM("SVO initialized"); +} + +FrameHandlerBase::~FrameHandlerBase() +{ + SVO_INFO_STREAM("SVO destructor invoked"); +#ifdef SVO_TRACE + delete g_permon; +#endif +} + +bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp) +{ + if(set_start_) + { + resetAll(); + stage_ = STAGE_FIRST_FRAME; + } + + if(stage_ == STAGE_PAUSED) + return false; + + SVO_LOG(timestamp); + SVO_DEBUG_STREAM("New Frame"); + SVO_START_TIMER("tot_time"); + timer_.start(); + + // some cleanup from last iteration, can't do before because of visualization + map_.emptyTrash(); + return true; +} + +int FrameHandlerBase::finishFrameProcessingCommon( + const size_t update_id, + const UpdateResult dropout, + const size_t num_observations) +{ + SVO_DEBUG_STREAM("Frame: "<writeToFile(); + { + boost::unique_lock lock(map_.point_candidates_.mut_); + size_t n_candidates = map_.point_candidates_.candidates_.size(); + SVO_LOG(n_candidates); + } +#endif + + if(dropout == RESULT_FAILURE && + (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING )) + { + stage_ = STAGE_RELOCALIZING; + tracking_quality_ = TRACKING_INSUFFICIENT; + } + else if (dropout == RESULT_FAILURE) + resetAll(); + if(set_reset_) + resetAll(); + + return 0; +} + +void FrameHandlerBase::resetCommon() +{ + map_.reset(); + stage_ = STAGE_PAUSED; + set_reset_ = false; + set_start_ = false; + tracking_quality_ = TRACKING_INSUFFICIENT; + num_obs_last_ = 0; + SVO_INFO_STREAM("RESET"); +} + +void FrameHandlerBase::setTrackingQuality(const size_t num_observations) +{ + tracking_quality_ = TRACKING_GOOD; + if(num_observations < Config::qualityMinFts()) + { + SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!"); + tracking_quality_ = TRACKING_INSUFFICIENT; + } + const int feature_drop = static_cast(std::min(num_obs_last_, Config::maxFts())) - num_observations; + if(feature_drop > Config::qualityMaxFtsDrop()) + { + SVO_WARN_STREAM("Lost "<< feature_drop <<" features!"); + tracking_quality_ = TRACKING_INSUFFICIENT; + } +} + +bool ptLastOptimComparator(Point* lhs, Point* rhs) +{ + return (lhs->last_structure_optim_ < rhs->last_structure_optim_); +} + +void FrameHandlerBase::optimizeStructure( + FramePtr frame, + size_t max_n_pts, + int max_iter) +{ + deque pts; + for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point != NULL) + pts.push_back((*it)->point); + } + max_n_pts = min(max_n_pts, pts.size()); + nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator); + for(deque::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it) + { + (*it)->optimize(max_iter); + (*it)->last_structure_optim_ = frame->id_; + } +} + + +} // namespace svo diff --git a/GPU_version/svo/src/frame_handler_mono.cpp b/GPU_version/svo/src/frame_handler_mono.cpp new file mode 100644 index 0000000..799cb8c --- /dev/null +++ b/GPU_version/svo/src/frame_handler_mono.cpp @@ -0,0 +1,307 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef USE_BUNDLE_ADJUSTMENT +#include +#endif + +namespace svo { + +FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init) : + FrameHandlerBase(), + cam_(cam), + reprojector_(cam_, map_), + depth_filter_(NULL) +{ + initialize(); + imu_integPtr_=std::make_unique(SE_init); +} + +void FrameHandlerMono::initialize() +{ + feature_detection::DetectorPtr feature_detector( + new feature_detection::FastDetector( + cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels())); + DepthFilter::callback_t depth_filter_cb = boost::bind( + &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2); + depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb); + depth_filter_->startThread(); +} + +FrameHandlerMono::~FrameHandlerMono() +{ + delete depth_filter_; +} + +void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp) +{ + if(!startFrameProcessingCommon(timestamp)) + return; + + // some cleanup from last iteration, can't do before because of visualization + core_kfs_.clear(); + overlap_kfs_.clear(); + + // create new frame + new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); + + // process frame + UpdateResult res = RESULT_FAILURE; + if(stage_ == STAGE_DEFAULT_FRAME) + res = processFrame(); + else if(stage_ == STAGE_SECOND_FRAME) + res = processSecondFrame(); + else if(stage_ == STAGE_FIRST_FRAME) + res = processFirstFrame(); + else if(stage_ == STAGE_RELOCALIZING) + res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()), + map_.getClosestKeyframe(last_frame_)); + + // set last frame + last_frame_ = new_frame_; + new_frame_.reset(); + // finish processing + finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); +} + +FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() +{ + new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero()); + if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE) + return RESULT_NO_KEYFRAME; + new_frame_->setKeyframe(); + map_.addKeyframe(new_frame_); + stage_ = STAGE_SECOND_FRAME; + SVO_INFO_STREAM("Init: Selected first frame."); + return RESULT_IS_KEYFRAME; +} + +FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() +{ + initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_); + if(res == initialization::FAILURE) + return RESULT_FAILURE; + else if(res == initialization::NO_KEYFRAME) + return RESULT_NO_KEYFRAME; + + // two-frame bundle adjustment +#ifdef USE_BUNDLE_ADJUSTMENT + ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_); +#endif + + new_frame_->setKeyframe(); + double depth_mean, depth_min; + frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); + + // add frame to map + map_.addKeyframe(new_frame_); + stage_ = STAGE_DEFAULT_FRAME; + klt_homography_init_.reset(); + SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map."); + return RESULT_IS_KEYFRAME; +} + +FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() +{ + // Set initial pose TODO use prior + new_frame_->T_f_w_ = last_frame_->T_f_w_; + + // sparse image align + SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), + 30, SparseImgAlign::GaussNewton, false, false); + img_align.run(last_frame_, new_frame_); + // map reprojection & feature alignment + reprojector_.reprojectMap(new_frame_, overlap_kfs_); + std::cerr<<"Reprojection Map:\t nPoints = "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps + tracking_quality_ = TRACKING_INSUFFICIENT; + return RESULT_FAILURE; + } + + // pose optimization subject to change for adding IMU + size_t sfba_n_edges_final; + if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh())) + return RESULT_FAILURE; + + + std::cerr<<"Reprojected points after opmization: "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps + return RESULT_FAILURE; + } + double depth_mean, depth_min; + frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD) + { + depth_filter_->addFrame(new_frame_); + return RESULT_NO_KEYFRAME; + } + new_frame_->setKeyframe(); + + // new keyframe selected + for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it) + if((*it)->point != NULL) + (*it)->point->addFrameRef(*it); + map_.point_candidates_.addCandidatePointToFrame(new_frame_); + + // optional bundle adjustment +#ifdef USE_BUNDLE_ADJUSTMENT + if(Config::lobaNumIter() > 0) + { + SVO_START_TIMER("local_ba"); + setCoreKfs(Config::coreNKfs()); + size_t loba_n_erredges_init, loba_n_erredges_fin; + double loba_err_init, loba_err_fin; + ba::localBA(new_frame_.get(), &core_kfs_, &map_, + loba_n_erredges_init, loba_n_erredges_fin, + loba_err_init, loba_err_fin); + SVO_STOP_TIMER("local_ba"); + SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin); + SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<addKeyframe(new_frame_, depth_mean, 0.5*depth_min); + + // if limited number of keyframes, remove the one furthest apart + if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs()) + { + FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos()); + depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better + map_.safeDeleteFrame(furthest_frame); + } + + // add keyframe to map + map_.addKeyframe(new_frame_); + std::cerr<<"Svo pipline was finished\n"; + return RESULT_IS_KEYFRAME; +} + +FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame( + const SE3& T_cur_ref, + FramePtr ref_keyframe) +{ + if(ref_keyframe == nullptr) + { + return RESULT_FAILURE; + } + SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), + 30, SparseImgAlign::GaussNewton, false, false); + size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_); + usleep(2000); + if(img_align_n_tracked > Config::qualityMinFts()) + { + SE3 T_f_w_last = last_frame_->T_f_w_; + last_frame_ = ref_keyframe; + FrameHandlerMono::UpdateResult res = processFrame(); + if(res != RESULT_FAILURE) + { + stage_ = STAGE_DEFAULT_FRAME; + } + else + new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose + return res; + } + return RESULT_FAILURE; +} + +bool FrameHandlerMono::relocalizeFrameAtPose( + const int keyframe_id, + const SE3& T_f_kf, + const cv::Mat& img, + const double timestamp) +{ + FramePtr ref_keyframe; + if(!map_.getKeyframeById(keyframe_id, ref_keyframe)) + return false; + new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); + UpdateResult res = relocalizeFrame(T_f_kf, ref_keyframe); + if(res != RESULT_FAILURE) { + last_frame_ = new_frame_; + return true; + } + return false; +} + +void FrameHandlerMono::resetAll() +{ + resetCommon(); + last_frame_.reset(); + new_frame_.reset(); + core_kfs_.clear(); + overlap_kfs_.clear(); + depth_filter_->reset(); +} + +void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame) +{ + resetAll(); + last_frame_ = first_frame; + last_frame_->setKeyframe(); + map_.addKeyframe(last_frame_); + stage_ = STAGE_DEFAULT_FRAME; +} + +bool FrameHandlerMono::needNewKf(double scene_depth_mean) +{ + for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it) + { + Vector3d relpos = new_frame_->w2f(it->first->pos()); + if(fabs(relpos.x())/scene_depth_mean < Config::kfSelectMinDist() && + fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist()*0.8 && + fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist()*1.3) + return false; + } + return true; +} + +void FrameHandlerMono::setCoreKfs(size_t n_closest) +{ + size_t n = min(n_closest, overlap_kfs_.size()-1); + std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(), + boost::bind(&pair::second, _1) > + boost::bind(&pair::second, _2)); + std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair& i){ core_kfs_.insert(i.first); }); +} + +} // namespace svo diff --git a/GPU_version/svo/src/imu_integration.cpp b/GPU_version/svo/src/imu_integration.cpp new file mode 100644 index 0000000..c2a720f --- /dev/null +++ b/GPU_version/svo/src/imu_integration.cpp @@ -0,0 +1,151 @@ +// +// Created by gfuse on 4/9/21. +// +//#define NDEBUG +#include "svo/imu_integration.h" +#include +#include +#include "svo/feature.h" +#include "svo/point.h" +#include "svo/svoVisionFactor.h" +#include +#include +#include +#include + +Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ + graphPtr=std::make_shared(); + valuesPtr=std::make_shared(); + parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame + parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous + parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous + parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous + parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC(); // acc bias in continuous + parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC(); // gyro bias in continuous + parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP(); // error in the bias used for preintegration + imu_biasPtr=std::make_shared(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8)); + preintegratedPtr = std::make_shared(parameterPtr,*imu_biasPtr); + ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection + optimizerParamPtr=std::make_shared(); + optimizerParamPtr->relinearizeThreshold = 0.1; + optimizerParamPtr->relinearizeSkip = 1; + //optimizerParamPtr->evaluateNonlinearError=true; + assert(preintegratedPtr); +} +Imu_Integration::~Imu_Integration(){ + +} +bool Imu_Integration::update(double* imu){ + if(syn)return false; + if(imu_factor_id<1)return false; + auto in=std::chrono::steady_clock::now(); + if(std::chrono::duration(in-t_1).count()>20.0){ + t_1=in; + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005); + }else{ + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration(in-t_1).count()); + t_1=in; + ++imu_n; + } + return true; + +} +bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame){ + gtsam::Values result = optimizer.calculateEstimate(); + // Overwrite the beginning of the preintegration for the next step. + statePtr=std::make_shared(result.at(P(imu_factor_id)), result.at(V(imu_factor_id))); + imu_biasPtr=std::make_shared(result.at(B(imu_factor_id))); + preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr); + ++imu_factor_id; + syn=false; + new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); + new_frame->Cov_=optimizer.marginalCovariance(P(imu_factor_id-1)); + graphPtr->resize(0); + valuesPtr->clear(); + valuesPtr->insert(P(imu_factor_id-1),statePtr->pose()); + valuesPtr->insert(V(imu_factor_id-1),statePtr->v()); + valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr); + graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),optimizer.marginalCovariance(P(imu_factor_id-1))); + graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),optimizer.marginalCovariance(V(imu_factor_id-1))); + graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.marginalCovariance(B(imu_factor_id-1))); + return true; +} +bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame){ + gtsam::Values result = optimizer.optimize(); + // Overwrite the beginning of the preintegration for the next step. + statePtr=std::make_shared(result.at(P(imu_factor_id)), result.at(V(imu_factor_id))); + imu_biasPtr=std::make_shared(result.at(B(imu_factor_id))); + preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr); + ++imu_factor_id; + syn=false; + gtsam::Marginals marginals(*graphPtr,result); + new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); + new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1)); + graphPtr->resize(0); + valuesPtr->clear(); + valuesPtr->insert(P(imu_factor_id-1),statePtr->pose()); + valuesPtr->insert(V(imu_factor_id-1),statePtr->v()); + valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr); + graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1))); + graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1))); + graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1))); + return true; +} +bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size_t& num_obs,const double reproj_thresh){ + usleep(5000); + syn=true; + if(imu_factor_id<1){ + statePtr=std::make_shared(gtsam::Pose3(new_frame->T_f_w_.matrix()), + gtsam::Vector3(1e-12,1e-12,1e-12)); + valuesPtr->insert(P(0),statePtr->pose()); + valuesPtr->insert(V(0),statePtr->v()); + valuesPtr->insert(B(0),*imu_biasPtr); + graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished())); + graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); + graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); + }else{ + if(imu_n<1)return false; + auto preint_imu_combined =dynamic_cast(*preintegratedPtr); + valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix())); + valuesPtr->insert(V(imu_factor_id), statePtr->v()); + valuesPtr->insert(B(imu_factor_id), *imu_biasPtr); + gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id), + preint_imu_combined); + graphPtr->emplace_shared(imu_factor); + imu_n=0; + } + + num_obs=0; + for(auto&& f:new_frame->fts_){ + if(f->point==NULL)continue; + graphPtr->emplace_shared(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id)); + ++num_obs; + } + // Now optimize. + //gtsam::ISAM2 optimizer(*optimizerParamPtr); + //optimizer.update(*graphPtr, *valuesPtr); + // Each call to iSAM2 update(*) performs one iteration of the iterative + // nonlinear solver. If accuracy is desired at the expense of time, + // update(*) can be called additional times to perform multiple optimizer + // iterations every step. + //optimizer.update(); + // optimizer.print(); + gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr); + + reset(optimizer,new_frame); + // Remove Measurements with too large reprojection error + double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2(); + for(auto&& f:new_frame->fts_) + { + if(f->point == NULL) + continue; + Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<level); + if(e.norm() > reproj_thresh_scaled) + { + // we don't need to delete a reference in the point since it was not created yet + f->point = NULL; + --num_obs; + } + } + return true; +} \ No newline at end of file diff --git a/GPU_version/svo/src/initialization.cpp b/GPU_version/svo/src/initialization.cpp new file mode 100644 index 0000000..571a0bc --- /dev/null +++ b/GPU_version/svo/src/initialization.cpp @@ -0,0 +1,199 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace initialization { + +InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref) +{ + reset(); + detectFeatures(frame_ref, px_ref_, f_ref_); + if(px_ref_.size() < 100) + { + SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment."); + return FAILURE; + } + frame_ref_ = frame_ref; + px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end()); + return SUCCESS; +} + +InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) +{ + trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_); + SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features"); + + if(disparities_.size() < Config::initMinTracked()) + return FAILURE; + + double disparity = vk::getMedian(disparities_); + SVO_INFO_STREAM("Init: KLT "<cam_->errorMultiplier2(), Config::poseOptimThresh(), + inliers_, xyz_in_cur_, T_cur_from_ref_); + SVO_INFO_STREAM("Init: Homography RANSAC "< depth_vec; + for(size_t i=0; iT_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_; + frame_cur->T_f_w_.translation() = + -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); + + // For each inlier create 3D point and add feature in both frames + SE3 T_world_cur = frame_cur->T_f_w_.inverse(); + for(vector::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) + { + Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y); + Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y); + if(frame_ref_->cam_->isInFrame(px_cur.cast(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast(), 10) && xyz_in_cur_[*it].z() > 0) + { + Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale); + Point* new_point = new Point(pos); + + Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0)); + frame_cur->addFeature(ftr_cur); + new_point->addFrameRef(ftr_cur); + + Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0)); + frame_ref_->addFeature(ftr_ref); + new_point->addFrameRef(ftr_ref); + } + } + return SUCCESS; +} + +void KltHomographyInit::reset() +{ + px_cur_.clear(); + frame_ref_.reset(); +} + +void detectFeatures( + FramePtr frame, + vector& px_vec, + vector& f_vec) +{ + Features new_features; + feature_detection::FastDetector detector( + frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels()); + detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features); + + // now for all maximum corners, initialize a new seed + px_vec.clear(); px_vec.reserve(new_features.size()); + f_vec.clear(); f_vec.reserve(new_features.size()); + std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ + px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1])); + f_vec.push_back(ftr->f); + delete ftr; + }); +} + +void trackKlt( + FramePtr frame_ref, + FramePtr frame_cur, + vector& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities) +{ + const double klt_win_size = 30.0; + const int klt_max_iter = 30; + const double klt_eps = 0.001; + vector status; + vector error; + vector min_eig_vec; + cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps); + cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0], + px_ref, px_cur, + status, error, + cv::Size2i(klt_win_size, klt_win_size), + 4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW); + + vector::iterator px_ref_it = px_ref.begin(); + vector::iterator px_cur_it = px_cur.begin(); + vector::iterator f_ref_it = f_ref.begin(); + f_cur.clear(); f_cur.reserve(px_cur.size()); + disparities.clear(); disparities.reserve(px_cur.size()); + for(size_t i=0; px_ref_it != px_ref.end(); ++i) + { + if(!status[i]) + { + px_ref_it = px_ref.erase(px_ref_it); + px_cur_it = px_cur.erase(px_cur_it); + f_ref_it = f_ref.erase(f_ref_it); + continue; + } + f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y)); + disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm()); + ++px_ref_it; + ++px_cur_it; + ++f_ref_it; + } +} + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE3& T_cur_from_ref) +{ + vector uv_ref(f_ref.size()); + vector uv_cur(f_cur.size()); + for(size_t i=0, i_max=f_ref.size(); i outliers; + vk::computeInliers(f_cur, f_ref, + Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(), + reprojection_threshold, focal_length, + xyz_in_cur, inliers, outliers); + T_cur_from_ref = Homography.T_c2_from_c1; +} + + +} // namespace initialization +} // namespace svo diff --git a/GPU_version/svo/src/map.cpp b/GPU_version/svo/src/map.cpp new file mode 100644 index 0000000..2ecad1c --- /dev/null +++ b/GPU_version/svo/src/map.cpp @@ -0,0 +1,368 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include + +namespace svo { + +Map::Map() {} + +Map::~Map() +{ + reset(); + SVO_INFO_STREAM("Map destructed"); +} + +void Map::reset() +{ + keyframes_.clear(); + point_candidates_.reset(); + emptyTrash(); +} + +bool Map::safeDeleteFrame(FramePtr frame) +{ + bool found = false; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + if(*it == frame) + { + std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){ + removePtFrameRef(it->get(), ftr); + }); + keyframes_.erase(it); + found = true; + break; + } + } + + point_candidates_.removeFrameCandidates(frame); + + if(found) + return true; + + SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there."); + return false; +} + +void Map::removePtFrameRef(Frame* frame, Feature* ftr) +{ + if(ftr->point == NULL) + return; // mappoint may have been deleted in a previous ref. removal + Point* pt = ftr->point; + ftr->point = NULL; + if(pt->obs_.size() <= 2) + { + // If the references list of mappoint has only size=2, delete mappoint + safeDeletePoint(pt); + return; + } + pt->deleteFrameRef(frame); // Remove reference from map_point + frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe +} + +void Map::safeDeletePoint(Point* pt) +{ + // Delete references to mappoints in all keyframes + std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature* ftr){ + ftr->point=NULL; + ftr->frame->removeKeyPoint(ftr); + }); + pt->obs_.clear(); + + // Delete mappoint + deletePoint(pt); +} + +void Map::deletePoint(Point* pt) +{ + pt->type_ = Point::TYPE_DELETED; + trash_points_.push_back(pt); +} + +void Map::addKeyframe(FramePtr new_keyframe) +{ + keyframes_.push_back(new_keyframe); +} + +void Map::getCloseKeyframes( + const FramePtr& frame, + std::list< std::pair >& close_kfs) const +{ + for(auto kf : keyframes_) + { + // check if kf has overlaping field of view with frame, use therefore KeyPoints + for(auto keypoint : kf->key_pts_) + { + if(keypoint == nullptr) + continue; + + if(frame->isVisible(keypoint->point->pos_)) + { + close_kfs.push_back( + std::make_pair( + kf, (frame->T_f_w_.translation()-kf->T_f_w_.translation()).norm())); + break; // this keyframe has an overlapping field of view -> add to close_kfs + } + } + } +} + +FramePtr Map::getClosestKeyframe(const FramePtr& frame) const +{ + list< pair > close_kfs; + getCloseKeyframes(frame, close_kfs); + if(close_kfs.empty()) + { + return nullptr; + } + + + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + if(close_kfs.front().first != frame) + return close_kfs.front().first; + close_kfs.pop_front(); + return close_kfs.front().first; +} + +FramePtr Map::getFurthestKeyframe(const Vector3d& pos) const +{ + FramePtr furthest_kf; + double maxdist = 0.0; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + double dist = ((*it)->pos()-pos).norm(); + if(dist > maxdist) { + maxdist = dist; + furthest_kf = *it; + } + } + return furthest_kf; +} + +bool Map::getKeyframeById(const int id, FramePtr& frame) const +{ + bool found = false; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + if((*it)->id_ == id) { + found = true; + frame = *it; + break; + } + return found; +} + +void Map::transform(const Matrix3d& R, const Vector3d& t, const double& s) +{ + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + Vector3d pos = s*R*(*it)->pos() + t; + Matrix3d rot = R*(*it)->T_f_w_.rotation_matrix().inverse(); + (*it)->T_f_w_ = SE3(rot, pos).inverse(); + for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr) + { + if((*ftr)->point == NULL) + continue; + if((*ftr)->point->last_published_ts_ == -1000) + continue; + (*ftr)->point->last_published_ts_ = -1000; + (*ftr)->point->pos_ = s*R*(*ftr)->point->pos_ + t; + } + } +} + +void Map::emptyTrash() +{ + std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){ + delete pt; + pt=NULL; + }); + trash_points_.clear(); + point_candidates_.emptyTrash(); +} + +MapPointCandidates::MapPointCandidates() +{} + +MapPointCandidates::~MapPointCandidates() +{ + reset(); +} + +void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2) +{ + point->type_ = Point::TYPE_CANDIDATE; + boost::unique_lock lock(mut_); + candidates_.push_back(PointCandidate(point, point->obs_.front())); +} + +void MapPointCandidates::addCandidatePointToFrame(FramePtr frame) +{ + boost::unique_lock lock(mut_); + PointCandidateList::iterator it=candidates_.begin(); + while(it != candidates_.end()) + { + if(it->first->obs_.front()->frame == frame.get()) + { + // insert feature in the frame + it->first->type_ = Point::TYPE_UNKNOWN; + it->first->n_failed_reproj_ = 0; + it->second->frame->addFeature(it->second); + it = candidates_.erase(it); + } + else + ++it; + } +} + +bool MapPointCandidates::deleteCandidatePoint(Point* point) +{ + boost::unique_lock lock(mut_); + for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it) + { + if(it->first == point) + { + deleteCandidate(*it); + candidates_.erase(it); + return true; + } + } + return false; +} + +void MapPointCandidates::removeFrameCandidates(FramePtr frame) +{ + boost::unique_lock lock(mut_); + auto it=candidates_.begin(); + while(it!=candidates_.end()) + { + if(it->second->frame == frame.get()) + { + deleteCandidate(*it); + it = candidates_.erase(it); + } + else + ++it; + } +} + +void MapPointCandidates::reset() +{ + boost::unique_lock lock(mut_); + std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){ + delete c.first; + delete c.second; + }); + candidates_.clear(); +} + +void MapPointCandidates::deleteCandidate(PointCandidate& c) +{ + // camera-rig: another frame might still be pointing to the candidate point + // therefore, we can't delete it right now. + delete c.second; c.second=NULL; + c.first->type_ = Point::TYPE_DELETED; + trash_points_.push_back(c.first); +} + +void MapPointCandidates::emptyTrash() +{ + std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){ + delete p; p=NULL; + }); + trash_points_.clear(); +} + +namespace map_debug { + +void mapValidation(Map* map, int id) +{ + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + frameValidation(it->get(), id); +} + +void frameValidation(Frame* frame, int id) +{ + for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point==NULL) + continue; + + if((*it)->point->type_ == Point::TYPE_DELETED) + printf("ERROR DataValidation %i: Referenced point was deleted.\n", id); + + if(!(*it)->point->findFrameRef(frame)) + printf("ERROR DataValidation %i: Frame has reference but point does not have a reference back.\n", id); + + pointValidation((*it)->point, id); + } + for(auto it=frame->key_pts_.begin(); it!=frame->key_pts_.end(); ++it) + if(*it != NULL) + if((*it)->point == NULL) + printf("ERROR DataValidation %i: KeyPoints not correct!\n", id); +} + +void pointValidation(Point* point, int id) +{ + for(auto it=point->obs_.begin(); it!=point->obs_.end(); ++it) + { + bool found=false; + for(auto it_ftr=(*it)->frame->fts_.begin(); it_ftr!=(*it)->frame->fts_.end(); ++it_ftr) + if((*it_ftr)->point == point) { + found=true; break; + } + if(!found) + printf("ERROR DataValidation %i: Point %i has inconsistent reference in frame %i, is candidate = %i\n", id, point->id_, (*it)->frame->id_, (int) point->type_); + } +} + +void mapStatistics(Map* map) +{ + // compute average number of features which each frame observes + size_t n_pt_obs(0); + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + n_pt_obs += (*it)->nObs(); + printf("\n\nMap Statistics: Frame avg. point obs = %f\n", (float) n_pt_obs/map->size()); + + // compute average number of observations that each point has + size_t n_frame_obs(0); + size_t n_pts(0); + std::set points; + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + { + for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr) + { + if((*ftr)->point == NULL) + continue; + if(points.insert((*ftr)->point).second) { + ++n_pts; + n_frame_obs += (*ftr)->point->nRefs(); + } + } + } + printf("Map Statistics: Point avg. frame obs = %f\n\n", (float) n_frame_obs/n_pts); +} + +} // namespace map_debug +} // namespace svo diff --git a/GPU_version/svo/src/matcher.cpp b/GPU_version/svo/src/matcher.cpp new file mode 100644 index 0000000..38a89fb --- /dev/null +++ b/GPU_version/svo/src/matcher.cpp @@ -0,0 +1,323 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +namespace warp { + +void getWarpMatrixAffine( + const vk::AbstractCamera& cam_ref, + const vk::AbstractCamera& cam_cur, + const Vector2d& px_ref, + const Vector3d& f_ref, + const double depth_ref, + const SE3& T_cur_ref, + const int level_ref, + Matrix2d& A_cur_ref) +{ + // Compute affine warp matrix A_ref_cur + const int halfpatch_size = 5; + const Vector3d xyz_ref(f_ref*depth_ref); + Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1< 3.0 && search_level < max_level) + { + search_level += 1; + D *= 0.25; + } + return search_level; +} + +void warpAffine( + const Matrix2d& A_cur_ref, + const cv::Mat& img_ref, + const Vector2d& px_ref, + const int level_ref, + const int search_level, + const int halfpatch_size, + uint8_t* patch) +{ + const int patch_size = halfpatch_size*2 ; + const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); + if(isnan(A_ref_cur(0,0))) + { + printf("Affine warp is NaN, probably camera has no translation\n"); // TODO + return; + } + + // Perform the warp on a larger patch. + uint8_t* patch_ptr = patch; + const Vector2f px_ref_pyr = px_ref.cast() / (1<=img_ref.cols-1 || px[1]>=img_ref.rows-1) + *patch_ptr = 0; + else + *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]); + } + } +} + +} // namespace warp + +bool depthFromTriangulation( + const SE3& T_search_ref, + const Vector3d& f_ref, + const Vector3d& f_cur, + double& depth) +{ + Matrix A; A << T_search_ref.rotation_matrix() * f_ref, f_cur; + const Matrix2d AtA = A.transpose()*A; + if(AtA.determinant() < 0.000001) + return false; + const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation(); + depth = fabs(depth2[0]); + return true; +} + +void Matcher::createPatchFromPatchWithBorder() +{ + uint8_t* ref_patch_ptr = patch_; + for(int y=1; yframe->cam_->isInFrame( + ref_ftr_->px.cast()/(1<level), halfpatch_size_+2, ref_ftr_->level)) + return false; + + // warp affine + warp::getWarpMatrixAffine( + *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f, + (ref_ftr_->frame->pos() - pt.pos_).norm(), + cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_); + search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); + warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, + ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_); + createPatchFromPatchWithBorder(); + + // px_cur should be set + Vector2d px_scaled(px_cur/(1<type == Feature::EDGELET) + { + Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); + dir_cur.normalize(); + success = feature_alignment::align1D( + cur_frame.img_pyr_[search_level_], dir_cur.cast(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + } + else + { + success = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + } + px_cur = px_scaled * (1<world2cam(A)); + Vector2d px_B(cur_frame.cam_->world2cam(B)); + epi_length_ = (px_A-px_B).norm() / (1<().normalized(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + else + res = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + if(res) + { + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) + return true; + } + return false; + } + + size_t n_steps = epi_length_/0.7; // one step per pixel + Vector2d step = epi_dir_/n_steps; + + if(n_steps > options_.max_epi_search_steps) + { + printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n", + n_steps, epi_length_, d_min, d_max); + return false; + } + + // for matching, precompute sum and sum2 of warped reference patch + int pixel_sum = 0; + int pixel_sum_square = 0; + PatchScore patch_score(patch_); + + // now we sample along the epipolar line + Vector2d uv = B-step; + Vector2i last_checked_pxi(0,0); + ++n_steps; + for(size_t i=0; iworld2cam(uv)); + Vector2i pxi(px[0]/(1<isInFrame(pxi, patch_size_, search_level_)) + continue; + + // TODO interpolation would probably be a good idea + uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data + + (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols + + (pxi[0]-halfpatch_size_); + int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols); + + if(zmssd < zmssd_best) { + zmssd_best = zmssd; + uv_best = uv; + } + } + + if(zmssd_best < PatchScore::threshold()) + { + if(options_.subpix_refinement) + { + px_cur_ = cur_frame.cam_->world2cam(uv_best); + Vector2d px_scaled(px_cur_/(1<().normalized(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + else + res = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + if(res) + { + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) + return true; + } + return false; + } + px_cur_ = cur_frame.cam_->world2cam(uv_best); + if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth)) + return true; + } + return false; +} + +} // namespace svo diff --git a/GPU_version/svo/src/point.cpp b/GPU_version/svo/src/point.cpp new file mode 100644 index 0000000..2e45e06 --- /dev/null +++ b/GPU_version/svo/src/point.cpp @@ -0,0 +1,179 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include + +namespace svo { + +int Point::point_counter_ = 0; + +Point::Point(const Vector3d& pos) : + id_(point_counter_++), + pos_(pos), + normal_set_(false), + n_obs_(0), + v_pt_(NULL), + last_published_ts_(0), + last_projected_kf_id_(-1), + type_(TYPE_UNKNOWN), + n_failed_reproj_(0), + n_succeeded_reproj_(0), + last_structure_optim_(0) +{} + +Point::Point(const Vector3d& pos, Feature* ftr) : + id_(point_counter_++), + pos_(pos), + normal_set_(false), + n_obs_(1), + v_pt_(NULL), + last_published_ts_(0), + last_projected_kf_id_(-1), + type_(TYPE_UNKNOWN), + n_failed_reproj_(0), + n_succeeded_reproj_(0), + last_structure_optim_(0) +{ + obs_.push_front(ftr); +} + +Point::~Point() +{} + +void Point::addFrameRef(Feature* ftr) +{ + obs_.push_front(ftr); + ++n_obs_; +} + +Feature* Point::findFrameRef(Frame* frame) +{ + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + if((*it)->frame == frame) + return *it; + return NULL; // no keyframe found +} + +bool Point::deleteFrameRef(Frame* frame) +{ + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + { + if((*it)->frame == frame) + { + obs_.erase(it); + return true; + } + } + return false; +} + +void Point::initNormal() +{ + assert(!obs_.empty()); + const Feature* ftr = obs_.back(); + assert(ftr->frame != NULL); + normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f); + normal_information_ = DiagonalMatrix(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0); + normal_set_ = true; +} + +bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const +{ + // TODO: get frame with same point of view AND same pyramid level! + Vector3d obs_dir(framepos - pos_); obs_dir.normalize(); + auto min_it=obs_.begin(); + double min_cos_angle = 0; + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + { + Vector3d dir((*it)->frame->pos() - pos_); dir.normalize(); + double cos_angle = obs_dir.dot(dir); + if(cos_angle > min_cos_angle) + { + min_cos_angle = cos_angle; + min_it = it; + } + } + ftr = *min_it; + if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless + return false; + return true; +} + +void Point::optimize(const size_t n_iter) +{ + Vector3d old_point = pos_; + double chi2 = 0.0; + Matrix3d A; + Vector3d b; + + for(size_t i=0; iframe->T_f_w_ * pos_); + Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J); + const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f)); + new_chi2 += e.squaredNorm(); + A.noalias() += J.transpose() * J; + b.noalias() -= J.transpose() * e; + } + + // solve linear system + const Vector3d dp(A.ldlt().solve(b)); + + // check if error increased + if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0])) + { +#ifdef POINT_OPTIMIZER_DEBUG + cout << "it " << i + << "\t FAILURE \t new_chi2 = " << new_chi2 << endl; +#endif + pos_ = old_point; // roll-back + break; + } + + // update the model + Vector3d new_point = pos_ + dp; + old_point = pos_; + pos_ = new_point; + chi2 = new_chi2; +#ifdef POINT_OPTIMIZER_DEBUG + cout << "it " << i + << "\t Success \t new_chi2 = " << new_chi2 + << "\t norm(b) = " << vk::norm_max(b) + << endl; +#endif + + // stop when converged + if(vk::norm_max(dp) <= EPS) + break; + } +#ifdef POINT_OPTIMIZER_DEBUG + cout << endl; +#endif +} + +} // namespace svo diff --git a/GPU_version/svo/src/pose_optimizer.cpp b/GPU_version/svo/src/pose_optimizer.cpp new file mode 100644 index 0000000..1c491b0 --- /dev/null +++ b/GPU_version/svo/src/pose_optimizer.cpp @@ -0,0 +1,165 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace pose_optimizer { + +void optimizeGaussNewton( + const double reproj_thresh, + const size_t n_iter, + const bool verbose, + FramePtr& frame, + double& estimated_scale, + double& error_init, + double& error_final, + size_t& num_obs) +{ + // init + double chi2(0.0); + vector chi2_vec_init, chi2_vec_final; + vk::robust_cost::TukeyWeightFunction weight_function; + SE3 T_old(frame->T_f_w_); + Matrix6d A; + Vector6d b; + + // compute the scale of the error for robust estimation + std::vector errors; errors.reserve(frame->fts_.size()); + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + //Reprojection error + Vector2d e = vk::project2d((*it)->f) + - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); + e *= 1.0 / (1<<(*it)->level); + errors.push_back(e.norm()); + } + if(errors.empty()) + return; + vk::robust_cost::MADScaleEstimator scale_estimator; + estimated_scale = scale_estimator.compute(errors); + + num_obs = errors.size(); + chi2_vec_init.reserve(num_obs); + chi2_vec_final.reserve(num_obs); + double scale = estimated_scale; + for(size_t iter=0; itercam_->errorMultiplier2(); + + b.setZero(); + A.setZero(); + double new_chi2(0.0); + + // compute residual + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Matrix26d J; + Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_); + Frame::jacobian_xyz2uv(xyz_f, J); + Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f); + double sqrt_inv_cov = 1.0 / (1<<(*it)->level); + e *= sqrt_inv_cov; + if(iter == 0) + chi2_vec_init.push_back(e.squaredNorm()); // just for debug + J *= sqrt_inv_cov; + double weight = weight_function.value(e.norm()/scale); + A.noalias() += J.transpose()*J*weight; + b.noalias() -= J.transpose()*e*weight; + new_chi2 += e.squaredNorm()*weight; + } + + // solve linear system + const Vector6d dT(A.ldlt().solve(b)); + + // check if error increased + if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0])) + { + if(verbose) + std::cout << "it " << iter + << "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl; + frame->T_f_w_ = T_old; // roll-back + break; + } + + // update the model + SE3 T_new = SE3::exp(dT)*frame->T_f_w_; + T_old = frame->T_f_w_; + frame->T_f_w_ = T_new; + chi2 = new_chi2; + if(verbose) + std::cout << "it " << iter + << "\t Success \t new_chi2 = " << new_chi2 + << "\t norm(dT) = " << vk::norm_max(dT) << std::endl; + + // stop when converged + if(vk::norm_max(dT) <= EPS) + break; + } + + // Set covariance as inverse information matrix. Optimistic estimator! + const double pixel_variance=1.0; + frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse(); + + // Remove Measurements with too large reprojection error + double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2(); + size_t n_deleted_refs = 0; + for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); + double sqrt_inv_cov = 1.0 / (1<<(*it)->level); + e *= sqrt_inv_cov; + chi2_vec_final.push_back(e.squaredNorm()); + if(e.norm() > reproj_thresh_scaled) + { + // we don't need to delete a reference in the point since it was not created yet + (*it)->point = NULL; + ++n_deleted_refs; + } + } + + error_init=0.0; + error_final=0.0; + if(!chi2_vec_init.empty()) + error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2(); + if(!chi2_vec_final.empty()) + error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2(); + + estimated_scale *= frame->cam_->errorMultiplier2(); + if(verbose) + std::cout << "n deleted obs = " << n_deleted_refs + << "\t scale = " << estimated_scale + << "\t error init = " << error_init + << "\t error end = " << error_final << std::endl; + num_obs -= n_deleted_refs; +} + +} // namespace pose_optimizer +} // namespace svo diff --git a/GPU_version/svo/src/reprojector.cpp b/GPU_version/svo/src/reprojector.cpp new file mode 100644 index 0000000..81a7835 --- /dev/null +++ b/GPU_version/svo/src/reprojector.cpp @@ -0,0 +1,219 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) : + map_(map) +{ + initializeGrid(cam); +} + +Reprojector::~Reprojector() +{ + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; }); +} + +void Reprojector::initializeGrid(vk::AbstractCamera* cam) +{ + grid_.cell_size = Config::gridSize(); + grid_.grid_n_cols = ceil(static_cast(cam->width())/grid_.cell_size); + grid_.grid_n_rows = ceil(static_cast(cam->height())/grid_.cell_size); + grid_.cells.resize(grid_.grid_n_cols*grid_.grid_n_rows); + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell*& c){ c = new Cell; }); + grid_.cell_order.resize(grid_.cells.size()); + for(size_t i=0; iclear(); }); +} + +void Reprojector::reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs) +{ + resetGrid(); + + // Identify those Keyframes which share a common field of view. + SVO_START_TIMER("reproject_kfs"); + list< pair > close_kfs; + map_.getCloseKeyframes(frame, close_kfs); + + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + // Reproject all mappoints of the closest N kfs with overlap. We only store + // in which grid cell the points fall. + size_t n = 0; + overlap_kfs.reserve(options_.max_n_kfs); + for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); + it_frame!=ite_frame && nfirst; + overlap_kfs.push_back(pair(ref_frame,0)); + + // Try to reproject each mappoint that the other KF observes + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); + it_ftr!=ite_ftr; ++it_ftr) + { + // check if the feature has a mappoint assigned + if((*it_ftr)->point == NULL) + continue; + + // make sure we project a point only once + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + continue; + (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + if(reprojectPoint(frame, (*it_ftr)->point)) + overlap_kfs.back().second++; + } + } + SVO_STOP_TIMER("reproject_kfs"); + + // Now project all point candidates + SVO_START_TIMER("reproject_candidates"); + { + boost::unique_lock lock(map_.point_candidates_.mut_); + auto it=map_.point_candidates_.candidates_.begin(); + while(it!=map_.point_candidates_.candidates_.end()) + { + if(!reprojectPoint(frame, it->first)) + { + it->first->n_failed_reproj_ += 3; + if(it->first->n_failed_reproj_ > 30) + { + map_.point_candidates_.deleteCandidate(*it); + it = map_.point_candidates_.candidates_.erase(it); + continue; + } + } + ++it; + } + } // unlock the mutex when out of scope + SVO_STOP_TIMER("reproject_candidates"); + + // Now we go through each grid cell and select one point to match. + // At the end, we should have at maximum one reprojected point per cell. + SVO_START_TIMER("feature_align"); + for(size_t i=0; i (size_t) Config::maxFts()) + break; + } + SVO_STOP_TIMER("feature_align"); +} + +bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs) +{ + if(lhs.pt->type_ > rhs.pt->type_) + return true; + return false; +} + +bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) +{ + cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2)); + Cell::iterator it=cell.begin(); + while(it!=cell.end()) + { + ++n_trials_; + + if(it->pt->type_ == Point::TYPE_DELETED) + { + it = cell.erase(it); + continue; + } + + bool found_match = true; + if(options_.find_match_direct) + found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); + if(!found_match) + { + it->pt->n_failed_reproj_++; + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) + map_.safeDeletePoint(it->pt); + if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30) + map_.point_candidates_.deleteCandidatePoint(it->pt); + it = cell.erase(it); + continue; + } + it->pt->n_succeeded_reproj_++; + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) + it->pt->type_ = Point::TYPE_GOOD; + + Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_); + frame->addFeature(new_feature); + + // Here we add a reference in the feature to the 3D point, the other way + // round is only done if this frame is selected as keyframe. + new_feature->point = it->pt; + + if(matcher_.ref_ftr_->type == Feature::EDGELET) + { + new_feature->type = Feature::EDGELET; + new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad; + new_feature->grad.normalize(); + } + + // If the keyframe is selected and we reproject the rest, we don't have to + // check this point anymore. + it = cell.erase(it); + + // Maximum one point per cell. + return true; + } + return false; +} + +bool Reprojector::reprojectPoint(FramePtr frame, Point* point) +{ + Vector2d px(frame->w2c(point->pos_)); + if(frame->cam_->isInFrame(px.cast(), 8)) // 8px is the patch size in the matcher + { + const int k = static_cast(px[1]/grid_.cell_size)*grid_.grid_n_cols + + static_cast(px[0]/grid_.cell_size); + grid_.cells.at(k)->push_back(Candidate(point, px)); + return true; + } + return false; +} + +} // namespace svo diff --git a/GPU_version/svo/src/sparse_img_align.cpp b/GPU_version/svo/src/sparse_img_align.cpp new file mode 100644 index 0000000..ce1570b --- /dev/null +++ b/GPU_version/svo/src/sparse_img_align.cpp @@ -0,0 +1,274 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +SparseImgAlign::SparseImgAlign( + int max_level, int min_level, int n_iter, + Method method, bool display, bool verbose) : + display_(display), + max_level_(max_level), + min_level_(min_level) +{ + n_iter_ = n_iter; + n_iter_init_ = n_iter_; + method_ = method; + verbose_ = verbose; + eps_ = 0.000001; +} + +size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) +{ + reset(); + + if(ref_frame->fts_.empty()) + { + SVO_WARN_STREAM("SparseImgAlign: no features to track!"); + return 0; + } + + ref_frame_ = ref_frame; + cur_frame_ = cur_frame; + ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F); + jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_); + visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? + + SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse()); + + for(level_=max_level_; level_>=min_level_; --level_) + { + mu_ = 0.1; + jacobian_cache_.setZero(); + have_ref_patch_cache_ = false; + if(verbose_) + printf("\nPYRAMID LEVEL %i\n---------------\n", level_); + optimize(T_cur_from_ref); + } + cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_; + + + + return n_meas_/patch_area_; +} + +Matrix SparseImgAlign::getFisherInformation() +{ + double sigma_i_sq = 5e-4*255*255; // image noise + Matrix I = H_/sigma_i_sq; + return I; +} + +void SparseImgAlign::precomputeReferencePatches() +{ + const int border = patch_halfsize_+1; + const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_); + const int stride = ref_img.cols; + const float scale = 1.0f/(1<pos(); + const double focal_length = ref_frame_->cam_->errorMultiplier2(); + size_t feature_counter = 0; + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(), ite=ref_frame_->fts_.end(); + it!=ite; ++it, ++feature_counter, ++visiblity_it) + { + // check if reference with patch size is within image + const float u_ref = (*it)->px[0]*scale; + const float v_ref = (*it)->px[1]*scale; + const int u_ref_i = floorf(u_ref); + const int v_ref_i = floorf(v_ref); + if((*it)->point == NULL || u_ref_i-border < 0 || v_ref_i-border < 0 || u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows) + continue; + *visiblity_it = true; + + // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!! + const double depth(((*it)->point->pos_ - ref_pos).norm()); + const Vector3d xyz_ref((*it)->f*depth); + + // evaluate projection jacobian + Matrix frame_jac; + Frame::jacobian_xyz2uv(xyz_ref, frame_jac); + + // compute bilateral interpolation weights for reference image + const float subpix_u_ref = u_ref-u_ref_i; + const float subpix_v_ref = v_ref-v_ref_i; + const float w_ref_tl = (1.0-subpix_u_ref) * (1.0-subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0-subpix_v_ref); + const float w_ref_bl = (1.0-subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + size_t pixel_counter = 0; + float* cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + for(int y=0; yimg_pyr_.at(level_); + + if(linearize_system && display_) + resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0)); + + if(have_ref_patch_cache_ == false) + precomputeReferencePatches(); + + // compute the weights on the first iteration + std::vector errors; + if(compute_weight_scale) + errors.reserve(visible_fts_.size()); + const int stride = cur_img.cols; + const int border = patch_halfsize_+1; + const float scale = 1.0f/(1<pos()); + float chi2 = 0.0; + size_t feature_counter = 0; // is used to compute the index of the cached jacobian + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(); it!=ref_frame_->fts_.end(); + ++it, ++feature_counter, ++visiblity_it) + { + // check if feature is within image + if(!*visiblity_it) + continue; + + // compute pixel location in cur img + const double depth = ((*it)->point->pos_ - ref_pos).norm(); + const Vector3d xyz_ref((*it)->f*depth); + const Vector3d xyz_cur(T_cur_from_ref * xyz_ref); + const Vector2f uv_cur_pyr(cur_frame_->cam_->world2cam(xyz_cur).cast() * scale); + const float u_cur = uv_cur_pyr[0]; + const float v_cur = uv_cur_pyr[1]; + const int u_cur_i = floorf(u_cur); + const int v_cur_i = floorf(v_cur); + + // check if projection is within the image + if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i-border < 0 || v_cur_i-border < 0 || u_cur_i+border >= cur_img.cols || v_cur_i+border >= cur_img.rows) + continue; + + // compute bilateral interpolation weights for the current image + const float subpix_u_cur = u_cur-u_cur_i; + const float subpix_v_cur = v_cur-v_cur_i; + const float w_cur_tl = (1.0-subpix_u_cur) * (1.0-subpix_v_cur); + const float w_cur_tr = subpix_u_cur * (1.0-subpix_v_cur); + const float w_cur_bl = (1.0-subpix_u_cur) * subpix_v_cur; + const float w_cur_br = subpix_u_cur * subpix_v_cur; + float* ref_patch_cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + size_t pixel_counter = 0; // is used to compute the index of the cached jacobian + for(int y=0; yvalue(res/scale_); + } + + chi2 += res*res*weight; + n_meas_++; + + if(linearize_system) + { + // compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error) + const Vector6d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter)); + H_.noalias() += J*J.transpose()*weight; + Jres_.noalias() -= J*res*weight; + if(display_) + resimg_.at((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0; + } + } + } + } + + // compute the weights on the first iteration + if(compute_weight_scale && iter_ == 0) + scale_ = scale_estimator_->compute(errors); + + return chi2/n_meas_; +} + +int SparseImgAlign::solve() +{ + x_ = H_.ldlt().solve(Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void SparseImgAlign::update( + const ModelType& T_curold_from_ref, + ModelType& T_curnew_from_ref) +{ + T_curnew_from_ref = T_curold_from_ref * SE3::exp(-x_); +} + +void SparseImgAlign::startIteration() +{} + +void SparseImgAlign::finishIteration() +{ + if(display_) + { + cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE); + cv::imshow("residuals", resimg_*10); + cv::waitKey(0); + } +} + +} // namespace svo + diff --git a/GPU_version/svo/test/README.md b/GPU_version/svo/test/README.md new file mode 100644 index 0000000..c70f832 --- /dev/null +++ b/GPU_version/svo/test/README.md @@ -0,0 +1,24 @@ +#### Download Test Data + +Create a folder where you will store all datasets, e.g.: + + mkdir Datasets + +In your `.bashrc` script, create a new environment variable pointing to the dataset folder: + + export SVO_DATASET_DIR=${HOME}/Datasets + +Source your new `.bashrc` script, go to the new dataset folder and download the test data: + + source ~/.bashrc + cd ${SVO_DATASET_DIR} + wget http://rpg.ifi.uzh.ch/datasets/sin2_tex2_h1_v8_d.tar.gz -O - | tar -xz + +You can then run the tests, e.g.: + + rosrun svo test_pipeline + +or if you are not using ROS: + + cd svo/bin + ./test_pipeline diff --git a/GPU_version/svo/test/benchmark.csv b/GPU_version/svo/test/benchmark.csv new file mode 100644 index 0000000..a3831e4 --- /dev/null +++ b/GPU_version/svo/test/benchmark.csv @@ -0,0 +1,307 @@ +local_ba,point_optimizer,pose_optimizer,pyramid_creation,reproject,sparse_img_align,tot_time,dropout,img_align_n_tracked,loba_err_fin,loba_err_init,loba_n_erredges_fin,loba_n_erredges_init,n_candidates,repr_n_mps,repr_n_new_references,sfba_error_final,sfba_error_init,sfba_n_edges_final,sfba_thresh +0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.005536000000000,1.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000326000000000,0.000000000000000,0.000000000000000,0.016564000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000189000000000,0.000000000000000,0.000000000000000,0.016754000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000133000000000,0.000000000000000,0.000000000000000,0.017082000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000136000000000,0.000000000000000,0.000000000000000,0.023259000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000120000000000,0.000000000000000,0.000000000000000,0.029114000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.032538000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000123000000000,0.000000000000000,0.000000000000000,0.031375000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000137000000000,0.000000000000000,0.000000000000000,0.029995000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000119000000000,0.000000000000000,0.000000000000000,0.030888000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000461000000000,0.000000000000000,0.000000000000000,0.036835000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000159000000000,0.000000000000000,0.000000000000000,0.090994000000000,1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000036000000000,0.000143000000000,0.000117000000000,0.000836000000000,0.001616000000000,0.002862000000000,0.000000000000000,279.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,148.000000000000000,121.000000000000000,0.170175036288445,0.209622100274678,121.000000000000000,0.310240723182540 +0.000000000000000,0.000025000000000,0.000180000000000,0.000123000000000,0.000817000000000,0.000802000000000,0.001981000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,151.000000000000000,121.000000000000000,0.200086249066279,0.226929968835210,119.000000000000000,0.335856340084193 +0.000000000000000,0.000043000000000,0.000247000000000,0.000202000000000,0.000836000000000,0.000882000000000,0.002289000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,150.000000000000000,121.000000000000000,0.220021653137765,0.250579787207819,120.000000000000000,0.370858080741496 +0.000000000000000,0.000027000000000,0.000173000000000,0.000125000000000,0.000796000000000,0.000863000000000,0.002018000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,153.000000000000000,121.000000000000000,0.241933923583207,0.315090761433966,119.000000000000000,0.466334367626374 +0.000000000000000,0.000025000000000,0.000171000000000,0.000123000000000,0.000752000000000,0.000893000000000,0.002012000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,145.000000000000000,121.000000000000000,0.272951476803891,0.278442112643762,118.000000000000000,0.412094340922640 +0.000000000000000,0.000058000000000,0.000180000000000,0.000213000000000,0.001057000000000,0.000834000000000,0.002415000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,176.000000000000000,121.000000000000000,0.295685268713144,0.345204254249458,120.000000000000000,0.510902319377397 +0.000000000000000,0.000029000000000,0.000104000000000,0.000125000000000,0.000868000000000,0.000845000000000,0.002006000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,154.000000000000000,121.000000000000000,0.280726297104857,0.351937246921774,119.000000000000000,0.520867118388044 +0.000000000000000,0.000030000000000,0.000173000000000,0.000117000000000,0.000872000000000,0.000829000000000,0.002054000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,160.000000000000000,121.000000000000000,0.299712702632539,0.325868379793256,119.000000000000000,0.482285213145440 +0.000000000000000,0.000030000000000,0.000135000000000,0.000312000000000,0.001611000000000,0.000955000000000,0.003094000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,157.000000000000000,121.000000000000000,0.266414418920375,0.325723952270968,121.000000000000000,0.482071463677943 +0.000000000000000,0.000019000000000,0.000134000000000,0.000120000000000,0.000807000000000,0.000746000000000,0.001858000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,52.000000000000000,146.000000000000000,121.000000000000000,0.268513470166789,0.275002624777888,120.000000000000000,0.407003878905728 +0.000000000000000,0.000023000000000,0.000116000000000,0.000137000000000,0.000918000000000,0.000845000000000,0.002086000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,76.000000000000000,141.000000000000000,121.000000000000000,0.291779184224341,0.283057831418736,119.000000000000000,0.418925579815900 +0.000000000000000,0.000020000000000,0.000174000000000,0.000139000000000,0.000841000000000,0.000842000000000,0.002050000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,97.000000000000000,146.000000000000000,121.000000000000000,0.284752630550729,0.313033048850307,118.000000000000000,0.463288905244113 +0.000000000000000,0.000018000000000,0.000125000000000,0.000116000000000,0.000918000000000,0.000737000000000,0.001957000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,108.000000000000000,164.000000000000000,121.000000000000000,0.266984542461140,0.310809088449218,117.000000000000000,0.459997446489587 +0.000000000000000,0.000025000000000,0.000187000000000,0.000215000000000,0.001123000000000,0.000838000000000,0.002412000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,172.000000000000000,121.000000000000000,0.281472742741066,0.318106456171594,118.000000000000000,0.470797581856187 +0.000000000000000,0.000032000000000,0.000104000000000,0.000133000000000,0.000982000000000,0.001074000000000,0.002360000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,169.000000000000000,121.000000000000000,0.259578544711984,0.296051216412790,118.000000000000000,0.438155829408007 +0.000000000000000,0.000026000000000,0.000173000000000,0.000114000000000,0.000975000000000,0.000678000000000,0.001991000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,87.000000000000000,169.000000000000000,121.000000000000000,0.260854021690335,0.463271429083291,119.000000000000000,0.685641725628487 +0.000000000000000,0.000020000000000,0.000231000000000,0.000213000000000,0.001255000000000,0.000764000000000,0.002547000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,100.000000000000000,162.000000000000000,121.000000000000000,0.224857460689169,0.241819176684415,120.000000000000000,0.357892398925400 +0.000000000000000,0.000016000000000,0.000174000000000,0.000123000000000,0.000964000000000,0.000761000000000,0.002072000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,177.000000000000000,163.000000000000000,121.000000000000000,0.232802572049831,0.307670939545318,121.000000000000000,0.455352982415734 +0.000000000000000,0.000019000000000,0.000171000000000,0.000117000000000,0.000954000000000,0.000685000000000,0.001989000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,215.000000000000000,163.000000000000000,121.000000000000000,0.223503314093231,0.245376537152124,121.000000000000000,0.363157262109869 +0.000000000000000,0.000022000000000,0.000153000000000,0.000321000000000,0.001789000000000,0.000999000000000,0.003323000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,236.000000000000000,160.000000000000000,121.000000000000000,0.238679756461930,0.260245176933572,121.000000000000000,0.385162868349003 +0.000000000000000,0.000017000000000,0.000103000000000,0.000121000000000,0.000950000000000,0.000690000000000,0.001925000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,253.000000000000000,156.000000000000000,121.000000000000000,0.240028083468930,0.347317483304434,119.000000000000000,0.514029864789977 +0.000000000000000,0.000026000000000,0.000155000000000,0.000141000000000,0.001421000000000,0.000834000000000,0.002605000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,225.000000000000000,203.000000000000000,121.000000000000000,0.237050818956117,0.342187690832661,118.000000000000000,0.506437791010399 +0.000000000000000,0.000031000000000,0.000105000000000,0.000133000000000,0.001151000000000,0.000787000000000,0.002244000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,224.000000000000000,192.000000000000000,121.000000000000000,0.228256287503905,0.255917399607126,118.000000000000000,0.378757763709815 +0.000000000000000,0.000029000000000,0.000173000000000,0.000116000000000,0.001046000000000,0.000670000000000,0.002067000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,237.000000000000000,174.000000000000000,121.000000000000000,0.253024524323249,0.270830988595337,117.000000000000000,0.400829860236131 +0.000000000000000,0.000032000000000,0.000206000000000,0.000201000000000,0.001159000000000,0.000767000000000,0.002415000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,243.000000000000000,168.000000000000000,121.000000000000000,0.255837803512081,0.309767343777557,117.000000000000000,0.458455710853014 +0.000000000000000,0.000029000000000,0.000102000000000,0.000134000000000,0.001078000000000,0.000785000000000,0.002160000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,247.000000000000000,176.000000000000000,121.000000000000000,0.242039161090674,0.295753112981567,120.000000000000000,0.437714633120047 +0.000000000000000,0.000018000000000,0.000174000000000,0.000117000000000,0.001043000000000,0.000946000000000,0.002334000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,284.000000000000000,173.000000000000000,121.000000000000000,0.234035474669157,0.262962395172133,120.000000000000000,0.389184330309739 +0.000000000000000,0.000042000000000,0.000251000000000,0.000203000000000,0.001816000000000,0.000807000000000,0.003189000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,323.000000000000000,168.000000000000000,121.000000000000000,0.243963461663248,0.254676688230514,118.000000000000000,0.376921484440742 +0.000000000000000,0.000022000000000,0.000128000000000,0.000121000000000,0.001084000000000,0.000899000000000,0.002287000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,346.000000000000000,164.000000000000000,121.000000000000000,0.237498671286056,0.242318617577444,120.000000000000000,0.358631550548166 +0.000000000000000,0.000042000000000,0.000198000000000,0.000115000000000,0.001158000000000,0.000720000000000,0.002269000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,174.000000000000000,121.000000000000000,0.231204372046851,0.310626801011824,119.000000000000000,0.459727694560518 +0.000000000000000,0.000020000000000,0.000128000000000,0.000280000000000,0.000998000000000,0.000883000000000,0.002349000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,371.000000000000000,154.000000000000000,121.000000000000000,0.226372262196515,0.240389813013775,118.000000000000000,0.355776916048274 +0.000000000000000,0.000020000000000,0.000125000000000,0.000116000000000,0.001040000000000,0.000676000000000,0.002025000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,383.000000000000000,163.000000000000000,121.000000000000000,0.243430360339485,0.289701992216838,116.000000000000000,0.428758939836188 +0.000000000000000,0.000028000000000,0.000186000000000,0.000201000000000,0.001357000000000,0.000862000000000,0.002663000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,189.000000000000000,121.000000000000000,0.238491256129826,0.289513389727138,119.000000000000000,0.428479837315611 +0.000000000000000,0.000024000000000,0.000104000000000,0.000131000000000,0.001197000000000,0.000689000000000,0.002178000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,187.000000000000000,121.000000000000000,0.253535521704090,0.299150905814168,119.000000000000000,0.442743330689269 +0.000000000000000,0.000013000000000,0.000128000000000,0.000142000000000,0.001157000000000,0.000751000000000,0.002226000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,182.000000000000000,121.000000000000000,0.242747326189240,0.331002657945821,119.000000000000000,0.489883908154680 +0.000000000000000,0.000041000000000,0.000225000000000,0.000214000000000,0.001614000000000,0.000892000000000,0.003073000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,420.000000000000000,166.000000000000000,121.000000000000000,0.214060199250618,0.291833988602564,120.000000000000000,0.431914284304601 +0.000000000000000,0.000016000000000,0.000113000000000,0.000126000000000,0.001256000000000,0.000871000000000,0.002419000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,467.000000000000000,189.000000000000000,121.000000000000000,0.191542639509251,0.219467558857437,120.000000000000000,0.324811977376153 +0.000000000000000,0.000022000000000,0.000110000000000,0.000116000000000,0.001163000000000,0.000737000000000,0.002178000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,491.000000000000000,176.000000000000000,121.000000000000000,0.237287779136815,0.290385733638894,119.000000000000000,0.429770876012296 +0.000000000000000,0.000016000000000,0.000108000000000,0.000532000000000,0.001507000000000,0.001176000000000,0.003390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,503.000000000000000,195.000000000000000,121.000000000000000,0.255418654779892,0.271366103996528,119.000000000000000,0.401621830064983 +0.000000000000000,0.000023000000000,0.000128000000000,0.000118000000000,0.001247000000000,0.000704000000000,0.002255000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,519.000000000000000,192.000000000000000,121.000000000000000,0.243354932726649,0.322133765000893,118.000000000000000,0.476758002689501 +0.000000000000000,0.000028000000000,0.000117000000000,0.000194000000000,0.001487000000000,0.000763000000000,0.002640000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,537.000000000000000,182.000000000000000,121.000000000000000,0.256839812012575,0.290001379744665,120.000000000000000,0.429202006242450 +0.000000000000000,0.000021000000000,0.000192000000000,0.000135000000000,0.001268000000000,0.000737000000000,0.002390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,551.000000000000000,186.000000000000000,121.000000000000000,0.234921062453291,0.278307687859489,121.000000000000000,0.411895400770342 +0.000000000000000,0.000023000000000,0.000110000000000,0.000128000000000,0.001205000000000,0.000853000000000,0.002375000000000,1.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,566.000000000000000,182.000000000000000,121.000000000000000,0.236213436253286,0.243545081705577,120.000000000000000,0.360446727806674 +0.000000000000000,0.000033000000000,0.000148000000000,0.000221000000000,0.001810000000000,0.000752000000000,0.003004000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,194.000000000000000,121.000000000000000,0.256770574347429,0.281629144927868,120.000000000000000,0.416811158357270 +0.000000000000000,0.000018000000000,0.000107000000000,0.000166000000000,0.001283000000000,0.000754000000000,0.002364000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,193.000000000000000,121.000000000000000,0.238569415669078,0.245006057597332,120.000000000000000,0.362608988913209 +0.000000000000000,0.000026000000000,0.000199000000000,0.000126000000000,0.001415000000000,0.000739000000000,0.002538000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,525.000000000000000,207.000000000000000,121.000000000000000,0.272522903927536,0.292694935292295,120.000000000000000,0.433188542480310 +0.000000000000000,0.000024000000000,0.000152000000000,0.000385000000000,0.001329000000000,0.000772000000000,0.002715000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,532.000000000000000,193.000000000000000,121.000000000000000,0.300157389608362,0.352279544787609,121.000000000000000,0.521373718272822 +0.000000000000000,0.000025000000000,0.000103000000000,0.000124000000000,0.001300000000000,0.000765000000000,0.002352000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,542.000000000000000,192.000000000000000,121.000000000000000,0.262925192895899,0.296136504354747,121.000000000000000,0.438282037121495 +0.000000000000000,0.000027000000000,0.000159000000000,0.000227000000000,0.001454000000000,0.000792000000000,0.002702000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,565.000000000000000,171.000000000000000,121.000000000000000,0.281949830225302,0.308467076957178,121.000000000000000,0.456531258033208 +0.000000000000000,0.000027000000000,0.000177000000000,0.000130000000000,0.001440000000000,0.000817000000000,0.002620000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,608.000000000000000,201.000000000000000,121.000000000000000,0.262127192155801,0.344178587739890,120.000000000000000,0.509384339297628 +0.000000000000000,0.000031000000000,0.000173000000000,0.000122000000000,0.001320000000000,0.000744000000000,0.002429000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,636.000000000000000,196.000000000000000,121.000000000000000,0.243118415564597,0.307364154563702,120.000000000000000,0.454898948018352 +0.000000000000000,0.000031000000000,0.000237000000000,0.000391000000000,0.002601000000000,0.000982000000000,0.004294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,663.000000000000000,198.000000000000000,121.000000000000000,0.308234194177187,0.324201216218454,120.000000000000000,0.479817768949582 +0.000000000000000,0.000016000000000,0.000102000000000,0.000118000000000,0.001370000000000,0.000653000000000,0.002310000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,687.000000000000000,198.000000000000000,121.000000000000000,0.262171425081880,0.309537417467615,120.000000000000000,0.458115349322126 +0.000000000000000,0.000024000000000,0.000120000000000,0.000223000000000,0.001806000000000,0.000776000000000,0.002976000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,229.000000000000000,121.000000000000000,0.297772053023002,0.357203726783215,120.000000000000000,0.528661518750357 +0.000000000000000,0.000022000000000,0.000131000000000,0.000225000000000,0.001322000000000,0.000818000000000,0.002549000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,195.000000000000000,121.000000000000000,0.273836774721146,0.292258185663714,119.000000000000000,0.432542138616555 +0.000000000000000,0.000028000000000,0.000189000000000,0.000113000000000,0.001354000000000,0.000656000000000,0.002369000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,668.000000000000000,208.000000000000000,121.000000000000000,0.290291300745150,0.351263582277828,120.000000000000000,0.519870092614610 +0.000000000000000,0.000020000000000,0.000106000000000,0.000362000000000,0.002527000000000,0.000881000000000,0.003941000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,682.000000000000000,211.000000000000000,121.000000000000000,0.258959916330166,0.286061529228379,120.000000000000000,0.423371078465586 +0.000000000000000,0.000023000000000,0.000183000000000,0.000117000000000,0.001452000000000,0.000716000000000,0.002520000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,696.000000000000000,219.000000000000000,121.000000000000000,0.299843168031565,0.306288599781015,118.000000000000000,0.453307123537539 +0.000000000000000,0.000024000000000,0.000190000000000,0.000229000000000,0.001590000000000,0.000704000000000,0.002778000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,704.000000000000000,201.000000000000000,121.000000000000000,0.274895969986079,0.339774469390948,119.000000000000000,0.502866218860539 +0.000000000000000,0.000032000000000,0.000184000000000,0.000127000000000,0.001388000000000,0.000658000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,712.000000000000000,205.000000000000000,121.000000000000000,0.276261011279968,0.281429264300083,120.000000000000000,0.416515325859454 +0.000000000000000,0.000019000000000,0.000103000000000,0.000122000000000,0.001302000000000,0.000759000000000,0.002334000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,743.000000000000000,194.000000000000000,121.000000000000000,0.308426080941849,0.293337004748022,118.000000000000000,0.434138764752467 +0.000000000000000,0.000020000000000,0.000188000000000,0.000336000000000,0.002590000000000,0.000953000000000,0.004130000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,769.000000000000000,216.000000000000000,121.000000000000000,0.273447755201539,0.322902569141660,119.000000000000000,0.477895792772933 +0.000000000000000,0.000019000000000,0.000104000000000,0.000118000000000,0.001466000000000,0.000743000000000,0.002480000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,791.000000000000000,204.000000000000000,121.000000000000000,0.288081750238067,0.251959938318303,120.000000000000000,0.372900704820467 +0.000000000000000,0.000024000000000,0.000118000000000,0.000231000000000,0.001575000000000,0.000986000000000,0.003013000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,190.000000000000000,121.000000000000000,0.282457846343284,0.276454604943847,117.000000000000000,0.409152821737331 +0.000000000000000,0.000016000000000,0.000210000000000,0.000129000000000,0.001436000000000,0.000881000000000,0.002693000000000,0.000000000000000,111.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,208.000000000000000,121.000000000000000,0.294182859046983,0.317841061067694,121.000000000000000,0.470404755925379 +0.000000000000000,0.000015000000000,0.000142000000000,0.000184000000000,0.001424000000000,0.000955000000000,0.002755000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,194.000000000000000,121.000000000000000,0.243748783908456,0.304183797141307,119.000000000000000,0.450192011884479 +0.000000000000000,0.000014000000000,0.000107000000000,0.000318000000000,0.001624000000000,0.000893000000000,0.002992000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,220.000000000000000,121.000000000000000,0.265012454662619,0.270354981784829,119.000000000000000,0.400125381617550 +0.000000000000000,0.000023000000000,0.000213000000000,0.000128000000000,0.001340000000000,0.000949000000000,0.002681000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,811.000000000000000,196.000000000000000,121.000000000000000,0.253755971722852,0.269982133879470,119.000000000000000,0.399573570359238 +0.000000000000000,0.000038000000000,0.000249000000000,0.000219000000000,0.001747000000000,0.000847000000000,0.003214000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,217.000000000000000,121.000000000000000,0.242158110541114,0.277566291448718,118.000000000000000,0.410798121492822 +0.000000000000000,0.000022000000000,0.000200000000000,0.000131000000000,0.001465000000000,0.000761000000000,0.002608000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,826.000000000000000,208.000000000000000,121.000000000000000,0.247025005586580,0.299927107710462,119.000000000000000,0.443892088763821 +0.000000000000000,0.000028000000000,0.000129000000000,0.000115000000000,0.001507000000000,0.000767000000000,0.002582000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,835.000000000000000,206.000000000000000,121.000000000000000,0.251694282278063,0.262688495423133,119.000000000000000,0.388778969531936 +0.000000000000000,0.000018000000000,0.000176000000000,0.000303000000000,0.001569000000000,0.000763000000000,0.002865000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,858.000000000000000,208.000000000000000,121.000000000000000,0.204259521447942,0.239471125876921,121.000000000000000,0.354417264227415 +0.000000000000000,0.000027000000000,0.000197000000000,0.000119000000000,0.001520000000000,0.000717000000000,0.002613000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,182.000000000000000,121.000000000000000,0.288688133937508,0.321107040561571,121.000000000000000,0.475238455753857 +0.000000000000000,0.000074000000000,0.000308000000000,0.000231000000000,0.002302000000000,0.000817000000000,0.004009000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,895.000000000000000,169.000000000000000,121.000000000000000,0.226466756684178,0.283662806855079,120.000000000000000,0.419820962132456 +0.000000000000000,0.000032000000000,0.000120000000000,0.000146000000000,0.001779000000000,0.000719000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,201.000000000000000,121.000000000000000,0.248964133477333,0.301424345755280,121.000000000000000,0.446108027319984 +0.000000000000000,0.000031000000000,0.000209000000000,0.000347000000000,0.002379000000000,0.001268000000000,0.004286000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,881.000000000000000,197.000000000000000,121.000000000000000,0.197089987231914,0.215315450958625,121.000000000000000,0.318666844452755 +0.000000000000000,0.000022000000000,0.000183000000000,0.000138000000000,0.001523000000000,0.000904000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,889.000000000000000,208.000000000000000,121.000000000000000,0.211913610123421,0.217730864556872,121.000000000000000,0.322241701945577 +0.000000000000000,0.000023000000000,0.000188000000000,0.000135000000000,0.001539000000000,0.001010000000000,0.002927000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,191.000000000000000,121.000000000000000,0.223781553150169,0.264534665335954,119.000000000000000,0.391511288186158 +0.000000000000000,0.000024000000000,0.000111000000000,0.000561000000000,0.002319000000000,0.001083000000000,0.004153000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,959.000000000000000,191.000000000000000,121.000000000000000,0.192480342616366,0.242812113218322,120.000000000000000,0.359361932832411 +0.000000000000000,0.000023000000000,0.000113000000000,0.000129000000000,0.001473000000000,0.000780000000000,0.002555000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1010.000000000000000,186.000000000000000,121.000000000000000,0.226239188570666,0.246580462159093,121.000000000000000,0.364939080501376 +0.000000000000000,0.000025000000000,0.000147000000000,0.000235000000000,0.001877000000000,0.001081000000000,0.003415000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1042.000000000000000,191.000000000000000,121.000000000000000,0.213010810373553,0.226959616128893,121.000000000000000,0.335900212048673 +0.000000000000000,0.000019000000000,0.000180000000000,0.000123000000000,0.001581000000000,0.000797000000000,0.002856000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1066.000000000000000,197.000000000000000,121.000000000000000,0.236500218717681,0.308339991149568,118.000000000000000,0.456343184784554 +0.000000000000000,0.000020000000000,0.000176000000000,0.000124000000000,0.001670000000000,0.001020000000000,0.003039000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1045.000000000000000,222.000000000000000,121.000000000000000,0.203403550946325,0.322649020817046,119.000000000000000,0.477520556062907 +0.000000000000000,0.000021000000000,0.000102000000000,0.000120000000000,0.001506000000000,0.000747000000000,0.002537000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,208.000000000000000,121.000000000000000,0.217457541731394,0.242874100604298,119.000000000000000,0.359453694988533 +0.000000000000000,0.000014000000000,0.000102000000000,0.000114000000000,0.001438000000000,0.000775000000000,0.002481000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,202.000000000000000,121.000000000000000,0.237066555963892,0.363698547046353,120.000000000000000,0.538273825733024 +0.000000000000000,0.000028000000000,0.000138000000000,0.000130000000000,0.001867000000000,0.000905000000000,0.003190000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1051.000000000000000,204.000000000000000,121.000000000000000,0.204004037194742,0.217771792656962,121.000000000000000,0.322302253343558 +0.000000000000000,0.000016000000000,0.000108000000000,0.000146000000000,0.001730000000000,0.000807000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1092.000000000000000,216.000000000000000,121.000000000000000,0.189498788663577,0.218460412109291,119.000000000000000,0.323321417274184 +0.000000000000000,0.000021000000000,0.000142000000000,0.000136000000000,0.001571000000000,0.000950000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1150.000000000000000,195.000000000000000,121.000000000000000,0.234911243099688,0.229840840010708,121.000000000000000,0.340164460854319 +0.000000000000000,0.000023000000000,0.000178000000000,0.000327000000000,0.001509000000000,0.001060000000000,0.003143000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1188.000000000000000,188.000000000000000,121.000000000000000,0.238938910296195,0.242239031677996,119.000000000000000,0.358513758367035 +0.000000000000000,0.000026000000000,0.000127000000000,0.000121000000000,0.001413000000000,0.000746000000000,0.002666000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,181.000000000000000,121.000000000000000,0.231069770120829,0.351821144360750,121.000000000000000,0.520695269679250 +0.000000000000000,0.000013000000000,0.000123000000000,0.000156000000000,0.002291000000000,0.000810000000000,0.003493000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,223.000000000000000,121.000000000000000,0.232050436410629,0.243086413240903,121.000000000000000,0.359767900135068 +0.000000000000000,0.000017000000000,0.000183000000000,0.000122000000000,0.001660000000000,0.000828000000000,0.002856000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1126.000000000000000,205.000000000000000,121.000000000000000,0.217672407670591,0.232176089084970,120.000000000000000,0.343620616378729 +0.000000000000000,0.000025000000000,0.000104000000000,0.000126000000000,0.001640000000000,0.000956000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1122.000000000000000,225.000000000000000,121.000000000000000,0.210874531577464,0.231177462797771,120.000000000000000,0.342142641667492 +0.000000000000000,0.000011000000000,0.000104000000000,0.000149000000000,0.001579000000000,0.000928000000000,0.002811000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1124.000000000000000,218.000000000000000,121.000000000000000,0.199068638688048,0.231930228681434,121.000000000000000,0.343256752009726 +0.000000000000000,0.000016000000000,0.000105000000000,0.000114000000000,0.001529000000000,0.000835000000000,0.002636000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,217.000000000000000,121.000000000000000,0.199461859922069,0.201973412653913,121.000000000000000,0.298920644384362 +0.000000000000000,0.000020000000000,0.000107000000000,0.000175000000000,0.002271000000000,0.000832000000000,0.003453000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1170.000000000000000,216.000000000000000,121.000000000000000,0.220686177070489,0.284068818474203,121.000000000000000,0.420421851360250 +0.000000000000000,0.000021000000000,0.000110000000000,0.000115000000000,0.001720000000000,0.000987000000000,0.002996000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1207.000000000000000,220.000000000000000,121.000000000000000,0.211157109658996,0.207022193542394,121.000000000000000,0.306392853689555 +0.000000000000000,0.000019000000000,0.000128000000000,0.000124000000000,0.001660000000000,0.000836000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1249.000000000000000,230.000000000000000,121.000000000000000,0.227298389602018,0.273505984323976,121.000000000000000,0.404788875408716 +0.000000000000000,0.000019000000000,0.000103000000000,0.000135000000000,0.001696000000000,0.000751000000000,0.002919000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,235.000000000000000,121.000000000000000,0.230641563123264,0.341660758731761,121.000000000000000,0.505657951678639 +0.000000000000000,0.000015000000000,0.000177000000000,0.000130000000000,0.001556000000000,0.000919000000000,0.002864000000000,0.000000000000000,106.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1118.000000000000000,225.000000000000000,121.000000000000000,0.192303276106424,0.284766422639249,121.000000000000000,0.421454308293938 +0.000000000000000,0.000013000000000,0.000138000000000,0.000178000000000,0.002041000000000,0.000992000000000,0.003412000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1112.000000000000000,223.000000000000000,121.000000000000000,0.206634709237684,0.414844586254086,120.000000000000000,0.613969997701391 +0.000000000000000,0.000015000000000,0.000134000000000,0.000127000000000,0.001905000000000,0.000764000000000,0.002995000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1116.000000000000000,248.000000000000000,121.000000000000000,0.217282816286385,0.386388750278064,121.000000000000000,0.571855388443357 +0.000000000000000,0.000016000000000,0.000128000000000,0.000127000000000,0.001825000000000,0.000925000000000,0.003066000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1115.000000000000000,245.000000000000000,121.000000000000000,0.205232081617916,0.260414398266005,121.000000000000000,0.385413312570202 +0.000000000000000,0.000020000000000,0.000107000000000,0.000141000000000,0.001807000000000,0.000923000000000,0.003060000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1141.000000000000000,234.000000000000000,121.000000000000000,0.215575249362573,0.253408200953891,120.000000000000000,0.375044163656510 +0.000000000000000,0.000019000000000,0.000128000000000,0.000120000000000,0.001568000000000,0.000884000000000,0.002762000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1162.000000000000000,189.000000000000000,121.000000000000000,0.263382144346756,0.344167879309156,121.000000000000000,0.509368468563933 +0.000000000000000,0.000023000000000,0.000131000000000,0.000373000000000,0.001757000000000,0.001182000000000,0.003515000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1191.000000000000000,211.000000000000000,121.000000000000000,0.211945557067404,0.302128504397490,121.000000000000000,0.447150188651345 +0.000000000000000,0.000025000000000,0.000154000000000,0.000124000000000,0.001844000000000,0.000803000000000,0.003004000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,227.000000000000000,121.000000000000000,0.202381916559221,0.390013881008811,120.000000000000000,0.577220555675650 +0.000000000000000,0.000015000000000,0.000109000000000,0.000129000000000,0.001708000000000,0.000890000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1231.000000000000000,213.000000000000000,121.000000000000000,0.214162701863959,0.216971594768139,120.000000000000000,0.321117988293146 +0.000000000000000,0.000019000000000,0.000106000000000,0.000141000000000,0.001738000000000,0.000788000000000,0.002834000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,223.000000000000000,121.000000000000000,0.199426583525084,0.257295226754736,121.000000000000000,0.380796925542741 +0.000000000000000,0.000015000000000,0.000104000000000,0.000127000000000,0.001749000000000,0.000817000000000,0.002854000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,237.000000000000000,121.000000000000000,0.197980287517504,0.261753748672962,121.000000000000000,0.387395536882405 +0.000000000000000,0.000022000000000,0.000120000000000,0.000159000000000,0.002300000000000,0.000963000000000,0.003628000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,228.000000000000000,121.000000000000000,0.236498759179881,0.240201532654298,121.000000000000000,0.355498268421338 +0.000000000000000,0.000017000000000,0.000181000000000,0.000128000000000,0.001959000000000,0.000872000000000,0.003382000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,248.000000000000000,121.000000000000000,0.184287569106640,0.201686577196243,121.000000000000000,0.298496127529902 +0.000000000000000,0.000013000000000,0.000178000000000,0.000136000000000,0.001684000000000,0.000852000000000,0.002943000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1105.000000000000000,230.000000000000000,121.000000000000000,0.190585604418642,0.200521952748998,120.000000000000000,0.296772510252565 +0.000000000000000,0.000019000000000,0.000106000000000,0.000143000000000,0.001668000000000,0.000981000000000,0.002955000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1104.000000000000000,218.000000000000000,121.000000000000000,0.200527511479873,0.209064190915326,120.000000000000000,0.309415015408653 +0.000000000000000,0.000014000000000,0.000104000000000,0.000123000000000,0.001517000000000,0.000849000000000,0.002647000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,201.000000000000000,121.000000000000000,0.227018266784565,0.269186250934732,121.000000000000000,0.398395648547930 +0.000000000000000,0.000017000000000,0.000178000000000,0.000348000000000,0.001620000000000,0.001141000000000,0.003354000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,206.000000000000000,121.000000000000000,0.206456021778136,0.213075158345683,121.000000000000000,0.315351251063391 +0.000000000000000,0.000026000000000,0.000108000000000,0.000150000000000,0.001619000000000,0.000821000000000,0.002770000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,197.000000000000000,121.000000000000000,0.195769620795380,0.235699009656539,120.000000000000000,0.348834556747284 +0.000000000000000,0.000018000000000,0.000178000000000,0.000127000000000,0.001593000000000,0.000826000000000,0.002776000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,206.000000000000000,121.000000000000000,0.202443810598108,0.259322628022743,121.000000000000000,0.383797505085764 +0.000000000000000,0.000018000000000,0.000105000000000,0.000140000000000,0.001539000000000,0.001030000000000,0.002870000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1255.000000000000000,195.000000000000000,121.000000000000000,0.234795060265796,0.251596569636394,120.000000000000000,0.372362919449417 +0.000000000000000,0.000017000000000,0.000175000000000,0.000125000000000,0.001584000000000,0.000833000000000,0.002923000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1267.000000000000000,221.000000000000000,121.000000000000000,0.207163879614002,0.272216719799699,121.000000000000000,0.402880748216245 +0.000000000000000,0.000017000000000,0.000133000000000,0.000153000000000,0.002442000000000,0.000831000000000,0.003670000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,216.000000000000000,121.000000000000000,0.170524915168911,0.199849692332816,121.000000000000000,0.295777531500515 +0.000000000000000,0.000013000000000,0.000157000000000,0.000149000000000,0.001748000000000,0.000942000000000,0.003057000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,234.000000000000000,121.000000000000000,0.189221335752284,0.197743002271810,121.000000000000000,0.292659639941872 +0.000000000000000,0.000020000000000,0.000178000000000,0.000125000000000,0.001721000000000,0.000742000000000,0.002827000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1176.000000000000000,225.000000000000000,121.000000000000000,0.198900978356214,0.245389821232497,119.000000000000000,0.363176948895773 +0.000000000000000,0.000013000000000,0.000176000000000,0.000145000000000,0.001697000000000,0.000855000000000,0.002929000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,222.000000000000000,121.000000000000000,0.196051895442564,0.262432687056220,121.000000000000000,0.388400396935210 +0.000000000000000,0.000015000000000,0.000176000000000,0.000128000000000,0.001658000000000,0.000736000000000,0.002754000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1264.000000000000000,222.000000000000000,121.000000000000000,0.188130722176495,0.226906201366918,120.000000000000000,0.335821186914497 +0.000000000000000,0.000015000000000,0.000114000000000,0.000128000000000,0.001732000000000,0.000994000000000,0.003025000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1280.000000000000000,221.000000000000000,121.000000000000000,0.195205519657246,0.208004184809569,119.000000000000000,0.307846188328575 +0.000000000000000,0.000020000000000,0.000195000000000,0.000126000000000,0.002006000000000,0.000791000000000,0.003194000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1295.000000000000000,262.000000000000000,121.000000000000000,0.204929539974912,0.216820480192029,119.000000000000000,0.320894306981155 +0.000000000000000,0.000017000000000,0.000176000000000,0.000139000000000,0.001546000000000,0.000896000000000,0.002812000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,192.000000000000000,121.000000000000000,0.226704780607630,0.257512342665443,120.000000000000000,0.381118257356317 +0.000000000000000,0.000011000000000,0.000105000000000,0.000299000000000,0.001761000000000,0.001102000000000,0.003328000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,234.000000000000000,121.000000000000000,0.206579202251784,0.240623999885611,121.000000000000000,0.356123519730790 +0.000000000000000,0.000020000000000,0.000103000000000,0.000130000000000,0.001498000000000,0.000871000000000,0.002766000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1312.000000000000000,207.000000000000000,121.000000000000000,0.208586912880041,0.214158703806542,121.000000000000000,0.316954877506999 +0.000000000000000,0.000017000000000,0.000176000000000,0.000374000000000,0.001665000000000,0.000976000000000,0.003373000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1139.000000000000000,223.000000000000000,121.000000000000000,0.209543770412094,0.213150938854858,121.000000000000000,0.315463382345879 +0.000000000000000,0.000021000000000,0.000157000000000,0.000124000000000,0.001652000000000,0.000947000000000,0.002947000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1137.000000000000000,212.000000000000000,121.000000000000000,0.214888927283406,0.261239649375707,120.000000000000000,0.386634701996087 +0.000000000000000,0.000018000000000,0.000104000000000,0.000124000000000,0.001479000000000,0.000897000000000,0.002661000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,188.000000000000000,121.000000000000000,0.205526459876717,0.194046988139053,121.000000000000000,0.287189543909738 +0.000000000000000,0.000016000000000,0.000175000000000,0.000142000000000,0.001509000000000,0.000744000000000,0.002626000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,190.000000000000000,121.000000000000000,0.193638964960760,0.209501566643394,121.000000000000000,0.310062329059690 +0.000000000000000,0.000022000000000,0.000103000000000,0.000125000000000,0.001561000000000,0.000725000000000,0.002572000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,218.000000000000000,121.000000000000000,0.192895828207804,0.213686959411965,121.000000000000000,0.316256716855654 +0.000000000000000,0.000023000000000,0.000184000000000,0.000162000000000,0.001899000000000,0.000812000000000,0.003140000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1258.000000000000000,187.000000000000000,121.000000000000000,0.178433372351327,0.196124969530122,120.000000000000000,0.290264953456693 +0.000000000000000,0.000016000000000,0.000134000000000,0.000140000000000,0.001663000000000,0.000876000000000,0.002874000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1274.000000000000000,203.000000000000000,121.000000000000000,0.204379056805537,0.211513754780143,119.000000000000000,0.313040366095618 +0.000000000000000,0.000020000000000,0.000176000000000,0.000130000000000,0.001587000000000,0.000869000000000,0.002823000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1284.000000000000000,206.000000000000000,121.000000000000000,0.194394953473234,0.214318086264921,121.000000000000000,0.317190765131688 +0.000000000000000,0.000017000000000,0.000107000000000,0.000148000000000,0.001575000000000,0.000805000000000,0.002693000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1286.000000000000000,205.000000000000000,121.000000000000000,0.192692252511560,0.217301832678175,121.000000000000000,0.321606720966583 +0.000000000000000,0.000020000000000,0.000173000000000,0.000124000000000,0.001507000000000,0.000785000000000,0.002649000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,210.000000000000000,121.000000000000000,0.204881580167579,0.231890569564824,121.000000000000000,0.343198045458178 +0.000000000000000,0.000041000000000,0.000220000000000,0.000144000000000,0.001900000000000,0.000655000000000,0.003038000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,206.000000000000000,121.000000000000000,0.174115401194611,0.223973720218107,121.000000000000000,0.331481122502030 +0.000000000000000,0.000043000000000,0.000182000000000,0.000145000000000,0.001457000000000,0.000728000000000,0.002734000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1272.000000000000000,191.000000000000000,121.000000000000000,0.194538477548438,0.187220557909554,121.000000000000000,0.277086431960381 +0.000000000000000,0.000020000000000,0.000182000000000,0.000129000000000,0.001451000000000,0.000813000000000,0.002657000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1148.000000000000000,207.000000000000000,121.000000000000000,0.209547468410092,0.205381330935579,119.000000000000000,0.303964378715603 +0.000000000000000,0.000037000000000,0.000255000000000,0.000211000000000,0.002943000000000,0.001198000000000,0.004740000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1146.000000000000000,192.000000000000000,121.000000000000000,0.194112090334278,0.213834860708738,121.000000000000000,0.316475596512546 +0.000000000000000,0.000021000000000,0.000119000000000,0.000128000000000,0.001482000000000,0.000836000000000,0.002622000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1196.000000000000000,188.000000000000000,121.000000000000000,0.194707145965396,0.200711676435348,120.000000000000000,0.297053280716492 +0.000000000000000,0.000018000000000,0.000128000000000,0.000353000000000,0.001530000000000,0.001123000000000,0.003198000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1222.000000000000000,182.000000000000000,121.000000000000000,0.165297929068422,0.223118899091734,120.000000000000000,0.330215962199139 +0.000000000000000,0.000019000000000,0.000133000000000,0.000124000000000,0.001554000000000,0.001016000000000,0.002890000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,187.000000000000000,121.000000000000000,0.189732092265728,0.231665076279646,121.000000000000000,0.342864305156952 +0.000000000000000,0.000018000000000,0.000195000000000,0.000130000000000,0.001387000000000,0.000871000000000,0.002639000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,167.000000000000000,121.000000000000000,0.173396135722069,0.185374303680201,121.000000000000000,0.274353961674946 +0.000000000000000,0.000015000000000,0.000105000000000,0.000165000000000,0.001566000000000,0.001052000000000,0.002950000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1292.000000000000000,196.000000000000000,121.000000000000000,0.192868888286218,0.205034141945327,120.000000000000000,0.303450551076308 +0.000000000000000,0.000014000000000,0.000102000000000,0.000127000000000,0.001521000000000,0.000791000000000,0.002592000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1302.000000000000000,199.000000000000000,121.000000000000000,0.190528673137881,0.222906754530888,121.000000000000000,0.329902009771293 +0.000000000000000,0.000021000000000,0.000215000000000,0.000167000000000,0.001934000000000,0.000805000000000,0.003204000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.202526348712512,0.259278841345545,120.000000000000000,0.383732682741932 +0.000000000000000,0.000022000000000,0.000156000000000,0.000126000000000,0.001696000000000,0.000788000000000,0.002834000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.196451913404442,0.201069768524068,120.000000000000000,0.297583282351909 +0.000000000000000,0.000025000000000,0.000103000000000,0.000132000000000,0.001543000000000,0.000902000000000,0.002745000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1305.000000000000000,194.000000000000000,121.000000000000000,0.200871633124814,0.183908150343481,120.000000000000000,0.272184068463992 +0.000000000000000,0.000025000000000,0.000102000000000,0.000150000000000,0.001478000000000,0.000854000000000,0.002788000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1335.000000000000000,179.000000000000000,121.000000000000000,0.193823047570574,0.212329442302877,119.000000000000000,0.314247578003029 +0.000000000000000,0.000016000000000,0.000196000000000,0.000126000000000,0.001299000000000,0.000752000000000,0.002455000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1194.000000000000000,176.000000000000000,121.000000000000000,0.207923081779578,0.229508478265270,121.000000000000000,0.339672543925396 +0.000000000000000,0.000018000000000,0.000110000000000,0.000166000000000,0.001844000000000,0.000982000000000,0.003170000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,180.000000000000000,121.000000000000000,0.163342705330096,0.249575746286056,120.000000000000000,0.369372120119674 +0.000000000000000,0.000015000000000,0.000183000000000,0.000124000000000,0.001749000000000,0.000847000000000,0.002963000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,212.000000000000000,121.000000000000000,0.173881865983057,0.194395754595030,121.000000000000000,0.287705721832844 +0.000000000000000,0.000026000000000,0.000176000000000,0.000127000000000,0.001498000000000,0.000877000000000,0.002752000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1259.000000000000000,185.000000000000000,121.000000000000000,0.155523040470466,0.162766723518840,120.000000000000000,0.240894740082153 +0.000000000000000,0.000015000000000,0.000177000000000,0.000144000000000,0.001550000000000,0.000743000000000,0.002669000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,192.000000000000000,121.000000000000000,0.152934150143871,0.272911472202902,121.000000000000000,0.403908984747820 +0.000000000000000,0.000014000000000,0.000174000000000,0.000116000000000,0.001439000000000,0.000731000000000,0.002513000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1291.000000000000000,196.000000000000000,121.000000000000000,0.188113809380047,0.184618919088813,121.000000000000000,0.273236010008633 +0.000000000000000,0.000016000000000,0.000106000000000,0.000145000000000,0.002198000000000,0.000804000000000,0.003318000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1299.000000000000000,184.000000000000000,121.000000000000000,0.189391540723312,0.210953756830538,121.000000000000000,0.312211549881851 +0.000000000000000,0.000019000000000,0.000181000000000,0.000116000000000,0.001662000000000,0.000840000000000,0.002865000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1306.000000000000000,204.000000000000000,121.000000000000000,0.184488742976474,0.215925329354907,121.000000000000000,0.319569479795696 +0.000000000000000,0.000016000000000,0.000183000000000,0.000132000000000,0.001540000000000,0.000737000000000,0.002647000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1313.000000000000000,197.000000000000000,121.000000000000000,0.164667840588859,0.233194073999862,121.000000000000000,0.345127224117476 +0.000000000000000,0.000017000000000,0.000177000000000,0.000144000000000,0.001453000000000,0.000747000000000,0.002725000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1307.000000000000000,179.000000000000000,121.000000000000000,0.190996220324968,0.267097150771334,121.000000000000000,0.395303787014294 +0.000000000000000,0.000017000000000,0.000177000000000,0.000120000000000,0.001414000000000,0.000681000000000,0.002474000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1109.000000000000000,205.000000000000000,121.000000000000000,0.175048358387321,0.227273301247532,121.000000000000000,0.336364481552976 +0.000000000000000,0.000023000000000,0.000186000000000,0.000160000000000,0.001932000000000,0.000738000000000,0.003097000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1100.000000000000000,194.000000000000000,121.000000000000000,0.173557283673669,0.191658131610694,121.000000000000000,0.283654034716855 +0.000000000000000,0.000019000000000,0.000107000000000,0.000116000000000,0.001671000000000,0.000745000000000,0.002708000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,204.000000000000000,121.000000000000000,0.158568907261423,0.243359531069700,121.000000000000000,0.360172098406900 +0.000000000000000,0.000018000000000,0.000103000000000,0.000132000000000,0.001512000000000,0.000732000000000,0.002547000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1187.000000000000000,194.000000000000000,121.000000000000000,0.146749823653506,0.164413086505490,121.000000000000000,0.243331352597904 +0.000000000000000,0.000014000000000,0.000181000000000,0.000144000000000,0.001537000000000,0.000892000000000,0.002808000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,196.000000000000000,121.000000000000000,0.131283592229348,0.155956326816494,121.000000000000000,0.230815358417731 +0.000000000000000,0.000012000000000,0.000176000000000,0.000121000000000,0.001482000000000,0.000877000000000,0.002705000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,196.000000000000000,121.000000000000000,0.159033296053125,0.194395333842714,121.000000000000000,0.287705090036121 +0.000000000000000,0.000020000000000,0.000211000000000,0.000162000000000,0.001881000000000,0.000840000000000,0.003174000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1262.000000000000000,184.000000000000000,121.000000000000000,0.163845159646448,0.177604150932044,121.000000000000000,0.262854149344864 +0.000000000000000,0.000015000000000,0.000115000000000,0.000124000000000,0.001771000000000,0.000924000000000,0.002997000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1269.000000000000000,218.000000000000000,121.000000000000000,0.167741708685618,0.187775885781358,121.000000000000000,0.277908323682058 +0.000000000000000,0.000012000000000,0.000104000000000,0.000127000000000,0.001764000000000,0.000780000000000,0.002830000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,229.000000000000000,121.000000000000000,0.186347950475868,0.251813866360726,121.000000000000000,0.372684503981683 +0.000000000000000,0.000020000000000,0.000102000000000,0.000142000000000,0.001444000000000,0.000872000000000,0.002621000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1275.000000000000000,174.000000000000000,121.000000000000000,0.166303486197133,0.189747084068580,120.000000000000000,0.280825682960661 +0.000000000000000,0.000016000000000,0.000125000000000,0.000117000000000,0.001504000000000,0.000769000000000,0.002568000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,202.000000000000000,121.000000000000000,0.167769122437661,0.195787376215580,121.000000000000000,0.289765303335874 +0.000000000000000,0.000029000000000,0.000158000000000,0.000154000000000,0.001818000000000,0.000771000000000,0.003044000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1304.000000000000000,177.000000000000000,121.000000000000000,0.158463858246833,0.191255881763862,119.000000000000000,0.283058705300228 +0.000000000000000,0.000020000000000,0.000185000000000,0.000118000000000,0.001683000000000,0.000786000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1339.000000000000000,206.000000000000000,121.000000000000000,0.161183566619254,0.178028171732775,121.000000000000000,0.263481700394391 +0.000000000000000,0.000017000000000,0.000177000000000,0.000136000000000,0.001597000000000,0.000782000000000,0.002874000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1383.000000000000000,198.000000000000000,121.000000000000000,0.142916686874711,0.162123928033689,121.000000000000000,0.239943431119633 +0.000000000000000,0.000016000000000,0.000127000000000,0.000234000000000,0.001629000000000,0.000716000000000,0.002906000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,214.000000000000000,121.000000000000000,0.154355717762879,0.218609409653489,121.000000000000000,0.323541914330688 +0.000000000000000,0.000018000000000,0.000175000000000,0.000117000000000,0.001623000000000,0.000874000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,227.000000000000000,121.000000000000000,0.174558038196839,0.199124535422790,121.000000000000000,0.294704311042227 +0.000000000000000,0.000023000000000,0.000101000000000,0.000109000000000,0.001533000000000,0.000703000000000,0.002504000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1228.000000000000000,215.000000000000000,121.000000000000000,0.137995003938898,0.152632389051692,121.000000000000000,0.225895936409808 +0.000000000000000,0.000027000000000,0.000177000000000,0.000139000000000,0.001775000000000,0.000662000000000,0.002820000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,237.000000000000000,121.000000000000000,0.135541904251767,0.167668386817417,121.000000000000000,0.248149206964557 +0.000000000000000,0.000022000000000,0.000181000000000,0.000307000000000,0.001635000000000,0.001033000000000,0.003237000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,200.000000000000000,121.000000000000000,0.163536557849499,0.206044880204936,121.000000000000000,0.304946443542624 +0.000000000000000,0.000013000000000,0.000197000000000,0.000138000000000,0.001693000000000,0.000665000000000,0.002750000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,224.000000000000000,121.000000000000000,0.153373223673703,0.199555553830275,120.000000000000000,0.295342223558018 +0.000000000000000,0.000022000000000,0.000130000000000,0.000127000000000,0.001747000000000,0.000744000000000,0.002816000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,234.000000000000000,121.000000000000000,0.139032943637894,0.186670354479152,121.000000000000000,0.276272121799386 +0.000000000000000,0.000023000000000,0.000189000000000,0.000117000000000,0.001436000000000,0.000905000000000,0.002707000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,191.000000000000000,121.000000000000000,0.139072087006321,0.170137357542569,121.000000000000000,0.251803291766507 +0.000000000000000,0.000019000000000,0.000189000000000,0.000134000000000,0.001636000000000,0.001043000000000,0.003059000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,205.000000000000000,121.000000000000000,0.147221191440148,0.219839242753776,121.000000000000000,0.325362095419246 +0.000000000000000,0.000026000000000,0.000136000000000,0.000141000000000,0.001640000000000,0.000834000000000,0.002814000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,198.000000000000000,121.000000000000000,0.134535729635055,0.177361976863745,120.000000000000000,0.262495743699553 +0.000000000000000,0.000024000000000,0.000188000000000,0.000137000000000,0.001576000000000,0.000999000000000,0.002968000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1224.000000000000000,202.000000000000000,121.000000000000000,0.150440594097383,0.170822892372034,120.000000000000000,0.252817881488855 +0.000000000000000,0.000018000000000,0.000128000000000,0.000123000000000,0.001595000000000,0.000906000000000,0.002923000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,211.000000000000000,121.000000000000000,0.161157776486683,0.176850558454008,121.000000000000000,0.261738825952921 +0.000000000000000,0.000023000000000,0.000171000000000,0.000136000000000,0.001546000000000,0.000836000000000,0.002772000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1094.000000000000000,211.000000000000000,121.000000000000000,0.135044752661059,0.176661372259443,121.000000000000000,0.261458838916931 +0.000000000000000,0.000016000000000,0.000180000000000,0.000129000000000,0.001692000000000,0.000849000000000,0.002919000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1089.000000000000000,203.000000000000000,121.000000000000000,0.143949354754192,0.167668782751111,120.000000000000000,0.248149788217542 +0.000000000000000,0.000017000000000,0.000108000000000,0.000139000000000,0.001568000000000,0.000704000000000,0.002578000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1166.000000000000000,200.000000000000000,121.000000000000000,0.155160670749007,0.231930876186467,120.000000000000000,0.343257712340746 +0.000000000000000,0.000035000000000,0.000227000000000,0.000132000000000,0.001664000000000,0.000790000000000,0.002968000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,195.000000000000000,121.000000000000000,0.133676069237326,0.147626375091364,120.000000000000000,0.218487058408179 +0.000000000000000,0.000015000000000,0.000128000000000,0.000120000000000,0.001551000000000,0.000732000000000,0.002590000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,204.000000000000000,121.000000000000000,0.122400626475427,0.146223124911287,121.000000000000000,0.216410216218663 +0.000000000000000,0.000020000000000,0.000245000000000,0.000118000000000,0.001618000000000,0.000770000000000,0.002815000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,219.000000000000000,121.000000000000000,0.137303784430655,0.150577933818061,121.000000000000000,0.222855351498252 +0.000000000000000,0.000016000000000,0.000214000000000,0.000131000000000,0.001570000000000,0.000888000000000,0.002875000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1239.000000000000000,183.000000000000000,121.000000000000000,0.131464957645487,0.148026075188344,121.000000000000000,0.219078597044465 +0.000000000000000,0.000018000000000,0.000103000000000,0.000139000000000,0.001556000000000,0.000929000000000,0.002796000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,197.000000000000000,121.000000000000000,0.128347170794260,0.131002024287506,121.000000000000000,0.193882998604601 +0.000000000000000,0.000034000000000,0.000281000000000,0.000139000000000,0.001662000000000,0.000725000000000,0.002956000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,194.000000000000000,121.000000000000000,0.162024538919116,0.207094523301695,121.000000000000000,0.306499905326386 +0.000000000000000,0.000019000000000,0.000198000000000,0.000119000000000,0.001621000000000,0.000844000000000,0.002973000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,215.000000000000000,121.000000000000000,0.153235488057770,0.262436341625619,121.000000000000000,0.388405779843294 +0.000000000000000,0.000021000000000,0.000198000000000,0.000131000000000,0.001561000000000,0.000847000000000,0.002828000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1054.000000000000000,201.000000000000000,121.000000000000000,0.157919685289792,0.167838875425546,121.000000000000000,0.248401521304057 +0.000000000000000,0.000015000000000,0.000170000000000,0.000130000000000,0.001648000000000,0.000666000000000,0.002683000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,211.000000000000000,121.000000000000000,0.142424009465490,0.209332762567431,120.000000000000000,0.309812491363346 +0.000000000000000,0.000020000000000,0.000127000000000,0.000139000000000,0.001528000000000,0.000726000000000,0.002588000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,207.000000000000000,121.000000000000000,0.140603779602412,0.159626649619649,121.000000000000000,0.236247445558979 +0.000000000000000,0.000019000000000,0.000194000000000,0.000143000000000,0.001999000000000,0.000624000000000,0.003025000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,205.000000000000000,121.000000000000000,0.147458908627836,0.237281094131071,121.000000000000000,0.351176020676474 +0.000000000000000,0.000022000000000,0.000175000000000,0.000126000000000,0.001454000000000,0.000837000000000,0.002656000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1106.000000000000000,198.000000000000000,121.000000000000000,0.125862132363062,0.146166756283214,120.000000000000000,0.216326793779291 +0.000000000000000,0.000018000000000,0.000233000000000,0.000119000000000,0.001470000000000,0.000923000000000,0.002807000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1145.000000000000000,188.000000000000000,121.000000000000000,0.148866800739627,0.175671536734951,121.000000000000000,0.259993879218198 +0.000000000000000,0.000021000000000,0.000205000000000,0.000128000000000,0.001543000000000,0.000761000000000,0.002708000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1156.000000000000000,186.000000000000000,121.000000000000000,0.127848678819928,0.170790549426913,121.000000000000000,0.252770016569082 +0.000000000000000,0.000016000000000,0.000176000000000,0.000145000000000,0.001515000000000,0.000719000000000,0.002758000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1165.000000000000000,189.000000000000000,121.000000000000000,0.145912191034524,0.246210134411681,121.000000000000000,0.364391034751536 +0.000000000000000,0.000017000000000,0.000137000000000,0.000142000000000,0.001535000000000,0.000625000000000,0.002544000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1007.000000000000000,197.000000000000000,121.000000000000000,0.155415123408675,0.250107101898265,121.000000000000000,0.370158504865490 +0.000000000000000,0.000018000000000,0.000187000000000,0.000129000000000,0.001485000000000,0.000745000000000,0.002609000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,997.000000000000000,196.000000000000000,121.000000000000000,0.156330994449520,0.195977327324711,121.000000000000000,0.290046452877835 +0.000000000000000,0.000027000000000,0.000216000000000,0.000122000000000,0.001463000000000,0.000715000000000,0.002603000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,994.000000000000000,202.000000000000000,121.000000000000000,0.148181823678736,0.163960783045368,121.000000000000000,0.242661951333412 +0.000000000000000,0.000021000000000,0.000148000000000,0.000131000000000,0.001726000000000,0.000788000000000,0.002871000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,990.000000000000000,214.000000000000000,121.000000000000000,0.159041550968813,0.290710356872846,121.000000000000000,0.430251294240852 +0.000000000000000,0.000014000000000,0.000109000000000,0.000146000000000,0.001782000000000,0.000832000000000,0.002935000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,237.000000000000000,121.000000000000000,0.144598980992546,0.166027958406800,121.000000000000000,0.245721363787328 +0.000000000000000,0.000033000000000,0.000225000000000,0.000141000000000,0.001562000000000,0.000724000000000,0.002797000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,200.000000000000000,121.000000000000000,0.138993500529387,0.196990361228138,121.000000000000000,0.291545731774589 +0.000000000000000,0.000018000000000,0.000102000000000,0.000130000000000,0.001577000000000,0.000768000000000,0.002776000000000,1.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1025.000000000000000,218.000000000000000,121.000000000000000,0.159928213604770,0.189984919538914,121.000000000000000,0.281177669551262 +0.000000000000000,0.000012000000000,0.000247000000000,0.000124000000000,0.001584000000000,0.000607000000000,0.002648000000000,0.000000000000000,108.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,232.000000000000000,121.000000000000000,0.169367654114597,0.221725877325533,121.000000000000000,0.328154308402856 +0.000000000000000,0.000029000000000,0.000128000000000,0.000128000000000,0.001642000000000,0.000782000000000,0.002760000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,211.000000000000000,121.000000000000000,0.147968518377667,0.154122927463339,121.000000000000000,0.228101943121478 +0.000000000000000,0.000016000000000,0.000176000000000,0.000142000000000,0.001527000000000,0.000917000000000,0.002821000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,946.000000000000000,207.000000000000000,121.000000000000000,0.137541055415491,0.157761641471241,121.000000000000000,0.233487226761447 +0.000000000000000,0.000032000000000,0.000147000000000,0.000136000000000,0.001548000000000,0.000751000000000,0.002712000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,954.000000000000000,199.000000000000000,121.000000000000000,0.162415187446687,0.197580421079148,121.000000000000000,0.292419026477691 +0.000000000000000,0.000019000000000,0.000178000000000,0.000129000000000,0.001473000000000,0.000728000000000,0.002570000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,952.000000000000000,191.000000000000000,121.000000000000000,0.171041015417160,0.194915117263576,121.000000000000000,0.288474365726664 +0.000000000000000,0.000029000000000,0.000197000000000,0.000115000000000,0.001354000000000,0.000809000000000,0.002551000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,965.000000000000000,175.000000000000000,121.000000000000000,0.154222763807780,0.285544163762687,120.000000000000000,0.422605366108563 +0.000000000000000,0.000019000000000,0.000139000000000,0.000125000000000,0.001460000000000,0.000814000000000,0.002600000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,968.000000000000000,171.000000000000000,121.000000000000000,0.150595603214115,0.221852768512487,121.000000000000000,0.328342103660952 +0.000000000000000,0.000013000000000,0.000104000000000,0.000134000000000,0.001356000000000,0.000752000000000,0.002398000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,996.000000000000000,168.000000000000000,121.000000000000000,0.147430237486138,0.148159285827304,121.000000000000000,0.219275742894069 +0.000000000000000,0.000020000000000,0.000193000000000,0.000142000000000,0.001953000000000,0.000947000000000,0.003301000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1030.000000000000000,177.000000000000000,121.000000000000000,0.145798815037739,0.159663663944367,121.000000000000000,0.236302234970841 +0.000000000000000,0.000015000000000,0.000103000000000,0.000115000000000,0.001290000000000,0.000699000000000,0.002255000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,172.000000000000000,121.000000000000000,0.142196014156506,0.187147109561421,121.000000000000000,0.276977737652069 +0.000000000000000,0.000024000000000,0.000160000000000,0.000150000000000,0.001811000000000,0.000777000000000,0.003271000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1069.000000000000000,202.000000000000000,121.000000000000000,0.139609449101900,0.165271225439012,121.000000000000000,0.244601415643369 +0.000000000000000,0.000018000000000,0.000179000000000,0.000151000000000,0.001532000000000,0.000737000000000,0.002693000000000,0.000000000000000,97.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,203.000000000000000,121.000000000000000,0.147947477270869,0.179555832175793,121.000000000000000,0.265742648149137 +0.000000000000000,0.000019000000000,0.000177000000000,0.000125000000000,0.001512000000000,0.000717000000000,0.002588000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,212.000000000000000,121.000000000000000,0.147575394987053,0.165266294276773,121.000000000000000,0.244594112073245 +0.000000000000000,0.000016000000000,0.000106000000000,0.000137000000000,0.001490000000000,0.000826000000000,0.002618000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,915.000000000000000,203.000000000000000,121.000000000000000,0.154152939014589,0.159181033056470,121.000000000000000,0.235587925595243 +0.000000000000000,0.000013000000000,0.000177000000000,0.000117000000000,0.001355000000000,0.000751000000000,0.002448000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,921.000000000000000,192.000000000000000,121.000000000000000,0.150692609588339,0.183809895950044,121.000000000000000,0.272038654130109 +0.000000000000000,0.000023000000000,0.000221000000000,0.000167000000000,0.001790000000000,0.000948000000000,0.003199000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,932.000000000000000,170.000000000000000,121.000000000000000,0.149963553230709,0.203273297272569,121.000000000000000,0.300844465407444 +0.000000000000000,0.000026000000000,0.000151000000000,0.000122000000000,0.001357000000000,0.000723000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,961.000000000000000,168.000000000000000,121.000000000000000,0.142178288778563,0.151033799800819,121.000000000000000,0.223530034583352 +0.000000000000000,0.000018000000000,0.000155000000000,0.000123000000000,0.001394000000000,0.000766000000000,0.002492000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,986.000000000000000,182.000000000000000,121.000000000000000,0.141207634148801,0.172667793318101,121.000000000000000,0.255548330024774 +0.000000000000000,0.000018000000000,0.000174000000000,0.000143000000000,0.001389000000000,0.000790000000000,0.002550000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1023.000000000000000,171.000000000000000,121.000000000000000,0.140177814193211,0.162740001476768,121.000000000000000,0.240855214879131 +0.000000000000000,0.000021000000000,0.000173000000000,0.000122000000000,0.001280000000000,0.000796000000000,0.002581000000000,1.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1049.000000000000000,171.000000000000000,121.000000000000000,0.138041804635574,0.175894893269503,121.000000000000000,0.260324460535807 +0.000000000000000,0.000040000000000,0.000240000000000,0.000156000000000,0.001610000000000,0.000905000000000,0.003078000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,181.000000000000000,121.000000000000000,0.128640393991057,0.132619727567557,121.000000000000000,0.196277204924269 +0.000000000000000,0.000016000000000,0.000181000000000,0.000117000000000,0.001424000000000,0.000760000000000,0.002543000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,886.000000000000000,172.000000000000000,121.000000000000000,0.138352531751683,0.243902625004102,121.000000000000000,0.360975895470416 +0.000000000000000,0.000020000000000,0.000178000000000,0.000131000000000,0.001344000000000,0.000965000000000,0.002674000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,904.000000000000000,177.000000000000000,121.000000000000000,0.132849298892330,0.165943140041726,121.000000000000000,0.245595863686171 +0.000000000000000,0.000018000000000,0.000129000000000,0.000145000000000,0.001297000000000,0.000878000000000,0.002506000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,916.000000000000000,164.000000000000000,121.000000000000000,0.139100399764650,0.179464095411716,121.000000000000000,0.265606862397320 +0.000000000000000,0.000015000000000,0.000175000000000,0.000119000000000,0.001188000000000,0.000717000000000,0.002249000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,926.000000000000000,158.000000000000000,121.000000000000000,0.124508577433483,0.232688205210390,121.000000000000000,0.344378545000118 +0.000000000000000,0.000041000000000,0.000173000000000,0.000189000000000,0.001590000000000,0.000722000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,950.000000000000000,167.000000000000000,121.000000000000000,0.124364211160562,0.198989442345874,121.000000000000000,0.294504385287040 +0.000000000000000,0.000022000000000,0.000180000000000,0.000121000000000,0.001454000000000,0.000762000000000,0.002586000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,981.000000000000000,174.000000000000000,121.000000000000000,0.121986273502660,0.181985786779231,121.000000000000000,0.269338961458690 +0.000000000000000,0.000022000000000,0.000175000000000,0.000128000000000,0.001283000000000,0.000822000000000,0.002468000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1006.000000000000000,154.000000000000000,121.000000000000000,0.120933033868538,0.208757551950541,121.000000000000000,0.308961183186222 +0.000000000000000,0.000026000000000,0.000175000000000,0.000143000000000,0.001286000000000,0.000813000000000,0.002485000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1029.000000000000000,160.000000000000000,121.000000000000000,0.109269904153490,0.130322654376541,121.000000000000000,0.192877532026941 +0.000000000000000,0.000023000000000,0.000175000000000,0.000120000000000,0.001254000000000,0.000637000000000,0.002411000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,160.000000000000000,121.000000000000000,0.126897062957379,0.186841162125259,121.000000000000000,0.276524916304396 +0.000000000000000,0.000021000000000,0.000140000000000,0.000122000000000,0.001471000000000,0.000691000000000,0.002520000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,891.000000000000000,190.000000000000000,121.000000000000000,0.112694197899670,0.185521960041402,121.000000000000000,0.274572512797542 +0.000000000000000,0.000020000000000,0.000176000000000,0.000199000000000,0.001263000000000,0.000879000000000,0.002581000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,888.000000000000000,161.000000000000000,121.000000000000000,0.099793871260750,0.108940668612828,121.000000000000000,0.161232198827340 +0.000000000000000,0.000018000000000,0.000103000000000,0.000137000000000,0.001309000000000,0.000726000000000,0.002333000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,901.000000000000000,171.000000000000000,121.000000000000000,0.091727983876207,0.161347683449736,121.000000000000000,0.238794571957605 +0.000000000000000,0.000018000000000,0.000175000000000,0.000143000000000,0.001336000000000,0.000860000000000,0.002571000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,909.000000000000000,175.000000000000000,121.000000000000000,0.124196463571817,0.147059251648374,121.000000000000000,0.217647703825064 +0.000000000000000,0.000019000000000,0.000174000000000,0.000125000000000,0.001265000000000,0.000845000000000,0.002464000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,175.000000000000000,121.000000000000000,0.114265472855587,0.148521989180687,121.000000000000000,0.219812542662230 +0.000000000000000,0.000030000000000,0.000158000000000,0.000151000000000,0.001613000000000,0.000756000000000,0.002826000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,943.000000000000000,170.000000000000000,121.000000000000000,0.092242007337942,0.147927216635033,121.000000000000000,0.218932272923301 +0.000000000000000,0.000017000000000,0.000106000000000,0.000120000000000,0.001308000000000,0.001030000000000,0.002622000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,970.000000000000000,159.000000000000000,121.000000000000000,0.106941604326635,0.142972503313410,121.000000000000000,0.211599311615969 +0.000000000000000,0.000021000000000,0.000127000000000,0.000130000000000,0.001305000000000,0.000748000000000,0.002368000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,164.000000000000000,121.000000000000000,0.114683633251297,0.167145956283516,121.000000000000000,0.247376014134327 +0.000000000000000,0.000023000000000,0.000135000000000,0.000297000000000,0.001353000000000,0.000968000000000,0.002824000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1014.000000000000000,170.000000000000000,121.000000000000000,0.103049615014965,0.141054167934770,121.000000000000000,0.208760168771737 +0.000000000000000,0.000020000000000,0.000101000000000,0.000125000000000,0.001114000000000,0.000766000000000,0.002163000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1033.000000000000000,149.000000000000000,121.000000000000000,0.088395574941732,0.099895628586074,121.000000000000000,0.147845538218435 +0.000000000000000,0.000023000000000,0.000190000000000,0.000110000000000,0.001226000000000,0.000774000000000,0.002371000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,162.000000000000000,121.000000000000000,0.098469632616480,0.121321004607324,121.000000000000000,0.179555087237014 +0.000000000000000,0.000022000000000,0.000180000000000,0.000138000000000,0.001374000000000,0.001109000000000,0.002867000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1058.000000000000000,162.000000000000000,121.000000000000000,0.106165831669812,0.125397650867987,120.000000000000000,0.185588518499696 +0.000000000000000,0.000022000000000,0.000177000000000,0.000138000000000,0.001328000000000,0.000617000000000,0.002320000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1063.000000000000000,154.000000000000000,121.000000000000000,0.100439787680905,0.214221605374360,121.000000000000000,0.317047979072175 +0.000000000000000,0.000019000000000,0.000114000000000,0.000131000000000,0.001262000000000,0.000730000000000,0.002294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1073.000000000000000,151.000000000000000,121.000000000000000,0.101286833254115,0.171921771357668,121.000000000000000,0.254444227342640 +0.000000000000000,0.000018000000000,0.000111000000000,0.000120000000000,0.001321000000000,0.000814000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1075.000000000000000,159.000000000000000,121.000000000000000,0.100562747826060,0.189141833234101,121.000000000000000,0.279929921566071 +0.000000000000000,0.000020000000000,0.000137000000000,0.000115000000000,0.001291000000000,0.000697000000000,0.002305000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1102.000000000000000,170.000000000000000,121.000000000000000,0.108513981039663,0.171230581801412,121.000000000000000,0.253421272631674 +0.000000000000000,0.000023000000000,0.000142000000000,0.000126000000000,0.001300000000000,0.000836000000000,0.002476000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1133.000000000000000,146.000000000000000,121.000000000000000,0.093525447667265,0.136448459114485,121.000000000000000,0.201943713921807 +0.000000000000000,0.000018000000000,0.000175000000000,0.000137000000000,0.001338000000000,0.000746000000000,0.002454000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1153.000000000000000,167.000000000000000,121.000000000000000,0.098830315127647,0.135984610426414,121.000000000000000,0.201257228873912 +0.000000000000000,0.000016000000000,0.000153000000000,0.000133000000000,0.001342000000000,0.000846000000000,0.002529000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1169.000000000000000,168.000000000000000,121.000000000000000,0.104911664860530,0.116528427363267,121.000000000000000,0.172462069689336 +0.000000000000000,0.000016000000000,0.000177000000000,0.000153000000000,0.001420000000000,0.001057000000000,0.002862000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,171.000000000000000,121.000000000000000,0.112357270101045,0.155883824635645,121.000000000000000,0.230708054062211 +0.000000000000000,0.000019000000000,0.000183000000000,0.000118000000000,0.001173000000000,0.000692000000000,0.002218000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1205.000000000000000,160.000000000000000,121.000000000000000,0.114134537418471,0.211839504506888,121.000000000000000,0.313522477539393 +0.000000000000000,0.000022000000000,0.000144000000000,0.000340000000000,0.002673000000000,0.001050000000000,0.004300000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1210.000000000000000,169.000000000000000,121.000000000000000,0.118247530316238,0.220707314595291,121.000000000000000,0.326646841420566 +0.000000000000000,0.000021000000000,0.000121000000000,0.000119000000000,0.001312000000000,0.000708000000000,0.002323000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,159.000000000000000,121.000000000000000,0.108077994964789,0.189119078021565,121.000000000000000,0.279896234164773 +0.000000000000000,0.000022000000000,0.000131000000000,0.000170000000000,0.001575000000000,0.000930000000000,0.002873000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,161.000000000000000,121.000000000000000,0.111392372838571,0.157100432720142,121.000000000000000,0.232508649452340 +0.000000000000000,0.000021000000000,0.000129000000000,0.000146000000000,0.001357000000000,0.001069000000000,0.002758000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,169.000000000000000,121.000000000000000,0.115843692072703,0.148606734789863,121.000000000000000,0.219937966947781 +0.000000000000000,0.000018000000000,0.000196000000000,0.000246000000000,0.003021000000000,0.001245000000000,0.004778000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1237.000000000000000,170.000000000000000,121.000000000000000,0.110486981424295,0.140524548316829,121.000000000000000,0.207976323848749 +0.000000000000000,0.000019000000000,0.000176000000000,0.000120000000000,0.001319000000000,0.000781000000000,0.002451000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,166.000000000000000,121.000000000000000,0.116858932300554,0.142202739311708,121.000000000000000,0.210460043128204 +0.000000000000000,0.000023000000000,0.000194000000000,0.000117000000000,0.001352000000000,0.000677000000000,0.002411000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1229.000000000000000,165.000000000000000,121.000000000000000,0.110055898076008,0.160160155663640,121.000000000000000,0.237037039832150 +0.000000000000000,0.000019000000000,0.000128000000000,0.000129000000000,0.001375000000000,0.000783000000000,0.002473000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,170.000000000000000,121.000000000000000,0.126041915882071,0.160651736154074,121.000000000000000,0.237764591667073 +0.000000000000000,0.000024000000000,0.000199000000000,0.000203000000000,0.001799000000000,0.000878000000000,0.003158000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,182.000000000000000,121.000000000000000,0.131395777815393,0.222014190834195,121.000000000000000,0.328581023909914 +0.000000000000000,0.000019000000000,0.000103000000000,0.000125000000000,0.001415000000000,0.000879000000000,0.002580000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,183.000000000000000,121.000000000000000,0.135634727713905,0.187089754575309,121.000000000000000,0.276892849444298 +0.000000000000000,0.000017000000000,0.000101000000000,0.000119000000000,0.001263000000000,0.000774000000000,0.002307000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,171.000000000000000,121.000000000000000,0.140713971719655,0.131521683676958,121.000000000000000,0.194652084756127 +0.000000000000000,0.000016000000000,0.000127000000000,0.000147000000000,0.001449000000000,0.000744000000000,0.002523000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1225.000000000000000,178.000000000000000,121.000000000000000,0.139061294677762,0.203192935815326,121.000000000000000,0.300725561264080 +0.000000000000000,0.000018000000000,0.000191000000000,0.000120000000000,0.001241000000000,0.000897000000000,0.002515000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,165.000000000000000,121.000000000000000,0.137698280788293,0.173674353597993,121.000000000000000,0.257038030883200 +0.000000000000000,0.000018000000000,0.000177000000000,0.000127000000000,0.001350000000000,0.000790000000000,0.002651000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,165.000000000000000,121.000000000000000,0.156706447381789,0.199509549579289,121.000000000000000,0.295274141143093 +0.000000000000000,0.000016000000000,0.000153000000000,0.000147000000000,0.001501000000000,0.000734000000000,0.002643000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1046.000000000000000,206.000000000000000,121.000000000000000,0.145254964747455,0.192623491876607,121.000000000000000,0.285082754555528 +0.000000000000000,0.000017000000000,0.000175000000000,0.000117000000000,0.001356000000000,0.000838000000000,0.002539000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1044.000000000000000,191.000000000000000,121.000000000000000,0.154344725976771,0.284210388757305,121.000000000000000,0.420631380425634 +0.000000000000000,0.000021000000000,0.000118000000000,0.000127000000000,0.001456000000000,0.000725000000000,0.002492000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,193.000000000000000,121.000000000000000,0.154361389589804,0.236960337646421,121.000000000000000,0.350701313890299 +0.000000000000000,0.000018000000000,0.000108000000000,0.000128000000000,0.001548000000000,0.000868000000000,0.002721000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1036.000000000000000,190.000000000000000,121.000000000000000,0.144847037730729,0.155773166896081,121.000000000000000,0.230544292351477 +0.000000000000000,0.000018000000000,0.000129000000000,0.000124000000000,0.001383000000000,0.000975000000000,0.002669000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1027.000000000000000,183.000000000000000,121.000000000000000,0.145841805810321,0.158516249676071,121.000000000000000,0.234604041193658 +0.000000000000000,0.000028000000000,0.000216000000000,0.000131000000000,0.001615000000000,0.000765000000000,0.002878000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1020.000000000000000,192.000000000000000,121.000000000000000,0.144426676201549,0.156437471874113,121.000000000000000,0.231527443868863 +0.000000000000000,0.000021000000000,0.000153000000000,0.000120000000000,0.001406000000000,0.000771000000000,0.002509000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1015.000000000000000,188.000000000000000,121.000000000000000,0.147869059454709,0.168692675134760,120.000000000000000,0.249665165294811 +0.000000000000000,0.000021000000000,0.000170000000000,0.000115000000000,0.001425000000000,0.000608000000000,0.002395000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1012.000000000000000,187.000000000000000,121.000000000000000,0.168987279193652,0.289264692226178,121.000000000000000,0.428111727272626 +0.000000000000000,0.000017000000000,0.000135000000000,0.000127000000000,0.001615000000000,0.000791000000000,0.002734000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1011.000000000000000,192.000000000000000,121.000000000000000,0.170578883496370,0.166353065557405,121.000000000000000,0.246202540171952 +0.000000000000000,0.000019000000000,0.000178000000000,0.000136000000000,0.001520000000000,0.000741000000000,0.002636000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1005.000000000000000,203.000000000000000,121.000000000000000,0.156640038600278,0.207055055847171,121.000000000000000,0.306441476765396 +0.000000000000000,0.000033000000000,0.000177000000000,0.000135000000000,0.001641000000000,0.000861000000000,0.003288000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,999.000000000000000,186.000000000000000,121.000000000000000,0.143274685502394,0.179257874186524,121.000000000000000,0.265301654036128 +0.000000000000000,0.000015000000000,0.000129000000000,0.000142000000000,0.001443000000000,0.000740000000000,0.002555000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,202.000000000000000,121.000000000000000,0.156881076812533,0.218447877056405,121.000000000000000,0.323302867722382 +0.000000000000000,0.000036000000000,0.000177000000000,0.000151000000000,0.001530000000000,0.000725000000000,0.002741000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,820.000000000000000,168.000000000000000,121.000000000000000,0.155951030020751,0.213023655758012,121.000000000000000,0.315275031106667 +0.000000000000000,0.000030000000000,0.000196000000000,0.000128000000000,0.001426000000000,0.000831000000000,0.002660000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,816.000000000000000 \ No newline at end of file diff --git a/GPU_version/svo/test/data/.gitignore b/GPU_version/svo/test/data/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/GPU_version/svo/test/data/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/GPU_version/svo/test/results/.gitignore b/GPU_version/svo/test/results/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/GPU_version/svo/test/results/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/GPU_version/svo/test/test_depth_filter.cpp b/GPU_version/svo/test/test_depth_filter.cpp new file mode 100644 index 0000000..4d22493 --- /dev/null +++ b/GPU_version/svo/test/test_depth_filter.cpp @@ -0,0 +1,206 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +struct ConvergedSeed { + int x_, y_; + double depth_, error_; + ConvergedSeed(int x, int y, double depth, double error) : + x_(x), y_(y), depth_(depth), error_(error) + {} +}; + +/// DepthFilter Test-Fixture +class DepthFilterTest { + public: + DepthFilterTest(); + virtual ~DepthFilterTest(); + void depthFilterCb(svo::Point* point, double depth_sigma2); + void testReconstruction(std::string dataset_dir, std::string experiment_name); + + std::list results_; + std::vector errors_; + size_t n_converged_seeds_; + vk::PinholeCamera* cam_; + svo::DepthFilter* depth_filter_; + svo::FramePtr frame_ref_; + svo::FramePtr frame_cur_; + cv::Mat depth_ref_; +}; + +DepthFilterTest::DepthFilterTest() : + n_converged_seeds_(0), + cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)), + depth_filter_(NULL) +{ + errors_.reserve(1000); +} + +DepthFilterTest::~DepthFilterTest() +{ + delete cam_; +} + +void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2) +{ + double depth = (frame_ref_->pos() - point->pos_).norm(); + double error = fabs(depth - depth_ref_.at(point->obs_.front()->px[1], + point->obs_.front()->px[0])); + results_.push_back(ConvergedSeed( + point->obs_.front()->px[0], point->obs_.front()->px[1], depth, error)); + errors_.push_back(error); + delete point->obs_.front(); + delete point; + ++n_converged_seeds_; +} + +void DepthFilterTest::testReconstruction( + std::string dataset_dir, + std::string experiment_name) +{ + vk::FileReader sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + std::cout << "RUN EXPERIMENT: read " << sequence.size() << " dataset entries." << std::endl; + std::vector::iterator it = sequence.begin(); + vk::Timer t; + std::list n_converged_per_iteration; + + svo::feature_detection::DetectorPtr feature_detector( + new svo::feature_detection::FastDetector( + cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels())); + svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2); + depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb); + depth_filter_->options_.verbose = true; + + for(int i=0; it != sequence.end() && i<20; ++it, ++i) + { + std::string img_name(dataset_dir+"/img/" + (*it).image_name_ + "_0.png"); + printf("reading image: '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + Sophus::SE3 T_w_f(it->q_, it->t_); + if(i == 0) + { + // create reference frame and load ground truth depthmap + frame_ref_ = boost::make_shared(cam_, img, 0.0); + frame_ref_->T_f_w_ = T_w_f.inverse(); + depth_filter_->addKeyframe(frame_ref_, 2, 0.5); + vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/" + (*it).image_name_ + "_0.depth", *cam_, depth_ref_); + continue; + } + + n_converged_seeds_ = 0; + frame_cur_ = boost::make_shared(cam_, img, 0.0); + frame_cur_->T_f_w_ = T_w_f.inverse(); + depth_filter_->addFrame(frame_cur_); + n_converged_per_iteration.push_back(n_converged_seeds_); + } + printf("Experiment '%s' took %f ms\n", experiment_name.c_str(), t.stop()*1000); + + // compute mean, median and variance of error in converged area + { + printf("# converged: \t %zu (ref: 287)\n", errors_.size()); + double sum_error = 0; + std::for_each(errors_.begin(), errors_.end(), [&](double& e){sum_error+=e;}); + printf("mean error: \t %f cm (ref: 0.080357)\n", sum_error*100/errors_.size()); + std::vector::iterator it = errors_.begin()+0.5*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("50-percentile: \t %f cm (ref: 0.062042)\n", *it*100); + it = errors_.begin()+0.8*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("80-percentile: \t %f cm (ref: 0.124526)\n", *it*100); + it = errors_.begin()+0.95*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("95-percentile: \t %f cm (ref: 0.200417)\n", *it*100); + } + + // trace error + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt"); + std::ofstream ofs(trace_name.c_str()); + for(std::list::iterator i=results_.begin(); i!=results_.end(); ++i) + ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl; + ofs.close(); + + // trace convergence rate + trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt"; + ofs.open(trace_name.c_str()); + for(std::list::iterator it=n_converged_per_iteration.begin(); + it!=n_converged_per_iteration.end(); ++it) + ofs << *it << std::endl; + ofs.close(); + + // write ply file for pointcloud visualization in Meshlab + trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply"; + ofs.open(trace_name.c_str()); + ofs << "ply" << std::endl + << "format ascii 1.0" << std::endl + << "element vertex " << results_.size() << std::endl + << "property float x" << std::endl + << "property float y" << std::endl + << "property float z" << std::endl + << "property uchar blue" << std::endl + << "property uchar green" << std::endl + << "property uchar red" << std::endl + << "end_header" << std::endl; + + for(std::list::iterator i=results_.begin(); i!=results_.end(); ++i) + { + cv::Vec3b c = frame_ref_->img_pyr_[0].at(i->y_, i->x_); + Eigen::Vector3d p = cam_->cam2world(i->x_, i->y_)*i->depth_; + ofs << p[0] << " " << p[1] << " " << p[2] << " " + << (int) c[0] << " " << (int) c[1] << " " << (int) c[2] << std::endl; + } +} + +} // namespace + +int main(int argc, char** argv) +{ + DepthFilterTest test; + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string experiment_name("sin2_tex2_h1_v8_d"); + test.testReconstruction(dataset_dir, experiment_name); + return 0; +} diff --git a/GPU_version/svo/test/test_feature_alignment.cpp b/GPU_version/svo/test/test_feature_alignment.cpp new file mode 100644 index 0000000..dac8911 --- /dev/null +++ b/GPU_version/svo/test/test_feature_alignment.cpp @@ -0,0 +1,124 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include "test_utils.h" +#if __ARM_NEON__ +#include +#endif + +using namespace Eigen; + +void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px, + cv::Mat& ref_patch_with_border) +{ + // compute interpolation weights + const int u_r = floor(px[0]); + const int v_r = floor(px[1]); + const float subpix_u = px[0]-u_r; + const float subpix_v = px[1]-v_r; + const float wTL = (1.0-subpix_u)*(1.0-subpix_v); + const float wTR = subpix_u * (1.0-subpix_v); + const float wBL = (1.0-subpix_u)*subpix_v; + const float wBR = subpix_u * subpix_v; + + // loop through search_patch, interpolate + ref_patch_with_border = cv::Mat(10, 10, CV_8UC1); + uint8_t* patch_ptr = ref_patch_with_border.data; + const int stride = img.step.p[0]; + for(int y=0; y<10; ++y) + { + uint8_t* img_ptr = (uint8_t*) img.data + (v_r+y-4-1)*stride + u_r-4-1; + for(int x=0; x<10; ++x, ++patch_ptr, ++img_ptr) + *patch_ptr = wTL*img_ptr[0] + wTR*img_ptr[1] + wBL*img_ptr[stride] + wBR*img_ptr[stride+1]; + } +} + +int main(int argc, char **argv) +{ + + std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(img.type() == CV_8UC1); + + // TODO: test on corner/gradient features! + Vector2d px_true(130.2,120.3); + + // create reference patch with border + cv::Mat ref_patch_with_border; + generateRefPatchNoWarpInterpolate(img, px_true, ref_patch_with_border); + + // create reference patch, aligned + uint8_t* ref_patch = vk::aligned_mem::aligned_alloc(64, 16); + uint8_t* ref_patch_ptr = ref_patch; + for(int y=1; y<9; ++y) + { + uint8_t* ref_patch_border_ptr = ref_patch_with_border.data + y*10 + 1; + for(int x=0; x<8; ++x, ++ref_patch_border_ptr, ++ref_patch_ptr) + *ref_patch_ptr = *ref_patch_border_ptr; + } + + Vector2d px_est, px_error(-1.1, -0.8); + double h_inv; + vk::Timer t; + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + Vector2f dir = px_error.normalized().cast(); + svo::feature_alignment::align1D(img, dir, ref_patch_with_border.data, ref_patch, 3, px_est, h_inv); + } + Vector2d e = px_est-px_true; + printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.982000ms, 0.000033px) \n", t.stop()*1000, e.norm()); + + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D(img, ref_patch_with_border.data, ref_patch, 3, px_est, true); + } + e = px_est-px_true; + printf("1000Xalign 2D took %fms, error = %fpx \t (ref i7-W520: 2.306000ms, 0.015102px)\n", t.stop()*1000, e.norm()); + +#ifdef __SSE2__ + // Note, this KLT implementation is not invariant to illuminatino changes! + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D_SSE2(img, ref_patch_with_border.data, ref_patch, 3, px_est); + } + e = px_est-px_true; + printf("1000Xalign 2D SSE2 %fms, error = %fpx \t (ref i7-W520: 0.460000ms, 0.021881px)\n", t.stop()*1000, e.norm()); +#endif + +#ifdef __ARM_NEON__ + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D_NEON(img, ref_patch_with_border.data, ref_patch, 3, px_est); + } + e = px_est-px_true; + printf("1000Xalign 2D NEON %fms, error = %fpx \t (ref Odroid-U2: ?, ?)\n", t.stop()*1000, e.norm()); +#endif + + return 0; +} diff --git a/GPU_version/svo/test/test_feature_detection.cpp b/GPU_version/svo/test/test_feature_detection.cpp new file mode 100644 index 0000000..3ba2b2f --- /dev/null +++ b/GPU_version/svo/test/test_feature_detection.cpp @@ -0,0 +1,74 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +using namespace Eigen; +using namespace std; + +void testCornerDetector() +{ + std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(img.type() == CV_8UC1 && !img.empty()); + + vk::AbstractCamera* cam = new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092); + svo::FramePtr frame(new svo::Frame(cam, img, 0.0)); + + // Corner detection + vk::Timer t; + svo::Features fts; + svo::feature_detection::FastDetector fast_detector( + img.cols, img.rows, svo::Config::gridSize(), svo::Config::nPyrLevels()); + for(int i=0; i<100; ++i) + { + fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts); + } + printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 7.166360ms, 40000)\n", t.stop()*10, fts.size()); + printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n"); + cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3); + cv::cvtColor(img, img_rgb, CV_GRAY2RGB); + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ + cv::circle(img_rgb, cv::Point2f(i->px[0], i->px[1]), 4*(i->level+1), cv::Scalar(0,255,0), 1); + }); + cv::imshow("ref_img", img_rgb); + cv::waitKey(0); + + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ delete i; }); +} + +} // namespace + + +int main(int argc, char **argv) +{ + testCornerDetector(); + return 0; +} diff --git a/GPU_version/svo/test/test_matcher.cpp b/GPU_version/svo/test/test_matcher.cpp new file mode 100644 index 0000000..0b65cb9 --- /dev/null +++ b/GPU_version/svo/test/test_matcher.cpp @@ -0,0 +1,200 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +/// Matcher Test-Fixture +class MatcherTest { + public: + MatcherTest() + { + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + + // load images + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string img_name(dataset_dir+"/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img_ref(cv::imread(img_name, 0)); + img_name = std::string(dataset_dir+"/img/frame_000006_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img_cur(cv::imread(img_name, 0)); + assert(!img_ref.empty() && !img_cur.empty()); + + // create frame + frame_ref_ = new svo::Frame(cam_, img_ref, 1.0); + frame_cur_ = new svo::Frame(cam_, img_cur, 2.0); + ref_ftr_ = new svo::Feature(frame_ref_, Eigen::Vector2d(300, 260), 0); + + // set poses + Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000); + Eigen::Vector3d t_w_cur(0.5673, 0.5641, 2.0000); + Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0); + Eigen::Quaterniond q_w_cur(0.0, 0.8235, 0.2130, 0.0); + frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse(); + frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur, t_w_cur).inverse(); + + // load ground-truth depth + vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000002_0.depth", *cam_, depth_ref_); + } + + virtual ~MatcherTest() + { + delete cam_; + delete frame_ref_; + delete frame_cur_; + delete ref_ftr_; + } + + // Individual tests + void testEpipolarSearchFullImg(); + void testWarpAffine(); + + // Objects declared here can be used by all tests + vk::PinholeCamera* cam_; + svo::Frame* frame_ref_; + svo::Frame* frame_cur_; + svo::Feature* ref_ftr_; + cv::Mat depth_ref_; +}; + +void MatcherTest::testEpipolarSearchFullImg() +{ + svo::Matcher matcher; + cv::Mat depthmap(frame_ref_->cam_->height(), frame_ref_->cam_->width(), CV_32FC1, cv::Scalar(0)); + cv::Mat error_map(depthmap.rows, depthmap.cols, CV_32FC1, cv::Scalar(0)); + cv::Mat mask(depthmap.rows, depthmap.cols, CV_8UC1, cv::Scalar(1)); + double depth_range = 0.8; + size_t n_converged = 0; + double sum_error = 0; + std::vector errors; errors.reserve(depthmap.cols*depthmap.rows); + for(int y=4; y(y, x); + double depth_estimate; + int res = matcher.findEpipolarMatchDirect( + *frame_ref_, *frame_cur_, ftr, depth_gt, fmax(depth_gt-depth_range,0.0), + depth_gt+depth_range, depth_estimate); + if(res) + { + depthmap.at(y,x) = depth_estimate; + error_map.at(y,x) = fabsf(depth_estimate-depth_gt); + ++n_converged; + sum_error += error_map.at(y,x); + errors.push_back(error_map.at(y,x)); + } + else + mask.at(y,x) = 0; + } + } + + // compute mean, median and variance of error in converged area + printf("n converged: \t %zu mm (ref: 216114)\n", n_converged); + printf("mean error: \t %f mm (ref: 0.410084)\n", sum_error*100/n_converged); + std::vector::iterator it = errors.begin()+0.5*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("50-percentile: \t %f mm (ref: 0.083203)\n", *it*100); + it = errors.begin()+0.8*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("80-percentile: \t %f mm (ref: 0.161824)\n", *it*100); + it = errors.begin()+0.95*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("95-percentile: \t %f mm (ref: 0.263539)\n", *it*100); + + // save results to file + std::ofstream output_stream; + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string output_filename(trace_dir + "/depthmap.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)depthmap.data, depthmap.rows*depthmap.cols*sizeof(float)); + output_stream.close(); + + output_filename.assign(trace_dir + "/depthmap_error.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)error_map.data, error_map.rows*error_map.cols*sizeof(float)); + output_stream.close(); + + output_filename.assign(trace_dir + "/depthmap_mask.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)mask.data, mask.rows*mask.cols*sizeof(uint8_t)); + output_stream.close(); +} + +void MatcherTest::testWarpAffine() +{ + const int halfpatch_size = 15; + const int patch_size=halfpatch_size*2; + svo::Matcher matcher; + double depth = 1.0; + + Sophus::SE3 T_cur_ref(frame_cur_->T_f_w_*frame_ref_->T_f_w_.inverse()); + Eigen::Vector2d px_cur(frame_cur_->cam_->world2cam(T_cur_ref*(ref_ftr_->f*depth))); + + // compute reference patch + cv::Mat ref_patch(matcher.halfpatch_size_+1, matcher.halfpatch_size_+1, + CV_8U, matcher.patch_with_border_); + + Eigen::Matrix2d A_cur_ref; + svo::warp::getWarpMatrixAffine( + *cam_, *cam_, ref_ftr_->px, ref_ftr_->f, 1.0, T_cur_ref, ref_ftr_->level, A_cur_ref); + int level_cur = svo::warp::getBestSearchLevel(A_cur_ref, svo::Config::nPyrLevels()-1); + svo::warp::warpAffine( + A_cur_ref, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, + ref_ftr_->level, level_cur, matcher.halfpatch_size_+1, matcher.patch_with_border_); + + // copy current patch + Eigen::Vector2i pxi(px_cur[0]/(1<img_pyr_.at(level_cur)(cv::Range(pxi[1]-halfpatch_size, + pxi[1]+halfpatch_size), + cv::Range(pxi[0]-halfpatch_size, + pxi[0]+halfpatch_size)).copyTo(cur_patch); + + // display results + cv::circle(frame_cur_->img_pyr_[level_cur], + cv::Point2f(pxi[0], pxi[1]), + patch_size, cv::Scalar(255), 2); + cv::circle(frame_ref_->img_pyr_[ref_ftr_->level], + cv::Point2f(ref_ftr_->px[0]/(1<level), ref_ftr_->px[1]/(1<level)), + patch_size, cv::Scalar(255), 2); + cv::imshow("cur_patch", cur_patch); + cv::imshow("ref_patch", ref_patch); + cv::imshow("ref_img", frame_ref_->img_pyr_[ref_ftr_->level]); + cv::imshow("cur_img", frame_cur_->img_pyr_[level_cur]); + cv::waitKey(0); +} + +} // namespace + +int main(int argc, char** argv) +{ + MatcherTest test; + test.testEpipolarSearchFullImg(); +// test.testWarpAffine(); + return 0; +} diff --git a/GPU_version/svo/test/test_pipeline.cpp b/GPU_version/svo/test/test_pipeline.cpp new file mode 100644 index 0000000..ede3b25 --- /dev/null +++ b/GPU_version/svo/test/test_pipeline.cpp @@ -0,0 +1,98 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace svo { + +class BenchmarkNode +{ + vk::AbstractCamera* cam_; + svo::FrameHandlerMono* vo_; + +public: + BenchmarkNode(); + ~BenchmarkNode(); + void runFromFolder(); +}; + +BenchmarkNode::BenchmarkNode() +{ + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + vo_ = new svo::FrameHandlerMono(cam_); + vo_->start(); +} + +BenchmarkNode::~BenchmarkNode() +{ + delete vo_; + delete cam_; +} + +void BenchmarkNode::runFromFolder() +{ + for(int img_id = 2; img_id < 188; ++img_id) + { + // load image + std::stringstream ss; + ss << svo::test_utils::getDatasetDir() << "/sin2_tex2_h1_v8_d/img/frame_" + << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png"; + if(img_id == 2) + std::cout << "reading image " << ss.str() << std::endl; + cv::Mat img(cv::imread(ss.str().c_str(), 0)); + assert(!img.empty()); + + // process frame + vo_->addImage(img, 0.01*img_id); + + // display tracking quality + if(vo_->lastFrame() != NULL) + { + std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t" + << "#Features: " << vo_->lastNumObservations() << " \t" + << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n"; + + // access the pose of the camera via vo_->lastFrame()->T_f_w_. + } + } +} + +} // namespace svo + +int main(int argc, char** argv) +{ + { + svo::BenchmarkNode benchmark; + benchmark.runFromFolder(); + } + printf("BenchmarkNode finished.\n"); + return 0; +} + diff --git a/GPU_version/svo/test/test_pose_optimizer.cpp b/GPU_version/svo/test/test_pose_optimizer.cpp new file mode 100644 index 0000000..59e0b46 --- /dev/null +++ b/GPU_version/svo/test/test_pose_optimizer.cpp @@ -0,0 +1,110 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +using namespace svo; +using namespace Eigen; + +/// Pose Optimizer Test-Fixture +class PoseOptimizerTest { + public: + PoseOptimizerTest() + { + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + + // load image + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string img_name(dataset_dir+"/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + // create frame + frame_.reset(new svo::Frame(cam_, img, 1.0)); + + // set pose + Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000); + Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0); + frame_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse(); + + // load ground-truth depth + vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000002_0.depth", *cam_, depthmap_); + + // detect features + feature_detection::FastDetector detector( + cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()); + detector.detect(frame_.get(), frame_->img_pyr_, Config::triangMinCornerScore(), frame_->fts_); + size_t n_fts = 0; + std::for_each(frame_->fts_.begin(), frame_->fts_.end(), [&](Feature* i){ + Point* point(new Point(frame_->f2w(i->f*depthmap_.at(i->px[1], i->px[0])), i)); + i->point = point; + ++n_fts; + }); + printf("Added %zu features to frame.\n", n_fts); + } + + virtual ~PoseOptimizerTest() + { + delete cam_; + } + + // Individual tests + void test(const Vector3d& pose_disturbance, double pixel_sigma2); + + // Objects declared here can be used by all tests + vk::PinholeCamera* cam_; + FramePtr frame_; + cv::Mat depthmap_; +}; + +void PoseOptimizerTest::test(const Vector3d& pose_disturbance, double pixel_sigma2) +{ + printf("Add %f px noise to each observation\n", pixel_sigma2); + for(Features::iterator it=frame_->fts_.begin(); it!=frame_->fts_.end(); ++it) + { + (*it)->px += Vector2d(vk::Sample::gaussian(pixel_sigma2), vk::Sample::gaussian(pixel_sigma2)); + (*it)->f = frame_->c2f((*it)->px); + } + frame_->T_f_w_ = frame_->T_f_w_*SE3(Matrix3d::Identity(), pose_disturbance); + double estimated_scale, error_init, error_final; + size_t num_obs; + pose_optimizer::optimizeGaussNewton( + Config::reprojThresh(), 10, true, frame_, + estimated_scale, error_init, error_final, num_obs); +} + +} // namespace + +int main(int argc, char** argv) +{ + PoseOptimizerTest test; + test.test(Eigen::Vector3d(0.2, 0.2, 0.2), 1.0); + return 0; +} diff --git a/GPU_version/svo/test/test_sparse_img_align.cpp b/GPU_version/svo/test/test_sparse_img_align.cpp new file mode 100644 index 0000000..1312da0 --- /dev/null +++ b/GPU_version/svo/test/test_sparse_img_align.cpp @@ -0,0 +1,163 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +/// SparseImgAlign Test-Fixture +class SparseImgAlignTest { + public: + + SparseImgAlignTest(); + virtual ~SparseImgAlignTest(); + void testSequence( + const std::string& dataset_dir, + const std::string& experiment_name, + svo::feature_detection::AbstractDetector* feature_detector); + + vk::PinholeCamera* cam_; + svo::FramePtr frame_ref_; + svo::FramePtr frame_cur_; +}; + +SparseImgAlignTest::SparseImgAlignTest() : + cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)) +{} + +SparseImgAlignTest::~SparseImgAlignTest() +{} + +void SparseImgAlignTest::testSequence( + const std::string& dataset_dir, + const std::string& experiment_name, + svo::feature_detection::AbstractDetector* feature_detector) +{ + vk::FileReader sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + printf("RUN EXPERIMENT: read %zu dataset entries.\n", sequence.size()); + std::vector::iterator iter = sequence.begin(); + std::list translation_error; + + Sophus::SE3 T_prev_w, T_prevgt_w; + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt"); + std::ofstream ofs(trace_name.c_str()); + for(int i=0; iter != sequence.end() && i<30; ++iter, ++i) + { + // load img + // IMPORTANT: We need to flip the image because the Povray dataset has a + // negative focal length in y direction which we didn't set. + std::string img_name(dataset_dir+"/img/" + iter->image_name_ + "_0.png"); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + // load pose + Sophus::SE3 T_w_gt(iter->q_, iter->t_); + Sophus::SE3 T_gt_w = T_w_gt.inverse(); // ground-truth + + if(i==0) + { + // set reference frame + frame_ref_.reset(new svo::Frame(cam_, img, 0.0)); + frame_ref_->T_f_w_ = T_gt_w; + + // load ground-truth depth + cv::Mat depthmap; + vk::blender_utils::loadBlenderDepthmap( + dataset_dir+"/depth/"+iter->image_name_+"_0.depth", *cam_, depthmap); + + // extract features, generate features with 3D points + feature_detector->detect( + frame_ref_.get(), frame_ref_->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref_->fts_); + std::for_each(frame_ref_->fts_.begin(), frame_ref_->fts_.end(), [&](svo::Feature* i) { + Eigen::Vector3d pt_pos_cur = i->f*depthmap.at(i->px[1], i->px[0]); + Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur; + svo::Point* pt = new svo::Point(pt_pos_w, i); + i->point = pt; + }); + + printf("Added %zu 3d pts to the reference frame.\n", frame_ref_->nObs()); + T_prev_w = frame_ref_->T_f_w_; + T_prevgt_w = T_gt_w; + continue; + } + + frame_cur_.reset(new svo::Frame(cam_, img, 0.0)); + //frame_cur_->T_f_w_ = frame_ref_->T_f_w_; // start at reference frame + frame_cur_->T_f_w_ = T_prev_w; // start at last frame + + // run image align + vk::Timer t; + svo::SparseImgAlign img_align(svo::Config::kltMaxLevel(), svo::Config::kltMinLevel(), + 30, svo::SparseImgAlign::GaussNewton, false, false); + img_align.run(frame_ref_, frame_cur_); + + // compute error + Sophus::SE3 T_f_gt = frame_cur_->T_f_w_ * T_gt_w.inverse(); + translation_error.push_back(T_f_gt.translation().norm()); + printf("[%3.i] time = %f ms \t |t| = %f \t translation error = %f\n", + i, t.stop()*1000, (frame_ref_->T_f_w_*T_gt_w.inverse()).translation().norm(), + translation_error.back()); + + // save old pose for next iteration + T_prev_w = frame_cur_->T_f_w_; + T_prevgt_w = T_gt_w; + + ofs << frame_cur_->T_f_w_.inverse().translation().transpose() << " " + << T_gt_w.inverse().translation().transpose() << std::endl; + } + ofs.close(); + + // trace error + trace_name = trace_dir + "/sparse_img_align_" + experiment_name + "_trans_error.txt"; + ofs.open(trace_name.c_str()); + for(std::list::iterator it=translation_error.begin(); it!=translation_error.end(); ++it) + ofs << *it << std::endl; + ofs.close(); +} + +} // namespace + + +int main(int argc, char** argv) +{ + std::string experiment_name("flying_room_1_rig_1_fast_minlev0"); + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + svo::Config::triangMinCornerScore() = 20; + svo::Config::kltMinLevel() = 0; + SparseImgAlignTest test; + svo::feature_detection::FastDetector detector( + test.cam_->width(), test.cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()); + test.testSequence(dataset_dir, experiment_name, &detector); + return 0; +} diff --git a/GPU_version/svo/test/test_utils.h b/GPU_version/svo/test/test_utils.h new file mode 100644 index 0000000..2759fe0 --- /dev/null +++ b/GPU_version/svo/test/test_utils.h @@ -0,0 +1,57 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef TEST_UTILS_H_ +#define TEST_UTILS_H_ + +#include +#include // for getenv +#ifdef SVO_USE_ROS +# include +# include +#endif + +namespace svo { +namespace test_utils { + +std::string getDatasetDir() +{ + const char* env_dir = std::getenv("SVO_DATASET_DIR"); +#ifdef SVO_USE_ROS + std::string dataset_dir(ros::package::getPath("svo")+"/test/data"); + if(env_dir != NULL) + dataset_dir = std::string(env_dir); + return dataset_dir; +#else + return std::string(env_dir); +#endif +} + +std::string getTraceDir() +{ +#ifdef SVO_USE_ROS + std::string default_dir(ros::package::getPath("svo")+"/test/results"); + return vk::getParam("svo/trace_dir", default_dir); +#else + return "/tmp"; +#endif +} + +} // namespace test_utils +} // namespace svo + + +#endif // TEST_UTILS_H_