public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
Christian Forster
2014-03-12 16:39:58 +01:00
commit 76801f72ac
98 changed files with 14580 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+137
View File
@@ -0,0 +1,137 @@
SVO
===
Disclaimer
----------
SVO has been tested under ROS Groovy and Hydro and Ubuntu 12.04 and 13.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
Installation Instructions
-------------------------
#### OPTIONAL: g2o - General Graph Optimization
Only required if you want to run bundle adjustment. It is not necessary for visual odometry.
g2o requires the following system dependencies: `cmake, libeigen3-dev, libsuitesparse-dev, libqt4-dev, qt4-qmake, libqglviewer-qt4-dev`, install them with `apt-get`
I suggest an out-of-source build of g2o:
cd workspace
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
mkdir build
cd build
cmake ..
make
sudo make install
If you don't want to make a system install, then you can replace the cmake command with `cmake .. -DCMAKE_INSTALL_PREFIX:PATH=$HOME/installdir`
#### Sophus - Lie groups
Sophus by Hauke Strasdat implements Lie groups that we need to describe rigid body transformations.
cd workspace
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build
cd build
cmake ..
make
You don't need to install the library since `cmake ..` writes the package location to `~.cmake/packages/` where CMake can later find it.
#### Fast Detector
The Fast detector by Edward Rosten is used to detect corners.
To simplify installation we provide a CMake package that contains the fast detector from the libCVD library (http://www.edwardrosten.com/cvd/).
cd workspace
git clone https://github.com/uzh-rpg/fast.git
cd fast
mkdir build
cd build
cmake ..
make
#### ViKit - Some useful tools that we need
ViKit for instance contains camera models, some math and interpolation functions that SVO needs.
ViKit is a catkin project, therefore, download it into your catkin workspace source folder.
cd catkin_ws/src
git clone https://github.com/uzh-rpg/rpg_vikit.git
#### SVO
Now we are ready to build SVO.
Clone it into your catkin workspace
cd catkin_ws/src
git clone https://github.com/uzh-rpg/rpg_slam.git
If you installed g2o then set `HAVE_G2O` in `svo/CMakeLists.txt` to TRUE.
Then build
catkin_make
Run SVO on a Dataset
-------------------------
Download this example dataset: [rpg.ifi.uzh.ch/datasets/airground_rig_s3_2013-03-18_21-38-48.bag](http://rpg.ifi.uzh.ch/datasets/airground_rig_s3_2013-03-18_21-38-48.bag)
Open a new console and start SVO with the prepared launchfile:
roslaunch svo_ros test_rig3.launch
Open a new console and start RViz
rosrun rviz rviz
In RViz, load the configuration file (File > Open Config) which is stored in `svo_ros/rviz_config.rviz`.
Now you are ready to start the rosbag. Open a new console and change to the directory where you have downloaded the example dataset. Then type:
rosbag play airground_rig_s3_2013-03-18_21-38-48.bag
Now you should see the video with tracked features (green) and in RViz how the camera moves.
SVO GUI
-------
Type `rosrun rqt_svo rqt_svo` to run the SVO widget that displays the number of tracked features, the frame rate and provides some interface buttons.
Keyboard Shortcuts
------------------
Make sure to active the console window when pressing the keys.
* `s` Start/Restart
* `q` Quit
* `r` Reset
Parameter Settings
------------------
A description of all parameters which can be set via the launchfile is provided in `svo/include/config.h`. The default parameters can be viewed in `svo/src/config.cpp`. Moreover, some additional parameters (mainly rostopic names etc.) are read from the ros parameter server in `svo_ros/slam_node.cpp`.
Generating Code Documentation
-----------------------------
You can generate a Doxygen documentation as follows
cd svo
doxygen Doxyfile
Contributing
------------
You are very welcome to contribute by opening a pull request via Github.
I try to follow the ROS C++ style guide.
Licence
-------
The source code is released under GPLv3 licence. A professional edition license for closed-source projects is also available. Please contact `forster at ifi dot uzh dot ch` for further information.
rqt_svo/CMakeLists.txt
+22
View File
@@ -0,0 +1,22 @@
cmake_minimum_required(VERSION 2.8.3)
project(rqt_svo)
find_package(catkin REQUIRED COMPONENTS
rospy
rqt_gui
rqt_gui_py
)
catkin_package()
catkin_python_setup()
install(FILES plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY resource
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(PROGRAMS scripts/rqt_svo
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
rqt_svo/README.md
+10
View File
@@ -0,0 +1,10 @@
run with:
rosrun rqt_svo rqt_svo
or start `rqt` and display the widget.
If it does not work, try this:
rm ~/.config/ros.org/rqt_gui.ini
rqt
rqt_svo/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<package>
<name>rqt_svo</name>
<version>0.0.0</version>
<description>The rqt_svo package</description>
<maintainer email="[email protected]">cforster</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>rqt_gui</build_depend>
<build_depend>rqt_gui_py</build_depend>
<run_depend>rospy</run_depend>
<run_depend>rqt_gui</run_depend>
<run_depend>rqt_gui_py</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<rqt_gui plugin="${prefix}/plugin.xml"/>
</export>
</package>
rqt_svo/plugin.xml
+12
View File
@@ -0,0 +1,12 @@
<library path="src">
<class name="Svo" type="rqt_svo.svo.Svo" base_class_type="rqt_gui_py::Plugin">
<description>
Plugin for SVO - Semi-Direct Visual Odometry
</description>
<qtgui>
<label>Svo</label>
<icon type="theme">system-help</icon>
<statustip>Great user interface to provide real value.</statustip>
</qtgui>
</class>
</library>
rqt_svo/resource/widget.ui
+101
View File
@@ -0,0 +1,101 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>SvoWidget</class>
<widget class="QWidget" name="SvoWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>467</width>
<height>140</height>
</rect>
</property>
<property name="windowTitle">
<string>SVO</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout" stretch="0,0,0,0">
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QLabel" name="label">
<property name="text">
<string>SVO Namespace:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="topic_line_edit">
<property name="toolTip">
<string>Topic to send Twist message to</string>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_4" stretch="0">
<item>
<widget class="QLabel" name="svo_info_label">
<property name="toolTip">
<string>Current angular velocity</string>
</property>
<property name="text">
<string>Status Message</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<widget class="QProgressBar" name="num_tracked_bar">
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>100</number>
</property>
<property name="format">
<string>%p%</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QPushButton" name="button_start">
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="button_reset">
<property name="toolTip">
<string>Reset velocities to zero</string>
</property>
<property name="text">
<string>Reset</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="button_quit">
<property name="text">
<string>Quit</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
rqt_svo/scripts/rqt_svo
Executable
+10
View File
@@ -0,0 +1,10 @@
#!/usr/bin/env python
import sys
from rqt_svo.svo import Svo
from rqt_gui.main import Main
plugin = 'rqt_svo'
main = Main(filename=plugin)
sys.exit(main.main(sys.argv, standalone=plugin)) # , plugin_argument_provider=Svo.add_arguments
rqt_svo/setup.py
+12
View File
@@ -0,0 +1,12 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['rqt_svo'],
package_dir={'': 'src'},
scripts=['scripts/rqt_svo']
)
setup(**d)
rqt_svo/src/rqt_svo/.gitignore
+1
View File
@@ -0,0 +1 @@
*.pyc
rqt_svo/src/rqt_svo/__init__.py
View File
rqt_svo/src/rqt_svo/svo.py
+66
View File
@@ -0,0 +1,66 @@
#!/usr/bin/env python
import os
import rospy
import argparse
from qt_gui.plugin import Plugin
from .svo_widget import SvoWidget
class Svo(Plugin):
"""
Subclass of Plugin to display SVO status
"""
def __init__(self, context):
# Init Plugin
super(Svo, self).__init__(context)
self.setObjectName('SvoPlugin')
# Load arguments
# TODO load topic name from args
args = self._parse_args(context.argv())
# Create QWidget
self._widget = SvoWidget()
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
def _parse_args(self, argv):
parser = argparse.ArgumentParser(prog='rqt_svo', add_help=False)
group = parser.add_argument_group('Options for rqt_svo plugin')
group.add_argument('topic', type=argparse.FileType('r'), nargs='*', default=[], help='Svo Info Topic to display')
return parser.parse_args(argv)
# def shutdown_plugin(self):
# # self._unregister_publisher()
# pass
#
# def save_settings(self, plugin_settings, instance_settings):
# # TODO save intrinsic configuration, usually using:
# # instance_settings.set_value(k, v)
# pass
#
# def restore_settings(self, plugin_settings, instance_settings):
# # TODO restore intrinsic configuration, usually using:
# # v = instance_settings.value(k)
# pass
# def _unregister_publisher(self):
# if self._publisher is not None:
# self._publisher.unregister()
# self._publisher = None
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
rqt_svo/src/rqt_svo/svo_widget.py
+126
View File
@@ -0,0 +1,126 @@
#!/usr/bin/env python
import os
import rospy
import rospkg
import numpy as np
from python_qt_binding import loadUi
from python_qt_binding.QtGui import QWidget
from python_qt_binding.QtCore import QTimer, Slot
from svo_msgs.msg import Info
from std_msgs.msg import String
class SvoWidget(QWidget):
_last_info_msg = Info()
_publisher = None
_subscriber = None
_num_received_msgs = 0
def __init__(self, svo_namespace='svo'):
# Init QWidget
super(SvoWidget, self).__init__()
self.setObjectName('SvoWidget')
# load UI
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_svo'), 'resource', 'widget.ui')
loadUi(ui_file, self)
# init and start update timer for data, the timer calls the function update_info all 40ms
self._update_info_timer = QTimer(self)
self._update_info_timer.timeout.connect(self.update_info)
self._update_info_timer.start(40)
# set the functions that are called when a button is pressed
self.button_start.pressed.connect(self.on_start_button_pressed)
self.button_reset.pressed.connect(self.on_reset_button_pressed)
self.button_quit.pressed.connect(self.on_quit_button_pressed)
# set callback for changed topic
self.topic_line_edit.setText(svo_namespace)
self.register(svo_namespace)
self.topic_line_edit.textChanged.connect(self._on_topic_changed)
# TODO: set a timer when the last message was received and give a warning if it is too long ago!
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self.unregister()
self.register(topic)
def register(self, svo_namespace):
# Load parameters
max_num_features = rospy.get_param(svo_namespace+'/max_fts', 120)
# Feature bar
self.num_tracked_bar.setMaximum(max_num_features)
print('set maximum number of features to '+str(max_num_features))
# Subscribe to ROS Info topic and register callback
self._subscriber = rospy.Subscriber(svo_namespace+'/info', Info, self.info_cb)
# Initialize Publisher
self._publisher = rospy.Publisher(svo_namespace+'/remote_key', String)
def unregister(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = None
def info_cb(self, msg):
self._last_info_msg = msg
self._num_received_msgs += 1
def update_info(self):
info_text = 'Not Connected'
if self._num_received_msgs > 0:
fps = 0
if self._last_info_msg.processing_time > 0:
fps = 1.0/self._last_info_msg.processing_time
info_text = 'fps = %.2f' % fps
info_text += '\t #Features = ' + str(self._last_info_msg.num_matches)
info_text += '\t'
if self._last_info_msg.stage == 0:
info_text += '\t PAUSED'
elif self._last_info_msg.stage == 1:
info_text += '\t FIRST_FRAME'
elif self._last_info_msg.stage == 2:
info_text += '\t SECOND_FRAME'
elif self._last_info_msg.stage == 3:
info_text += '\t RUNNING'
if self._last_info_msg.tracking_quality == 0:
info_text += '\t CRITICAL'
elif self._last_info_msg.tracking_quality == 1:
info_text += '\t BAD TRACKING'
elif self._last_info_msg.tracking_quality == 2:
info_text += '\t GOOD TRACKING'
# set info text
self.svo_info_label.setText(info_text)
# set progress bar
self.num_tracked_bar.setValue(self._last_info_msg.num_matches)
def on_start_button_pressed(self):
print('START SLAM')
self.send_command('s')
def on_reset_button_pressed(self):
print('RESET SLAM')
self.send_command('r')
def on_quit_button_pressed(self):
print('QUIT SLAM')
self.send_command('q')
def send_command(self, cmd):
if self._publisher is None:
return
self._publisher.publish(String(cmd))
svo/CMakeLists.txt
+156
View File
@@ -0,0 +1,156 @@
################################################################################
# user build settings
SET(DEBUG FALSE)
SET(TRACE TRUE)
SET(HAVE_G2O FALSE)
################################################################################
SET(PROJECT_NAME 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 ARM_ARCHITECTURE environment variable on Odroid
IF(DEFINED ENV{ARM_ARCHITECTURE})
SET(IS_ARM TRUE)
# ADD_DEFINITIONS(-DHAVE_FAST_NEON)
ELSE()
SET(IS_ARM FALSE)
ENDIF()
# Set definitions
IF(DEBUG)
ADD_DEFINITIONS(-DSVO_DEBUG)
ENDIF()
IF(TRACE)
ADD_DEFINITIONS(-DSVO_TRACE)
ENDIF()
IF(HAVE_G2O)
ADD_DEFINITIONS(-DUSE_BUNDLE_ADJUSTMENT)
ENDIF()
ADD_DEFINITIONS(-DUSE_ROS)
# Set build flags
SET(CMAKE_CXX_FLAGS "-Wall -Werror -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -O0 -g")
IF(IS_ARM)
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -mfpu=neon -march=armv7-a -fomit-frame-pointer -fsee -funsafe-loop-optimizations -fno-signed-zeros -fno-math-errno -funroll-loops")
ELSE()
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -mmmx -msse -msse -msse2 -msse3 -mssse3 -fomit-frame-pointer -fsee -funsafe-loop-optimizations -fno-signed-zeros -fno-math-errno -funroll-loops")
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()
# Add catkin and required ROS packages
FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp vikit_common vikit_ros)
# Add plain cmake packages
FIND_PACKAGE(G2O REQUIRED)
FIND_PACKAGE(OpenCV REQUIRED)
FIND_PACKAGE(Eigen REQUIRED)
FIND_PACKAGE(Sophus REQUIRED)
FIND_PACKAGE(fast REQUIRED)
FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread signals)
# Describe catkin Project
catkin_package(
DEPENDS Eigen OpenCV Sophus Boost fast
CATKIN_DEPENDS roscpp vikit_common vikit_ros
INCLUDE_DIRS include
LIBRARIES svo
)
# Include dirs
INCLUDE_DIRECTORIES(
include
${Eigen_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
${fast_INCLUDE_DIRS}
)
IF(HAVE_G2O)
INCLUDE_DIRECTORIES(
${G2O_ROOT}/include
/usr/include/suitesparse # for cholmod
)
ENDIF()
# Set link libraries
LIST(APPEND LINK_LIBS
${OpenCV_LIBRARIES}
${Sophus_LIBRARIES}
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${fast_LIBRARIES}
)
IF(HAVE_G2O)
LIST(APPEND LINK_LIBS
g2o_core g2o_stuff g2o_solver_cholmod g2o_solver_csparse g2o_solver_dense g2o_solver_pcg g2o_types_sba
cholmod cxsparse
)
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/reprojection.cpp
src/feature_alignment.cpp
src/feature_detection.cpp
src/depth_filter.cpp
src/config.cpp
src/sparse_img_align.cpp)
IF(HAVE_G2O)
LIST(APPEND SOURCEFILES src/bundle_adjustment.cpp)
ENDIF(HAVE_G2O)
# Create svo library
ADD_LIBRARY(svo SHARED ${SOURCEFILES})
TARGET_LINK_LIBRARIES(svo ${LINK_LIBS})
################################################################################
# TESTS
IF(DEFINED ENV{SVO_DATASET_DIR})
ADD_DEFINITIONS(-DTEST_DATA_DIR=\"$ENV{SVO_DATASET_DIR}\")
ELSE()
ADD_DEFINITIONS(-DTEST_DATA_DIR=\"${PROJECT_SOURCE_DIR}/test/data\")
ENDIF()
ADD_DEFINITIONS(-DTEST_TRACE_DIR=\"${PROJECT_SOURCE_DIR}/test/results\")
ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
TARGET_LINK_LIBRARIES(test_feature_align svo)
ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp)
TARGET_LINK_LIBRARIES(test_pipeline svo)
ADD_EXECUTABLE(test_matcher test/test_matcher.cpp)
TARGET_LINK_LIBRARIES(test_matcher svo)
ADD_EXECUTABLE(test_feature_detection test/test_feature_detection.cpp)
TARGET_LINK_LIBRARIES(test_feature_detection svo)
ADD_EXECUTABLE(test_depth_filter test/test_depth_filter.cpp)
TARGET_LINK_LIBRARIES(test_depth_filter svo)
ADD_EXECUTABLE(test_sparse_img_align test/test_sparse_img_align.cpp)
TARGET_LINK_LIBRARIES(test_sparse_img_align svo)
ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp)
TARGET_LINK_LIBRARIES(test_pose_optimizer svo)
svo/CMakeModules/FindG2O.cmake
+113
View File
@@ -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)
svo/Doxyfile
+1782
View File
File diff suppressed because it is too large Load Diff
svo/doc/.gitignore
+2
View File
@@ -0,0 +1,2 @@
*
!.gitignore
svo/include/svo/bundle_adjustment.h
+103
View File
@@ -0,0 +1,103 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_BUNDLE_ADJUSTMENT_H_
#define SVO_BUNDLE_ADJUSTMENT_H_
#include <svo/global.h>
namespace g2o {
class EdgeProjectXYZ2UV;
class SparseOptimizer;
}
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
namespace svo {
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<FramePtr>* 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_
svo/include/svo/config.h
+159
View File
@@ -0,0 +1,159 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef NSLAM_CONFIG_H_
#define NSLAM_CONFIG_H_
#include <string>
#include <stdint.h>
#include <stdio.h>
namespace svo {
using namespace std;
/// 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; }
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;
};
} // namespace nslam
#endif // NSLAM_CONFIG_H_
svo/include/svo/depth_filter.h
+163
View File
@@ -0,0 +1,163 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_
#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
#include <svo/feature_detection.h>
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 angle, 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<boost::mutex> lock_t;
typedef boost::function<void ( const Seed&, const Vector3d& )> 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);
~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<Seed>& seeds);
/// Return a reference to the seeds. This is NOT THREAD SAFE!
std::list<Seed, aligned_allocator<Seed> >& 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);
private:
feature_detection::DetectorPtr feature_detector_;
callback_t seed_converged_cb_;
std::list<Seed, aligned_allocator<Seed> > 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<FramePtr> 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.
/// Initialize new seeds from a frame.
void initializeSeeds(FramePtr frame);
/// Update all seeds with a new measurement frame.
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_
svo/include/svo/feature.h
+62
View File
@@ -0,0 +1,62 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FEATURE_H_
#define SVO_FEATURE_H_
#include <svo/frame.h>
namespace svo {
/// A salient image region that is tracked across frames.
struct Feature
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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.
Feature(Frame* _frame, const Vector2d& _px, int _level)
: frame(_frame),
px(_px),
f(frame->cam_->cam2world(px)),
level(_level),
point(NULL)
{}
Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level)
: frame(_frame),
px(_px),
f(_f),
level(_level),
point(NULL)
{}
Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level)
: frame(_frame),
px(_px),
f(_f),
level(_level),
point(_point)
{}
};
} // namespace svo
#endif // SVO_FEATURE_H_
svo/include/svo/feature_alignment.h
+63
View File
@@ -0,0 +1,63 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FEATURE_ALIGNMENT_H_
#define SVO_FEATURE_ALIGNMENT_H_
#include <svo/global.h>
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_
svo/include/svo/feature_detection.h
+76
View File
@@ -0,0 +1,76 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FEATURE_DETECTION_H_
#define SVO_FEATURE_DETECTION_H_
#include <svo/global.h>
#include <svo/frame.h>
namespace svo {
/// Implementation of various feature detectors.
namespace feature_detection {
/// Temporary container used for corner detection. Features are initialized from these.
// TODO: why not use feature struct?
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<Corner> Corners;
/// All detectors should derive from this abstract class.
class AbstractDetector
{
public:
virtual ~AbstractDetector() {};
virtual void detect(
const ImgPyr& img_pyr,
const Features& fts,
const int cell_size,
const int n_levels,
const double detection_threshold,
Corners* corners) const = 0;
};
typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
/// FAST detector by Edward Rosten.
class FastDetector : public AbstractDetector
{
public:
FastDetector() {}
virtual ~FastDetector() {}
virtual void detect(
const ImgPyr& img_pyr,
const Features& fts,
const int cell_size,
const int n_levels,
const double detection_threshold,
Corners* corners) const;
};
} // namespace feature_detection
} // namespace svo
#endif // SVO_FEATURE_DETECTION_H_
svo/include/svo/frame.h
+130
View File
@@ -0,0 +1,130 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FRAME_H_
#define SVO_FRAME_H_
#include <algorithm>
#include <sophus/se3.h>
#include <vikit/math_utils.h>
#include <vikit/abstract_camera.h>
#include <boost/noncopyable.hpp>
#include <svo/global.h>
namespace g2o {
class VertexSE3Expmap;
}
typedef g2o::VertexSE3Expmap g2oFrameSE3;
namespace svo {
class Point;
class Frame;
struct Feature;
typedef list<Feature*> Features;
typedef vector<cv::Mat> 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<double, 6, 6> Cov_; //!< Covariance.
ImgPyr img_pyr_; //!< Image Pyramid.
Features fts_; //!< List of features in the image.
vector<Feature*> 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=0.0);
~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.
size_t nObs() const;
/// 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(); }
};
/// 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_
svo/include/svo/frame_handler_base.h
+116
View File
@@ -0,0 +1,116 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FRAME_HANDLER_BASE_H_
#define SVO_FRAME_HANDLER_BASE_H_
#include <queue>
#include <vikit/timer.h>
#include <vikit/ringbuffer.h>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <svo/global.h>
#include <svo/map.h>
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 { PAUSED, FIRST_FRAME, SECOND_FRAME, DEFAULT_FRAME };
enum TrackingQuality { INSUFFICIENT, BAD, GOOD };
enum UpdateResult { NO_KEYFRAME, IS_KEYFRAME, FAILURE };
FrameHandlerBase();
virtual ~FrameHandlerBase();
/// Set initial pose of the camera.
void setInitialPose(const SE3& T_frame_from_world) { T_f_w_init_ = T_frame_from_world; }
/// 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_; }
/// Access the depth filter.
DepthFilter* depthFilter() const { return depth_filter_; }
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.
SE3 T_f_w_init_; //!< Initial pose of the camera.
vk::Timer timer_; //!< Stopwatch to measure time to process frame.
vk::RingBuffer<double> acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
vk::RingBuffer<size_t> 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.
DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
/// Before a frame is processed, this function is called.
bool startFrameProcessingCommon();
/// When a frame is finished processing, this function is called.
int finishFrameProcessingCommon(
size_t update_id,
UpdateResult dropout,
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.
void setTrackingQuality(const size_t num_observations);
/// Optimize some of the observed 3D points.
void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
};
} // namespace nslam
#endif // SVO_FRAME_HANDLER_BASE_H_
svo/include/svo/frame_handler_mono.h
+75
View File
@@ -0,0 +1,75 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FRAME_HANDLER_H_
#define SVO_FRAME_HANDLER_H_
#include <vikit/abstract_camera.h>
#include <svo/frame_handler_base.h>
#include <svo/reprojection.h>
#include <svo/initialization.h>
namespace svo {
/// Monocular Visual Odometry Pipeline as described in the SVO paper.
class FrameHandlerMono : public FrameHandlerBase
{
public:
FrameHandlerMono(vk::AbstractCamera* cam);
~FrameHandlerMono();
/// Provide an image.
void addImage(const cv::Mat& img, double timestamp);
/// 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<FramePtr>& coreKeyframes() { return core_kfs_; }
/// Return the feature track to visualize the KLT tracking during initialization.
const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
protected:
vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
reprojection::Grid reprojection_grid_; //!< Grid used for reprojection to make sure we have evenly distributed features.
FramePtr new_frame_; //!< Current frame.
FramePtr last_frame_; //!< Last frame, not necessarily a keyframe.
set<FramePtr> core_kfs_; //!< Keyframes in the closer neighbourhood.
vector< pair<FramePtr,size_t> > 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.
/// Processes the first frame and sets it as a keyframe.
UpdateResult processFirstFrame();
/// Processes all frames after the first frame until a keyframe is selected.
UpdateResult processSecondFrame();
/// Processes all frames after the first two keyframes.
UpdateResult processFrame();
/// Reset the frame handler. Implement in derived class.
virtual void resetAll();
void setCoreKfs(size_t n_closest);
bool needNewKf(double scene_depth_mean);
};
} // namespace svo
#endif // SVO_FRAME_HANDLER_H_
svo/include/svo/global.h
+95
View File
@@ -0,0 +1,95 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_GLOBAL_H_
#define SVO_GLOBAL_H_
#include <list>
#include <vector>
#include <string>
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <vikit/performance_monitor.h>
#include <boost/shared_ptr.hpp>
#ifdef USE_ROS
#include <ros/console.h>
#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] "<<x<<"\033[0;0m"<<std::endl;
#ifdef SVO_DEBUG
#define SVO_DEBUG_STREAM(x) SVO_INFO_STREAM(x)
#else
#define SVO_DEBUG_STREAM(x)
#endif
#define SVO_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<<x<<"\033[0;0m"<<std::endl;
#define SVO_ERROR_STREAM(x) std::cerr<<"\033[1;31m[ERROR] "<<x<<"\033[0;0m"<<std::endl;
#include <chrono> // 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<std::chrono::system_clock> __log_stream_throttle__now__ = \
std::chrono::system_clock::now(); \
if (__log_stream_throttle__last_hit__ + rate <= \
std::chrono::duration_cast<std::chrono::seconds>( \
__log_stream_throttle__now__.time_since_epoch()).count()) { \
__log_stream_throttle__last_hit__ = \
std::chrono::duration_cast<std::chrono::seconds>( \
__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<Frame> FramePtr;
} // namespace svo
#endif // SVO_GLOBAL_H_
svo/include/svo/initialization.h
+83
View File
@@ -0,0 +1,83 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_INITIALIZATION_H
#define SVO_INITIALIZATION_H
#include <svo/global.h>
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<cv::Point2f> px_ref_; //!< keypoints to be tracked in reference frame.
vector<cv::Point2f> px_cur_; //!< tracked keypoints in current frame.
vector<Vector3d> f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image.
vector<Vector3d> f_cur_; //!< bearing vectors corresponding to the keypoints in the current image.
vector<double> disparities_; //!< disparity between first and second frame.
vector<int> inliers_; //!< inliers after the geometric check (e.g., Homography).
vector<Vector3d> 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<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec);
/// Compute optical flow (Lucas Kanade) for selected keypoints.
void trackKlt(
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities);
void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref);
} // namespace initialization
} // namespace nslam
#endif // NSLAM_INITIALIZATION_H
svo/include/svo/map.h
+134
View File
@@ -0,0 +1,134 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_MAP_H_
#define SVO_MAP_H_
#include <queue>
#include <boost/noncopyable.hpp>
#include <boost/thread.hpp>
#include <svo/global.h>
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<Point*, Feature*> PointCandidate;
typedef list<PointCandidate> 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(const Seed& seed, const Vector3d& xyz_world);
/// 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<FramePtr,double> >& 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;
/// 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_
svo/include/svo/matcher.h
+105
View File
@@ -0,0 +1,105 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_MATCHER_H_
#define SVO_MATCHER_H_
#include <svo/global.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
class Point;
class Frame;
class Feature;
/// Patch-matcher for reprojection-matching and epipolar search in triangulation.
namespace matcher {
const int g_halfpatch_size = 4; // do not change, because of sse instructions alignment.
const int g_patch_size = 8; // do not change, because of sse instructions alignment.
/// Manages the aligned memory of a patch. Aligned for SIMD instructions.
struct PatchData
{
const int patch_size;
uint8_t* patch;
uint8_t* patch_with_border;
PatchData(int patch_size);
~PatchData();
};
/// 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,
PatchData& ref_patch);
} // namespace warp
// TODO take function from vikit
bool depthFromTriangulation(
const SE3& T_cur_ref,
const Vector3d& f_ref,
const Vector3d& f_cur,
double& depth);
/// 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,
int& level_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,
const bool align_1d,
double& depth,
double& h_inv);
} // namespace matcher
} // namespace svo
#endif // SVO_MATCHER_H_
svo/include/svo/point.h
+83
View File
@@ -0,0 +1,83 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_POINT_H_
#define SVO_POINT_H_
#include <boost/noncopyable.hpp>
#include <svo/global.h>
namespace g2o {
class VertexSBAPointXYZ;
}
typedef g2o::VertexSBAPointXYZ g2oPoint;
namespace svo {
class Feature;
typedef Matrix<double, 2, 3> Matrix23d;
/// A 3D point that was triangulated from at least two corresponding image features.
class Point : boost::noncopyable
{
public:
enum PointType {DELETED, CANDIDATE, UNKNOWN_QUALITY, GOOD_QUALITY };
enum PointOrigin { TRIANGULATION, DEPTH, VOGIATZIS, IDFILTER };
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.
list<Feature*> obs_; //!< References to keyframes which observe the point.
size_t n_obs_; //!< Number of obervations, not only keyframes.
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.
PointOrigin origin_; //!< What is the source of the point, i.e. DEPTH if a kinect is used.
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_;
Point(Vector3d pos, PointOrigin origin);
~Point();
/// Add a reference to a frame.
void addFrameRef(Feature* ftr);
/// Remove reference to a frame.
bool deleteFrameRef(Frame* frame);
/// 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(); }
/// Point jacobian w.r.t frame f.
void pointJacobian(const Vector3d& p_in_f, const Matrix3d& R_f_w, Matrix23d& point_jac) const;
/// Optimize point position through minimizing the reprojection error.
void optimize(const size_t n_iter);
};
} // namespace svo
#endif // SVO_POINT_H_
svo/include/svo/pose_optimizer.h
+77
View File
@@ -0,0 +1,77 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_POSE_OPTIMIZER_H_
#define SVO_POSE_OPTIMIZER_H_
#include <svo/global.h>
namespace svo {
using namespace Eigen;
using namespace Sophus;
using namespace std;
typedef Matrix<double,6,6> Matrix6d;
typedef Matrix<double,2,6> Matrix26d;
typedef Matrix<double,6,1> Vector6d;
class Point;
/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame.
namespace pose_optimizer {
inline Matrix26d
frameJac(const SE3 & T,
const Vector3d & xyz,
const Vector2d & focal_length)
{
const Vector3d & xyz_trans = T*xyz;
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z*z;
Matrix26d frame_jac;
frame_jac(0,0) = -1./z *focal_length[0];
frame_jac(0,1) = 0;
frame_jac(0,2) = x/z_2 *focal_length[0];
frame_jac(0,3) = x*y/z_2 * focal_length[0];
frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length[0];
frame_jac(0,5) = y/z *focal_length[0];
frame_jac(1,0) = 0;
frame_jac(1,1) = -1./z *focal_length[1];
frame_jac(1,2) = y/z_2 *focal_length[1];
frame_jac(1,3) = (1+y*y/z_2) *focal_length[1];
frame_jac(1,4) = -x*y/z_2 *focal_length[1];
frame_jac(1,5) = -x/z *focal_length[1];
return frame_jac;
}
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_
svo/include/svo/reprojection.h
+98
View File
@@ -0,0 +1,98 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_REPROJECTION_H_
#define SVO_REPROJECTION_H_
#include <svo/global.h>
#include <svo/matcher.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
class Map;
class Point;
class MapPointCandidates;
/// 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.
namespace reprojection {
/// 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<Candidate, aligned_allocator<Candidate> > Cell;
typedef std::vector<Cell*> CandidateGrid;
/// The grid stores a set of candidate matches. For every grid cell we try to find one match.
struct Grid
{
CandidateGrid cells;
vector<int> cell_order;
int cell_size;
int grid_n_cols;
int grid_n_rows;
Grid();
void initialize(vk::AbstractCamera* cam);
~Grid();
};
/// Reset the grid
void resetGrid(Grid& grid);
/// Project points from the map into the image. First finds keyframes with
/// overlapping field of view and projects only those map-points.
void reprojectMap(
Map& map,
FramePtr frame,
Grid& grid,
MapPointCandidates& candidate_points,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
size_t& n_matches,
size_t& n_trials);
bool reprojectCell(
Cell& cell,
FramePtr frame,
Map& map,
MapPointCandidates& candidate_points,
size_t& n_trials);
bool reprojectPoint(
Grid& grid,
FramePtr frame,
Point* point);
} // namespace reprojection
} // namespace svo
#endif // SVO_REPROJECTION_H_
svo/include/svo/sparse_img_align.h
+78
View File
@@ -0,0 +1,78 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_SPARSE_IMG_ALIGN_H_
#define SVO_SPARSE_IMG_ALIGN_H_
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
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:
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);
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<double, 6, Dynamic, ColMajor> jacobian_cache_; //!< the first row is a flag whether the jacobian has been computed.
bool have_ref_patch_cache_;
cv::Mat ref_patch_cache_;
std::vector<bool> 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_
svo/package.xml
+24
View File
@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<package>
<name>svo</name>
<version>0.1.0</version>
<description>
Monocular Visual Odometry
</description>
<author>Christian Forster</author>
<maintainer email="[email protected]">Christian Forster</maintainer>
<license>GPLv3</license>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<build_depend>roscpp</build_depend>
<build_depend>vikit_common</build_depend>
<build_depend>vikit_ros</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>vikit_common</run_depend>
<run_depend>vikit_ros</run_depend>
</package>
svo/src/bundle_adjustment.cpp
+435
View File
@@ -0,0 +1,435 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <vikit/math_utils.h>
#include <boost/thread.hpp>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#include <svo/bundle_adjustment.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/config.h>
#include <svo/map.h>
#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<EdgeContainerSE3> 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<EdgeContainerSE3>::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<FramePtr>* 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<EdgeContainerSE3> edges;
set<Point*> mps;
list<Frame*> 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<FramePtr>::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<Point*>::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
Observations::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<g2o::OptimizableGraph::Vertex*>(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<FramePtr>::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<Frame*>::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it)
(*it)->v_kf_ = NULL;
// Update Mappoints
for(set<Point*>::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<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
{
if(it->edge_->chi2() > reproj_thresh_2_squared) //*(1<<it->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<EdgeContainerSE3> edges;
list< pair<FramePtr,Feature*> > 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<FramePtr>::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<FramePtr,Feature*>(*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<FramePtr>::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<FramePtr,Feature*> >::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<EdgeContainerSE3>::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<g2o::BlockSolver_6_3::PoseMatrixType>();
//linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
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<g2o::BlockSolverX::PoseMatrixType>();
//linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
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<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(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
svo/src/config.cpp
+95
View File
@@ -0,0 +1,95 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifdef USE_ROS
#include <vikit/params_helper.h>
#endif
#include <svo/config.h>
namespace svo {
Config::Config() :
#ifdef USE_ROS
trace_name(vk::getParam<string>("svo/trace_name", "svo")),
trace_dir(vk::getParam<string>("svo/trace_dir", "/tmp")),
n_pyr_levels(vk::getParam<int>("svo/n_pyr_levels", 3)),
use_imu(vk::getParam<bool>("svo/use_imu", false)),
core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)),
map_scale(vk::getParam<double>("svo/map_scale", 1.0)),
grid_size(vk::getParam<int>("svo/grid_size", 30)),
init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)),
init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)),
init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)),
klt_max_level(vk::getParam<int>("svo/klt_max_level", 4)),
klt_min_level(vk::getParam<int>("svo/klt_min_level", 2)),
reproj_thresh(vk::getParam<double>("svo/reproj_thresh", 2.0)),
poseoptim_thresh(vk::getParam<double>("svo/poseoptim_thresh", 2.0)),
poseoptim_num_iter(vk::getParam<int>("svo/poseoptim_num_iter", 10)),
structureoptim_max_pts(vk::getParam<int>("svo/structureoptim_max_pts", 20)),
structureoptim_num_iter(vk::getParam<int>("svo/structureoptim_num_iter", 5)),
loba_thresh(vk::getParam<double>("svo/loba_thresh", 2.0)),
loba_robust_huber_width(vk::getParam<double>("svo/loba_robust_huber_width", 1.0)),
loba_num_iter(vk::getParam<int>("svo/loba_num_iter", 0)),
kfselect_mindist(vk::getParam<double>("svo/kfselect_mindist", 0.12)),
triang_min_corner_score(vk::getParam<double>("svo/triang_min_corner_score", 20.0)),
triang_half_patch_size(vk::getParam<int>("svo/triang_half_patch_size", 4)),
subpix_n_iter(vk::getParam<int>("svo/subpix_n_iter", 10)),
max_n_kfs(vk::getParam<int>("svo/max_n_kfs", 10)),
img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)),
max_fts(vk::getParam<int>("svo/max_fts", 120)),
quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)),
quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40))
#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
svo/src/depth_filter.cpp
+362
View File
@@ -0,0 +1,362 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <vikit/math_utils.h>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <svo/global.h>
#include <svo/depth_filter.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/matcher.h>
#include <svo/config.h>
#include <svo/feature_detection.h>
namespace svo {
int Seed::batch_counter = 0;
int Seed::seed_counter = 0;
Seed::Seed(Feature* ftr, float angle, 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)
{
feature_detection::Corners corners;
feature_detector_->detect(frame->img_pyr_, frame->fts_, svo::Config::gridSize(),
svo::Config::nPyrLevels(), Config::triangMinCornerScore(),
&corners);
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
size_t new_seeds = 0;
++Seed::batch_counter;
// now for all maximum corners, initialize a new seed
for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
{
// we must check again if the score is above the threshold because the Corners
// vector is initialized with dummy corners
if(it->score > Config::triangMinCornerScore())
{
Feature* new_feature(new Feature(frame.get(), Vector2d(it->x, it->y), it->level));
seeds_.push_back(Seed(new_feature, it->angle, new_keyframe_mean_depth_, new_keyframe_min_depth_));
++new_seeds;
}
}
if(options_.verbose)
SVO_INFO_STREAM("DepthFilter: Initialized "<<new_seeds<<" new seeds");
seeds_updating_halt_ = false;
}
void DepthFilter::removeKeyframe(FramePtr frame)
{
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_);
list<Seed>::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;
//printf("Vogiatzis: removed %zu seeds belonging to old keyframe\n", n_removed);
}
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;
clearFrameQueue();
initializeSeeds(new_keyframe_);
continue;
}
frame = frame_queue_.front();
frame_queue_.pop();
}
updateSeeds(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();
double reproj_thresh = Config::lobaThresh() / frame->cam_->errorMultiplier2();
lock_t lock(seeds_mut_);
list<Seed>::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;
}
SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
// check if point is visible in the current image
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;
}
const Vector2d px(it->ftr->frame->f2c(xyz_f));
if(!it->ftr->frame->cam_->isInFrame(px.cast<int>())) {
++it;
if(options_.verbose)
SVO_INFO_STREAM("not visible");
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, h_inv = -1.0;
if(!matcher::findEpipolarMatchDirect(
*it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max,
options_.epi_search_1d, z, h_inv))
{
if(options_.verbose)
SVO_INFO_STREAM("no match found");
it->b++; // increase outlier probability when no match was found
++it;
++n_failed_matches;
continue;
}
if(options_.use_photometric_disparity_error && h_inv > 0)
{
px_noise = fmax(2.0*options_.sigma_i_sq*h_inv, 1.0);
px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
if(options_.verbose)
SVO_INFO_STREAM("Photometric disparity error "<<px_noise<<" px");
}
if(options_.verbose)
SVO_INFO_STREAM("updated seed");
// 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 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)
{
if(it->ftr->point == NULL)
{
Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
seed_converged_cb_(*it, xyz_world);
if(options_.verbose)
SVO_INFO_STREAM("seed converged");
}
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<Seed>& seeds)
{
lock_t lock(seeds_mut_);
for(std::list<Seed>::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(isnanf(norm_scale))
return;
boost::math::normal_distribution<float> 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
svo/src/feature_alignment.cpp
+601
View File
@@ -0,0 +1,601 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifdef __SSE2__
#include <emmintrin.h>
#endif
#ifdef __ARM_NEON__
#include <arm_neon.h>
#endif
#include <svo/feature_alignment.h>
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<patch_size; ++y)
{
uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
for(int x=0; x<patch_size; ++x, ++it, ++it_dv)
{
Vector2f J;
J[0] = 0.5*(dir[0]*(it[1] - it[-1]) + dir[1]*(it[ref_step] - it[-ref_step]));
J[1] = 1;
*it_dv = J[0];
H += J*J.transpose();
}
}
h_inv = 1.0/H(0,0)*patch_size*patch_size;
Matrix2f Hinv = H.inverse();
float mean_diff = 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];
float chi2 = 0;
Vector2f update; update.setZero();
for(int iter = 0; iter<n_iter; ++iter)
{
int u_r = floor(u);
int v_r = floor(v);
if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= 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<patch_size; ++y)
{
uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
for(int x=0; x<patch_size; ++x, ++it, ++it_ref, ++it_ref_dv)
{
float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
float res = search_pixel - *it_ref + mean_diff;
Jres[0] -= res*(*it_ref_dv);
Jres[1] -= res;
new_chi2 += res*res;
}
}
if(iter > 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<patch_size_; ++y)
{
uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)
{
Vector3f J;
J[0] = 0.5 * (it[1] - it[-1]);
J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
J[2] = 1;
*it_dx = J[0];
*it_dy = J[1];
H += J*J.transpose();
}
}
Matrix3f Hinv = H.inverse();
float mean_diff = 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];
float chi2 = 0;
Vector3f update; update.setZero();
for(int iter = 0; iter<n_iter; ++iter)
{
int u_r = floor(u);
int v_r = floor(v);
if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= 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<patch_size_; ++y)
{
uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
{
float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
float res = search_pixel - *it_ref + mean_diff;
Jres[0] -= res*(*it_ref_dx);
Jres[1] -= res*(*it_ref_dy);
Jres[2] -= res;
new_chi2 += res*res;
}
}
if(iter > 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<patch_size; ++y)
{
uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
{
int16_t dx = static_cast<int16_t>(it[1]) - it[-1];
int16_t dy = static_cast<int16_t>(it[ref_step]) - it[-ref_step];
*it_dx = dx;
*it_dy = dy; // we are missing a factor 1/2
A11 += static_cast<float>(dx*dx); // we are missing a factor 1/4
A12 += static_cast<float>(dx*dy);
A22 += static_cast<float>(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<n_iter; ++iter)
{
int u_r = floor(u);
int v_r = floor(v);
if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= 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<int>((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS));
int wTR = static_cast<int>(subpix_x * (1.0f-subpix_y)*(1 << W_BITS));
int wBL = static_cast<int>((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<patch_size; ++y)
{
const uint8_t* it = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
// Iptr is aligned!
//__m128i diff0 = _mm_load_si128((const __m128i*)(ref_patch + y*8));
__m128i diff = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(ref_patch + y*8)), z);
// load the lower 64 bits and unpack [8u 0 8u 0..]
__m128i v00 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it)), z);
__m128i v01 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + 1)), z);
__m128i v10 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step)), z);
__m128i v11 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step + 1)), z);
// interpolate top row and bottom row
// _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers in a with the lower 4 signed or unsigned 16-bit integers in b.
// _mm_madd_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
__m128i t0 = _mm_add_epi32(_mm_madd_epi16(_mm_unpacklo_epi16(v00, v01), qw0),
_mm_madd_epi16(_mm_unpacklo_epi16(v10, v11), qw1));
__m128i t1 = _mm_add_epi32(_mm_madd_epi16(_mm_unpackhi_epi16(v00, v01), qw0),
_mm_madd_epi16(_mm_unpackhi_epi16(v10, v11), qw1));
// _mm_srai_epi32: Shifts the 4 signed 32-bit integers in A right by count bits while shifting in the sign bit.
t0 = _mm_srai_epi32(_mm_add_epi32(t0, qdelta), W_BITS); // adding qdelta is for rounding closest int
t1 = _mm_srai_epi32(_mm_add_epi32(t1, qdelta), W_BITS);
// compute 8xres:
// _mm_packs_epi32: Packs the 8 signed 32-bit integers from a and b into signed 16-bit integers and saturates.
diff = _mm_subs_epi16(_mm_packs_epi32(t0, t1), diff);
// load gradient dX and dY, both are aligned!
v00 = _mm_load_si128((const __m128i*)(ref_patch_dx + y*patch_size)); // [dx1, dx2, dx3, dx4 ...]
v01 = _mm_load_si128((const __m128i*)(ref_patch_dy + y*patch_size)); // [dy1, dy2, dy3, dy4 ...]
// _mm_mulhi_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
v10 = _mm_mullo_epi16(v00, diff); // Packs the lower 16 bits of the 8 signed 32-bit results. [15:0]
v11 = _mm_mulhi_epi16(v00, diff); // Packs the upper 16 bits of the 8 signed 32-bit results. [31:16]
// _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers with the lower 4 signed or unsigned 16-bit integers in b.
v00 = _mm_unpacklo_epi16(v10, v11);
v10 = _mm_unpackhi_epi16(v10, v11);
// convert to float and add to dx
qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v00));
qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v10));
// same with dY
v10 = _mm_mullo_epi16(v01, diff);
v11 = _mm_mulhi_epi16(v01, diff);
v00 = _mm_unpacklo_epi16(v10, v11);
v10 = _mm_unpackhi_epi16(v10, v11);
qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v00));
qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v10));
}
float __attribute__((__aligned__(16))) buf[4];
_mm_store_ps(buf, qb0);
b1 += buf[0]+buf[1]+buf[2]+buf[3];
_mm_store_ps(buf, qb1);
b2 += buf[0]+buf[1]+buf[2]+buf[3];
#endif
// compute -A^-1*b
update_u = ((A12*b2 - A22*b1) * Dinv)*2; // * 2 to compensate because above, we did not compute the derivative correctly
update_v = ((A12*b1 - A11*b2) * Dinv)*2;
u += update_u;
v += update_v;
#if SUBPIX_VERBOSE
cout << "Iter " << iter << ":"
<< "\t u=" << u << ", v=" << v
<< "\t update = " << update_u << ", " << update_v
<< "\t new chi2 = " << new_chi2 << endl;
#endif
if(update_u*update_u+update_v*update_v < min_update_squared)
{
#if SUBPIX_VERBOSE
cout << "converged." << endl;
#endif
converged=true;
break;
}
chi2 = new_chi2;
}
cur_px_estimate << u, v;
return converged;
}
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)
{
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;
Matrix3f H; H.setZero();
for(int y=0; y<patch_size; ++y)
{
uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
{
*it_dx = static_cast<int16_t>(it[1] - it[-1]);
*it_dy = static_cast<int16_t>(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<n_iter; ++iter)
{
int u_r = floor(u);
int v_r = floor(v);
if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= 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<uint16_t>((1.0f-subpix_x)*(1.0f-subpix_y)*(1<<SHIFT_BITS));
const uint16_t wTR = static_cast<uint16_t>(subpix_x*(1.0f-subpix_y)*(1<<SHIFT_BITS));
const uint16_t wBL = static_cast<uint16_t>((1.0f-subpix_x)*subpix_y*(1<<SHIFT_BITS));
const uint16_t wBR = static_cast<uint16_t>((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<patch_size; ++y)
{
const uint8_t* it = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
// load and convert from uint8 to uint16
uint16x8_t v00 = vmovl_u8( vld1_u8( it ) );
uint16x8_t v01 = vmovl_u8( vld1_u8( it + 1 ) );
uint16x8_t v10 = vmovl_u8( vld1_u8( it + cur_step ) );
uint16x8_t v11 = vmovl_u8( vld1_u8( it + cur_step + 1 ) );
// vector multiply by scalar
v00 = vmulq_n_u16( v00, wTL );
v01 = vmulq_n_u16( v01, wTR );
v10 = vmulq_n_u16( v10, wBL );
v11 = vmulq_n_u16( v11, wBR );
// add all results together
v00 = vaddq_u16( v00, vaddq_u16( v01, vaddq_u16( v10, v11 ) ) );
// descale: shift right by constant
v00 = vshrq_n_u16(v00, SHIFT_BITS);
// compute difference between reference and interpolated patch,
// use reinterpet-cast to make signed [-255,255]
int16x8_t res = vsubq_s16(vreinterpretq_s16_u16(v00), vreinterpretq_s16_u16(vmovl_u8(vld1_u8( ref_patch + y*8 ))));
// correct res with mean difference
res = vaddq_s16(res, vmean_diff);
// compute sum of the residual
vres_sum = vaddq_s16(vres_sum, res);
// Vector multiply accumulate long: vmla -> 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
svo/src/feature_detection.cpp
+77
View File
@@ -0,0 +1,77 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <svo/feature_detection.h>
#include <svo/feature.h>
#include <fast/fast.h>
#include <vikit/vision.h>
namespace svo {
namespace feature_detection {
void FastDetector::detect(
const ImgPyr& img_pyr,
const Features& existing_fts,
const int cell_size,
const int n_levels,
const double detection_threshold,
Corners* corners) const
{
// first, find which parts of the image already have a matched feature
const int grid_n_cols = ceil(static_cast<double>(img_pyr[0].cols)/cell_size);
const int grid_n_rows = ceil(static_cast<double>(img_pyr[0].rows)/cell_size);
vector<bool> occupancy(grid_n_cols*grid_n_rows, false);
for(Features::const_iterator it=existing_fts.begin(); it!=existing_fts.end(); ++it)
{
if((*it)->point == false)
continue;
const int k = static_cast<int>((*it)->px[1]/cell_size)*grid_n_cols + static_cast<int>((*it)->px[0]/cell_size);
occupancy.at(k) = true;
}
// now, detect features and perform nonmax suppression
corners->resize(grid_n_cols*grid_n_rows, Corner(0,0,detection_threshold,0,0.0f));
for(int L=0; L<n_levels; ++L)
{
const int scale = (1<<L);
vector<fast::fast_xy> 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<int> 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(vector<int>::iterator it=nm_corners.begin(); it!=nm_corners.end(); ++it)
{
fast::fast_xy& xy = fast_corners.at(*it);
const int k = static_cast<int>((xy.y*scale)/cell_size)*grid_n_cols + static_cast<int>((xy.x*scale)/cell_size);
if(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);
}
}
}
} // namespace feature_detection
} // namespace svo
svo/src/frame.cpp
+197
View File
@@ -0,0 +1,197 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include <boost/bind.hpp>
#include <vikit/math_utils.h>
#include <vikit/vision.h>
#include <vikit/performance_monitor.h>
#include <fast/fast.h>
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");
// 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(max(abs(ftr->px[0]-cu), abs(ftr->px[1]-cv))
< max(abs(key_pts_[0]->px[0]-cu), abs(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[0]-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[0]-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[0]-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[0]-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();
}
size_t Frame::nObs() const
{
size_t n=0;
std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){if(i->point != NULL) ++n; });
return n;
}
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<n_levels; ++i)
{
pyr[i] = cv::Mat(pyr[i-1].rows/2, pyr[i-1].cols/2, CV_8U);
vk::halfSample(pyr[i-1], pyr[i]);
}
}
bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
{
vector<double> depth_vec;
depth_vec.reserve(frame.fts_.size());
depth_min = std::numeric_limits<double>::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
svo/src/frame_handler_base.cpp
+200
View File
@@ -0,0 +1,200 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <vikit/abstract_camera.h>
#include <stdlib.h>
#include <Eigen/StdVector>
#include <boost/bind.hpp>
#include <fstream>
#include <svo/frame_handler_base.h>
#include <svo/config.h>
#include <svo/feature.h>
#include <svo/matcher.h>
#include <svo/map.h>
#include <svo/point.h>
#include <svo/reprojection.h>
#include <svo/depth_filter.h>
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_(PAUSED),
set_reset_(false),
set_start_(false),
acc_frame_timings_(10),
acc_num_obs_(10),
num_obs_last_(0),
tracking_quality_(BAD),
depth_filter_(NULL)
{
#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("pose_optimizer");
g_permon->addTimer("point_optimizer");
g_permon->addTimer("local_ba");
g_permon->addTimer("tot_time");
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
// create depth filter and set callback
feature_detection::DetectorPtr feature_detector(new feature_detection::FastDetector());
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();
SVO_INFO_STREAM("NanoSlam initialized");
}
FrameHandlerBase::
~FrameHandlerBase()
{
SVO_INFO_STREAM("SVO destructor invoked.");
delete depth_filter_;
SVO_INFO_STREAM("SVO destructed.");
}
bool FrameHandlerBase::
startFrameProcessingCommon()
{
if(set_start_)
{
resetAll();
stage_ = FIRST_FRAME;
set_start_ = false;
}
if(stage_ == PAUSED)
return false;
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(size_t update_id, UpdateResult dropout, size_t num_observations)
{
SVO_DEBUG_STREAM("Frame: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
SVO_LOG(dropout);
// save processing time to calculate fps
acc_frame_timings_.push_back(timer_.stop());
if(stage_ == DEFAULT_FRAME)
acc_num_obs_.push_back(num_observations);
num_obs_last_ = num_observations;
SVO_STOP_TIMER("tot_time");
#ifdef SVO_TRACE
g_permon->writeToFile();
{
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
size_t n_candidates = map_.point_candidates_.candidates_.size();
SVO_LOG(n_candidates);
}
#endif
// reset if failure or requested
if(dropout == FAILURE || set_reset_)
resetAll();
return 0;
}
void FrameHandlerBase::
resetCommon()
{
map_.reset();
stage_ = PAUSED;
set_reset_ = false;
tracking_quality_ = BAD;
depth_filter_->reset();
num_obs_last_ = 0;
SVO_INFO_STREAM("RESET");
}
void FrameHandlerBase::
setTrackingQuality(const size_t num_observations)
{
tracking_quality_ = GOOD;
if(num_observations < Config::qualityMinFts())
{
ROS_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
tracking_quality_ = BAD;
}
const int feature_drop = static_cast<int>(num_obs_last_) - num_observations;
if(feature_drop > Config::qualityMaxFtsDrop())
{
ROS_WARN_STREAM("Lost "<< feature_drop <<" features!");
tracking_quality_ = BAD;
}
if(num_observations < 20)
{
ROS_ERROR_STREAM("Tracking less than 20 features!");
tracking_quality_ = 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<Point*> 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<Point*>::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it)
{
(*it)->optimize(max_iter);
(*it)->last_structure_optim_ = frame->id_;
}
}
} // namespace svo
svo/src/frame_handler_mono.cpp
+254
View File
@@ -0,0 +1,254 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <svo/config.h>
#include <svo/frame_handler_mono.h>
#include <svo/map.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/pose_optimizer.h>
#include <svo/sparse_img_align.h>
#include <vikit/performance_monitor.h>
#include <svo/depth_filter.h>
#ifdef USE_BUNDLE_ADJUSTMENT
#include <svo/bundle_adjustment.h>
#endif
namespace svo {
FrameHandlerMono::
FrameHandlerMono(vk::AbstractCamera* cam) :
FrameHandlerBase(),
cam_(cam)
{
reprojection_grid_.initialize(cam_);
}
FrameHandlerMono::
~FrameHandlerMono()
{}
void FrameHandlerMono::
addImage(const cv::Mat& img, double timestamp)
{
if(!startFrameProcessingCommon())
return;
// some cleanup from last iteration, can't do before because of visualization
core_kfs_.clear();
overlap_kfs_.clear();
// create new frame
SVO_START_TIMER("pyramid_creation");
new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
SVO_STOP_TIMER("pyramid_creation");
// process frame
UpdateResult res = FAILURE;
if(stage_ == DEFAULT_FRAME)
res = processFrame();
else if(stage_ == SECOND_FRAME)
res = processSecondFrame();
else if(stage_ == FIRST_FRAME)
res = processFirstFrame();
// 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_ = T_f_w_init_;
if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
return NO_KEYFRAME;
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
stage_ = SECOND_FRAME;
SVO_INFO_STREAM("Init: Selected first frame.");
return IS_KEYFRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::
processSecondFrame()
{
initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
if(res == initialization::FAILURE)
return FAILURE;
else if(res == initialization::NO_KEYFRAME)
return NO_KEYFRAME;
// two-frame bundle adjustment
#ifdef USE_BUNDLE_ADJUSTMENT
ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), map_);
#endif
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
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
stage_ = DEFAULT_FRAME;
klt_homography_init_.reset();
SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
return IS_KEYFRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::
processFrame()
{
// Set initial pose TODO use prior
new_frame_->T_f_w_ = last_frame_->T_f_w_;
// sparse image align
SVO_START_TIMER("sparse_img_align");
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::GaussNewton, false, false);
size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);
SVO_STOP_TIMER("sparse_img_align");
SVO_LOG(img_align_n_tracked);
SVO_DEBUG_STREAM("Img Align:\t Tracked = " << img_align_n_tracked);
// map reprojection & feature alignment
SVO_START_TIMER("reproject");
size_t repr_n_new_references, repr_n_mps;
reprojection::reprojectMap(
map_, new_frame_, reprojection_grid_, map_.point_candidates_,
overlap_kfs_, repr_n_new_references, repr_n_mps);
SVO_STOP_TIMER("reproject");
SVO_LOG2(repr_n_mps, repr_n_new_references);
SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references);
if(repr_n_new_references == 0) {
SVO_ERROR_STREAM("Not enough matched features.");
return FAILURE;
}
// pose optimization
SVO_START_TIMER("pose_optimizer");
size_t sfba_n_edges_final;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
SVO_STOP_TIMER("pose_optimizer");
SVO_LOG4(sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
if(sfba_n_edges_final < 10)
return FAILURE;
// structure optimization
SVO_START_TIMER("point_optimizer");
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
SVO_STOP_TIMER("point_optimizer");
// select keyframe
setTrackingQuality(sfba_n_edges_final);
if(tracking_quality_ == INSUFFICIENT)
return FAILURE;
double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
if(!needNewKf(depth_mean) || tracking_quality_ == BAD)
{
depth_filter_->addFrame(new_frame_);
return NO_KEYFRAME;
}
// 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
core_kfs_.insert(new_frame_);
#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, ba_final_error;
ba::localBA(new_frame_.get(), &core_kfs_, &map_,
loba_n_erredges_init, ba_incorrect_edges_2,
ba_init_error, 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 {"<<loba_n_erredges_init<<", "<<loba_n_erredges_fin<<"} \t "
"Error {"<<loba_err_init<<", "<<loba_err_fin<<"}");
}
#endif
// init new depth-filters
depth_filter_->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
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
return IS_KEYFRAME;
}
void FrameHandlerMono::
resetAll()
{
resetCommon();
last_frame_.reset();
new_frame_.reset();
core_kfs_.clear();
overlap_kfs_.clear();
}
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(abs(relpos.x())/scene_depth_mean < Config::kfSelectMinDist() &&
abs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist()*0.8 &&
abs(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<FramePtr, size_t>::second, _1) >
boost::bind(&pair<FramePtr, size_t>::second, _2));
std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair<FramePtr,size_t>& i){ core_kfs_.insert(i.first); });
}
} // namespace svo
svo/src/initialization.cpp
+205
View File
@@ -0,0 +1,205 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <svo/config.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/initialization.h>
#include <svo/feature_detection.h>
#include <vikit/math_utils.h>
#include <vikit/homography.h>
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 "<<disparity<<"px average disparity.");
if(disparity < Config::initMinDisparity())
return NO_KEYFRAME;
computeHomography(
f_ref_, f_cur_,
frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
inliers_, xyz_in_cur_, T_cur_from_ref_);
SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");
if(inliers_.size() < Config::initMinInliers())
{
SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
return FAILURE;
}
// Rescale the map such that the mean scene depth is equal to the specified scale
vector<double> depth_vec;
for(size_t i=0; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
frame_cur->T_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<int>::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<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
{
Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
Point* new_point = new Point(pos, Point::TRIANGULATION);
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<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec)
{
feature_detection::Corners corners;
feature_detection::FastDetector detector;
detector.detect(frame->img_pyr_, frame->fts_, Config::gridSize(),
Config::nPyrLevels(), Config::triangMinCornerScore(),
&corners);
// now for all maximum corners, initialize a new seed
px_vec.clear(); px_vec.reserve(corners.size());
f_vec.clear(); f_vec.reserve(corners.size());
for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
{
// we must check again if the score is above the threshold because the Corners
// vector is initialized with dummy corners
if(it->score > Config::triangMinCornerScore())
{
px_vec.push_back(cv::Point2f(it->x, it->y));
f_vec.push_back(frame->c2f(it->x, it->y));
}
}
}
void trackKlt(
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities)
{
const double klt_win_size = 30.0;
const int klt_max_iter = 30;
const double klt_eps = 0.001;
vector<uchar> status;
vector<float> error;
vector<float> 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);
vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
vector<Vector3d>::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<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref)
{
vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.size());
for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
{
uv_ref[i] = vk::project2d(f_ref[i]);
uv_cur[i] = vk::project2d(f_cur[i]);
}
vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
vector<int> 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
svo/src/map.cpp
+357
View File
@@ -0,0 +1,357 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <svo/map.h>
#include <svo/point.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/depth_filter.h>
#include <boost/bind.hpp>
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::DELETED;
trash_points_.push_back(pt);
}
void Map::addKeyframe(FramePtr new_keyframe)
{
keyframes_.push_back(new_keyframe);
}
void Map::getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const
{
const Vector3d frame_pos(frame->pos());
for(auto it_kf=keyframes_.begin(), ite_kf=keyframes_.end(); it_kf!=ite_kf; ++it_kf)
{
// check first if Point is visible in the Keyframe, use therefore KeyPoints
for(auto it=(*it_kf)->key_pts_.begin(), ite=(*it_kf)->key_pts_.end(); it!=ite; ++it)
{
if(*it == NULL)
continue;
assert((*it)->point != NULL);
if((*it_kf)->isVisible((*it)->point->pos_))
{
close_kfs.push_back(pair<FramePtr,double>(*it_kf,
(frame->T_f_w_.translation()-(*it_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<FramePtr,double> > close_kfs;
getCloseKeyframes(frame, close_kfs);
// Sort KFs with overlap according to their closeness
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::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;
}
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(const Seed& seed, const Vector3d& xyz_world)
{
// initalize a new point
Point* point = new Point(xyz_world, Point::VOGIATZIS);
point->addFrameRef(seed.ftr);
point->type_ = Point::CANDIDATE;
seed.ftr->point = point;
// add candidate to list
boost::unique_lock<boost::mutex> lock(mut_);
candidates_.push_back(PointCandidate(point, seed.ftr));
}
void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
{
boost::unique_lock<boost::mutex> 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::UNKNOWN_QUALITY;
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<boost::mutex> 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<boost::mutex> 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<boost::mutex> 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::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::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);
set<Point*> 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
svo/src/matcher.cpp
+343
View File
@@ -0,0 +1,343 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <cstdlib>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <vikit/math_utils.h>
#include <vikit/aligned_mem.h>
#include <vikit/patch_score.h>
#include <svo/matcher.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include <svo/feature_alignment.h>
namespace svo {
namespace matcher {
PatchData::PatchData(int patch_size) :
patch_size(patch_size)
{
patch = vk::aligned_mem::aligned_alloc<uint8_t>(patch_size*patch_size, 16);
patch_with_border = vk::aligned_mem::aligned_alloc<uint8_t>((patch_size+2)*(patch_size+2), 16);
}
PatchData::~PatchData()
{
free(patch);
free(patch_with_border);
}
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<<level_ref)));
Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];
xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];
const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));
A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;
}
int getBestSearchLevel(const Matrix2d& A_cur_ref, const int max_level)
{
// Compute patch level in other image
int level_cur = 0;
double D = A_cur_ref.determinant();
while(D > 3.0 && level_cur < max_level)
{
level_cur += 1;
D *= 0.25;
}
return level_cur;
}
void warpAffine(
const Matrix2d& A_cur_ref,
const cv::Mat& img_ref,
const Vector2d& px_ref,
const int level_ref,
const int level_cur,
PatchData& ref_patch)
{
const int halfpatch_size = ref_patch.patch_size/2+1;
const int patch_size = halfpatch_size*2 ;
Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
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* ref_patch_ptr = ref_patch.patch_with_border;
const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
for (int y=0; y<patch_size; ++y)
{
for (int x=0; x<patch_size; ++x, ++ref_patch_ptr)
{
Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
px_patch *= (1<<level_cur);
const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
*ref_patch_ptr = 0;
else
*ref_patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
}
}
ref_patch_ptr = ref_patch.patch;
for(int y=1; y<g_patch_size+1; ++y, ref_patch_ptr += g_patch_size)
{
uint8_t* ref_patch_border_ptr = ref_patch.patch_with_border + y*(g_patch_size+2) + 1;
for(int x=0; x<g_patch_size; ++x)
ref_patch_ptr[x] = ref_patch_border_ptr[x];
}
}
} // namespace warp
bool
findMatchDirect(
const Point& pt,
const Frame& cur_frame,
Vector2d& px_cur,
int& level_cur)
{
Feature* ref_ftr;
if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr))
return false;
if(!ref_ftr->frame->cam_->isInFrame(
ref_ftr->px.cast<int>()/(1<<ref_ftr->level), g_halfpatch_size+2, ref_ftr->level))
return false;
// warp affine
PatchData ref_patch(g_patch_size);
Matrix2d A_cur_ref;
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);
level_cur = 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, level_cur, ref_patch);
// px_cur should be set
Vector2d px_scaled(px_cur/(1<<level_cur));
bool success = feature_alignment::align2D(
cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled);
px_cur = px_scaled * (1<<level_cur);
return success;
}
bool
depthFromTriangulation(
const SE3& T_search_ref,
const Vector3d& f_ref,
const Vector3d& f_cur,
double& depth)
{
Matrix<double,3,2> 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;
}
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,
const bool align_1d,
double& depth,
double& h_inv)
{
typedef vk::patch_score::ZMSSD<g_halfpatch_size> PatchScore;
PatchData ref_patch(g_patch_size);
SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
int zmssd_best = PatchScore::threshold();
Vector2d uv_best;
// Compute start and end of epipolar line in old_kf for match search, on unit plane!
Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));
Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
Vector2d epi_dir = A - B;
// Compute affine warp matrix
Matrix2d A_cur_ref;
warp::getWarpMatrixAffine(
*ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref);
int level_cur = warp::getBestSearchLevel(A_cur_ref, Config::nPyrLevels()-1);
// Find length of search range on epipolar line
Vector2d px_A(cur_frame.cam_->world2cam(A));
Vector2d px_B(cur_frame.cam_->world2cam(B));
double px_length = (px_A-px_B).norm() / (1<<level_cur);
#ifdef DEBUG_MATCHER_EPISEARCH
printf("px_length = %f\n", px_length);
cv::Mat img_rgb = cv::Mat(cur_frame.img_pyr_.at(level_cur).size(), CV_8UC3);
cv::cvtColor(cur_frame.img_pyr_.at(level_cur), img_rgb, CV_GRAY2RGB);
#endif
// Warp reference patch at ref_level
warp::warpAffine(
A_cur_ref, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px, ref_ftr.level, level_cur, ref_patch);
//printf("px_length = %f\n", px_length);
if(px_length < 2.0)
{
Vector2d px = (px_A+px_B)/2.0;
Vector2d px_scaled(px/(1<<level_cur));
bool res;
if(align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[level_cur], (px_A-px_B).cast<float>().normalized(),
ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled, h_inv);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border,
ref_patch.patch, 10, px_scaled);
if(res)
{
px = px_scaled*(1<<level_cur);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
return true;
}
return false;
}
size_t n_steps = px_length/0.7; // one step per pixel
Vector2d step = epi_dir/n_steps;
if(n_steps > 1000)
{
printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
n_steps, px_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(ref_patch.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; i<n_steps; ++i, uv+=step)
{
Vector2d px(cur_frame.cam_->world2cam(uv));
Vector2i pxi(px[0]/(1<<level_cur)+0.5,
px[1]/(1<<level_cur)+0.5); // +0.5 to round to closest int
#ifdef DEBUG_MATCHER_EPISEARCH
cv::circle(img_rgb, cv::Point2f(pxi[0], pxi[1]), 1, cv::Scalar(0,0,255), 1);
#endif
if(pxi == last_checked_pxi)
continue;
last_checked_pxi = pxi;
// check if the patch is full within the new frame
if(!cur_frame.cam_->isInFrame(pxi, g_patch_size, level_cur))
continue;
// TODO interpolation would probably be a good idea
uint8_t* cur_patch_ptr = cur_frame.img_pyr_[level_cur].data + (pxi[1]-g_halfpatch_size)*cur_frame.img_pyr_[level_cur].cols + (pxi[0]-g_halfpatch_size);
int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[level_cur].cols);
if(zmssd < zmssd_best) {
zmssd_best = zmssd;
uv_best = uv;
}
}
#ifdef DEBUG_MATCHER_EPISEARCH
if(zmssd_best < g_zmssd_thresh) {
Vector2d px(cur_frame.cam_->world2cam(uv_best));
Vector2i pxi(px[0]/(1<<level_cur)+0.5, px[1]/(1<<level_cur)+0.5);
cv::circle(img_rgb, cv::Point2f(px[0]/(1<<level_cur), px[1]/(1<<level_cur)), 8, cv::Scalar(255,0,0), 2);
}
cv::imshow("cur_img", img_rgb);
cv::Mat ref_img_rgb = cv::Mat(ref_frame.img_pyr_.at(ref_ftr.level).size(), CV_8UC3);
cv::cvtColor(ref_frame.img_pyr_.at(ref_ftr.level), ref_img_rgb, CV_GRAY2RGB);
cv::circle(ref_img_rgb, cv::Point2f(ref_ftr.px[0]/(1<<ref_ftr.level), ref_ftr.px[1]/(1<<ref_ftr.level)), 8, cv::Scalar(255,0,0), 2);
cv::imshow("ref_img", ref_img_rgb);
uint8_t* cur_patch_ptr = cur_frame.img_pyr_[level_cur].data + (pxi[1]-g_halfpatch_size)*cur_frame.img_pyr_[level_cur].cols + (pxi[0]-g_halfpatch_size);
cv::Mat cur_patch(g_patch_size,g_patch_size,CV_8U,cur_patch_ptr);
cv::imshow("cur_patch", cur_patch);
cv::waitKey(0);
#endif
if(zmssd_best < PatchScore::threshold())
{
// if(px_length < 4.0)
{
Vector2d px(cur_frame.cam_->world2cam(uv_best));
Vector2d px_scaled(px/(1<<level_cur));
bool res;
if(align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[level_cur], (px_A-px_B).cast<float>().normalized(),
ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled, h_inv);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border,
ref_patch.patch, 10, px_scaled);
if(res)
{
px = px_scaled*(1<<level_cur);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
return true;
}
return false;
}
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
return true;
}
return false;
}
} // namespace matcher
} // namespace svo
svo/src/point.cpp
+170
View File
@@ -0,0 +1,170 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <stdexcept>
#include <vikit/math_utils.h>
#include <svo/point.h>
#include <svo/frame.h>
#include <svo/feature.h>
namespace svo {
int Point::point_counter_ = 0;
Point::Point(Vector3d pos, PointOrigin origin) :
id_(point_counter_++),
pos_(pos),
n_obs_(0),
v_pt_(NULL),
last_published_ts_(0),
last_projected_kf_id_(-1),
type_(UNKNOWN_QUALITY),
origin_(origin),
n_failed_reproj_(0),
n_succeeded_reproj_(0),
last_structure_optim_(0)
{}
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;
}
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::pointJacobian(
const Vector3d& p_in_f,
const Matrix3d& R_f_w,
Matrix23d& point_jac) const
{
// we work on the unit plane, therefore focal lenght = 1
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;
}
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; i<n_iter; i++)
{
A.setZero();
b.setZero();
double new_chi2 = 0.0;
// compute residuals
for(auto it=obs_.begin(); it!=obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
pointJacobian(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
svo/src/pose_optimizer.cpp
+164
View File
@@ -0,0 +1,164 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <stdexcept>
#include <svo/pose_optimizer.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
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);
Vector2d focal_length(1,1);
vector<double> 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<float> errors; errors.reserve(frame->fts_.size());
for(auto 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_);
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; iter<n_iter; iter++)
{
// overwrite scale
if(iter == 5)
scale = 0.85/frame->cam_->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 = frameJac(frame->T_f_w_, (*it)->point->pos_, focal_length);
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;
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)
{
// (*it)->point->deleteFrameRef(frame);
(*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
svo/src/reprojection.cpp
+214
View File
@@ -0,0 +1,214 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <stdexcept>
#include <svo/reprojection.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/map.h>
#include <svo/config.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <vikit/abstract_camera.h>
#include <vikit/math_utils.h>
#include <vikit/timer.h>
namespace svo {
namespace reprojection {
Grid::Grid()
{}
void Grid::initialize(vk::AbstractCamera* cam)
{
cell_size = Config::gridSize();
grid_n_cols = ceil(static_cast<double>(cam->width())/cell_size);
grid_n_rows = ceil(static_cast<double>(cam->height())/cell_size);
cells.resize(grid_n_cols*grid_n_rows);
std::for_each(cells.begin(), cells.end(), [&](Cell*& c){ c = new Cell; });
cell_order.resize(cells.size());
for(size_t i=0; i<cells.size(); ++i)
cell_order[i] = i;
random_shuffle(cell_order.begin(), cell_order.end()); // maybe we should do it at every iteration!
}
Grid::~Grid()
{
std::for_each(cells.begin(), cells.end(), [&](Cell* c){ delete c; });
cells.clear();
}
void resetGrid(Grid& grid)
{
std::for_each(grid.cells.begin(), grid.cells.end(), [&](Cell* c){ c->clear(); });
}
void reprojectMap(
Map& map,
FramePtr frame,
Grid& grid,
MapPointCandidates& candidate_points,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
size_t& n_matches,
size_t& n_trials)
{
resetGrid(grid);
// Identify those Keyframes which share a common field of view.
list< pair<FramePtr,double> > close_kfs;
map.getCloseKeyframes(frame, close_kfs);
// Sort KFs with overlap according to their closeness // TODO is this still necessary?
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::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, max_n = 10;
overlap_kfs.reserve(max_n);
for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
it_frame!=ite_frame && n<max_n; ++it_frame, ++n)
{
FramePtr ref_frame = it_frame->first;
overlap_kfs.push_back(pair<FramePtr,size_t>(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(grid, frame, (*it_ftr)->point))
overlap_kfs.back().second++;
}
}
// Now project all point candidates
{
boost::unique_lock<boost::mutex> lock(candidate_points.mut_);
auto it=candidate_points.candidates_.begin();
while(it!=candidate_points.candidates_.end())
{
if(!reprojectPoint(grid, frame, it->first))
{
it->first->n_failed_reproj_ += 3;
if(it->first->n_failed_reproj_ > 30)
{
candidate_points.deleteCandidate(*it);
it = candidate_points.candidates_.erase(it);
continue;
}
}
++it;
}
}
// 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.
n_matches=0; n_trials=0;
for(size_t i=0; i<grid.cells.size(); ++i)
{
// we prefer good quality points over unkown quality (more likely to match)
// and unknown quality over candidates (position not optimized)
if(reprojectCell(*grid.cells.at(grid.cell_order[i]), frame, map, candidate_points, n_trials))
++n_matches;
if(n_matches > (size_t) Config::maxFts())
break;
}
}
bool pointQualityComparator(Candidate& lhs, Candidate& rhs)
{
if(lhs.pt->type_ > rhs.pt->type_)
return true;
return false;
}
bool reprojectCell(
Cell& cell,
FramePtr frame,
Map& map,
MapPointCandidates& candidate_points,
size_t& n_trials)
{
cell.sort(boost::bind(&pointQualityComparator, _1, _2));
Cell::iterator it=cell.begin();
while(it!=cell.end())
{
++n_trials;
if(it->pt->type_ == Point::DELETED)
{
it = cell.erase(it);
continue;
}
int feature_level;
bool found_match = matcher::findMatchDirect(*it->pt, *frame, it->px, feature_level);
if(!found_match)
{
it->pt->n_failed_reproj_++;
if(it->pt->type_ == Point::UNKNOWN_QUALITY && it->pt->n_failed_reproj_ > 15)
map.safeDeletePoint(it->pt);
if(it->pt->type_ == Point::CANDIDATE && it->pt->n_failed_reproj_ > 30)
candidate_points.deleteCandidatePoint(it->pt);
it = cell.erase(it);
continue;
}
it->pt->n_succeeded_reproj_++;
if(it->pt->type_ == Point::UNKNOWN_QUALITY && it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::GOOD_QUALITY;
Feature* new_feature = new Feature(frame.get(), it->px, feature_level);
frame->addFeature(new_feature);
new_feature->point = it->pt;
// note, we add a reference to the point only if the frame is selected as a keyframe
// 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 reprojectPoint(
Grid& grid,
FramePtr frame,
Point* point)
{
Vector2d px(frame->w2c(point->pos_));
if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{
const int k = static_cast<int>(px[1]/grid.cell_size)*grid.grid_n_cols + static_cast<int>(px[0]/grid.cell_size);
grid.cells.at(k)->push_back(Candidate(point, px));
return true;
}
return false;
}
} // namespace reprojection
} // namespace svo
svo/src/sparse_img_align.cpp
+296
View File
@@ -0,0 +1,296 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <svo/sparse_img_align.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/config.h>
#include <svo/point.h>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <vikit/math_utils.h>
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);
SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());
for(level_=max_level_; level_>=min_level_; --level_)
{
setInitialChi2(1.0);
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_;
}
// TODO take jacobian from Frame Class!
void
frameJac(const Vector3d & xyz,
Matrix<double,2,6> & J)
{
const double x = xyz[0];
const double y = xyz[1];
const double z_inv = 1./xyz[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) = J(0,2)*y; // -x*y/z^2
J(0,4) = 1.0 - J(0,2)*x; // 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 + J(1,2)*y; // -1.0 - y^2/z^2
J(1,4) = -J(0,3); // x*y/^2
J(1,5) = x*z_inv; // x/z
}
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<<level_);
const Vector3d ref_pos = ref_frame_->pos();
const double focal_length = ref_frame_->cam_->errorMultiplier2();
size_t feature_counter = 0;
std::vector<bool>::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);
// 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<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
for(int y=0; y<patch_size_; ++y)
{
uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i+y-patch_halfsize_)*stride + (u_ref_i-patch_halfsize_);
for(int x=0; x<patch_size_; ++x, ++ref_img_ptr, ++cache_ptr, ++pixel_counter)
{
// precompute interpolated reference patch color
*cache_ptr = w_ref_tl*ref_img_ptr[0] + w_ref_tr*ref_img_ptr[1] + w_ref_bl*ref_img_ptr[stride] + w_ref_br*ref_img_ptr[stride+1];
// we use the inverse compositional: thereby we can take the gradient always at the same position
// get gradient of warped image (~gradient at warped position)
float dx = 0.5f * ((w_ref_tl*ref_img_ptr[1] + w_ref_tr*ref_img_ptr[2] + w_ref_bl*ref_img_ptr[stride+1] + w_ref_br*ref_img_ptr[stride+2])
-(w_ref_tl*ref_img_ptr[-1] + w_ref_tr*ref_img_ptr[0] + w_ref_bl*ref_img_ptr[stride-1] + w_ref_br*ref_img_ptr[stride]));
float dy = 0.5f * ((w_ref_tl*ref_img_ptr[stride] + w_ref_tr*ref_img_ptr[1+stride] + w_ref_bl*ref_img_ptr[stride*2] + w_ref_br*ref_img_ptr[stride*2+1])
-(w_ref_tl*ref_img_ptr[-stride] + w_ref_tr*ref_img_ptr[1-stride] + w_ref_bl*ref_img_ptr[0] + w_ref_br*ref_img_ptr[1]));
// evaluate jacobian and cache
Matrix<double,2,6> frame_jac;
frameJac(xyz_ref, frame_jac);
// cache the jacobian
jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
(dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
}
}
}
have_ref_patch_cache_ = true;
}
double SparseImgAlign::
computeResiduals(const SE3& T_cur_from_ref, bool linearize_system, bool compute_weight_scale)
{
// Warp the (cur)rent image such that it aligns with the (ref)erence image
const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_);
const cv::Mat& cur_img = cur_frame_->img_pyr_.at(level_);
if(linearize_system && display_)
resimg_ = cv::Mat(ref_img.size(), CV_32F, cv::Scalar(0));
if(have_ref_patch_cache_ == false)
precomputeReferencePatches();
// compute the weights on the first iteration
std::vector<float> errors;
if(compute_weight_scale)
errors.reserve(visible_fts_.size());
const int stride = ref_img.cols;
const int border = patch_halfsize_+1;
const float scale = 1.0f/(1<<level_);
const Vector3d ref_pos(ref_frame_->pos());
float chi2 = 0.0;
size_t feature_counter = 0; // is used to compute the index of the cached jacobian
std::vector<bool>::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<float>() * 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<float*>(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; y<patch_size_; ++y)
{
uint8_t* cur_img_ptr = (uint8_t*) cur_img.data + (v_cur_i+y-patch_halfsize_)*stride + (u_cur_i-patch_halfsize_);
for(int x=0; x<patch_size_; ++x, ++pixel_counter, ++cur_img_ptr, ++ref_patch_cache_ptr)
{
// compute residual
const float intensity_cur = w_cur_tl*cur_img_ptr[0] + w_cur_tr*cur_img_ptr[1] + w_cur_bl*cur_img_ptr[stride] + w_cur_br*cur_img_ptr[stride+1];
const float res = intensity_cur - (*ref_patch_cache_ptr);
// used to compute scale for robust cost
if(compute_weight_scale)
errors.push_back(fabsf(res));
// robustification
float weight = 1.0;
if(use_weights_) {
weight = weight_function_->value(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_ += J*J.transpose()*weight;
Jres_ -= J*res*weight;
if(display_)
resimg_.at<float>((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)
{
// the jacobian is computed in the coordinate frame of the reference image
// T_ref_from_curnew = T_ref_from_curold * SE3::exp(x_).inverse();
// but our state is in the reference frame of the new image, thus using (AB)^-1 = B^-1*A^-1
T_curnew_from_ref = SE3::exp(x_) * T_curold_from_ref;
}
void SparseImgAlign::
startIteration()
{}
void SparseImgAlign::
finishIteration()
{
if(display_)
{
cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
cv::imshow("residuals", resimg_*4);
cv::waitKey(0);
}
}
} // namespace svo
svo/test/README.md
+11
View File
@@ -0,0 +1,11 @@
Download Synthetic Test Data:
-----------------------------
Downloads and extracts the test data in the folder `data`:
cd rpg_slam/svo/test/data
wget http://rpg.ifi.uzh.ch/datasets/flying_room_1_rig_1.tar.gz -O - | tar -xz
If you want to store the data in another location (for instance, if you maintain a special folder with all datasets), then you can move the dataset there and set the environment variable `TEST_DATA_DIR` in your `.bashrc` like this:
export SVO_DATASET_DIR=${HOME}/Datasets
svo/test/benchmark.csv
+307
View File
@@ -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.000203000000000,0.000000000000000,0.000000000000000,0.006850000000000,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.000109000000000,0.000000000000000,0.000000000000000,0.015619000000000,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.000085000000000,0.000000000000000,0.000000000000000,0.015440000000000,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.000096000000000,0.000000000000000,0.000000000000000,0.016054000000000,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.000104000000000,0.000000000000000,0.000000000000000,0.022404000000000,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.000086000000000,0.000000000000000,0.000000000000000,0.027997000000000,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.031598000000000,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.000085000000000,0.000000000000000,0.000000000000000,0.030167000000000,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.000115000000000,0.000000000000000,0.000000000000000,0.029319000000000,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.000085000000000,0.000000000000000,0.000000000000000,0.030061000000000,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.000118000000000,0.000000000000000,0.000000000000000,0.034386000000000,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.000086000000000,0.000000000000000,0.000000000000000,0.089886000000000,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.000031000000000,0.000123000000000,0.000083000000000,0.000787000000000,0.001478000000000,0.002580000000000,0.000000000000000,279.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,148.000000000000000,121.000000000000000,0.170175036288459,0.209622100274688,121.000000000000000,0.310240723182540
0.000000000000000,0.000021000000000,0.000169000000000,0.000094000000000,0.000742000000000,0.000759000000000,0.001820000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,151.000000000000000,121.000000000000000,0.200086249073657,0.226929968841294,119.000000000000000,0.335856340084193
0.000000000000000,0.000025000000000,0.000161000000000,0.000087000000000,0.000786000000000,0.000849000000000,0.001929000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,150.000000000000000,121.000000000000000,0.220021652685224,0.250579787227198,120.000000000000000,0.370858080741496
0.000000000000000,0.000022000000000,0.000154000000000,0.000080000000000,0.000833000000000,0.000799000000000,0.001908000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,153.000000000000000,121.000000000000000,0.241933925519325,0.315090765827434,119.000000000000000,0.466334367626374
0.000000000000000,0.000022000000000,0.000147000000000,0.000078000000000,0.000733000000000,0.000857000000000,0.001878000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,145.000000000000000,121.000000000000000,0.272951477661557,0.278442109473506,118.000000000000000,0.412094340922640
0.000000000000000,0.000028000000000,0.000163000000000,0.000105000000000,0.000998000000000,0.000769000000000,0.002090000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,176.000000000000000,121.000000000000000,0.295685270342958,0.345204254144622,120.000000000000000,0.510902319377397
0.000000000000000,0.000026000000000,0.000096000000000,0.000108000000000,0.000884000000000,0.000849000000000,0.001987000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,154.000000000000000,121.000000000000000,0.280726298758396,0.351937246615374,119.000000000000000,0.520867118388044
0.000000000000000,0.000027000000000,0.000145000000000,0.000077000000000,0.000795000000000,0.000757000000000,0.001822000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,160.000000000000000,121.000000000000000,0.299712701755874,0.325868377142253,119.000000000000000,0.482285213145440
0.000000000000000,0.000017000000000,0.000088000000000,0.000082000000000,0.000819000000000,0.000723000000000,0.001753000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,157.000000000000000,121.000000000000000,0.266414412346027,0.325724036228748,121.000000000000000,0.482071564765419
0.000000000000000,0.000018000000000,0.000117000000000,0.000090000000000,0.000805000000000,0.000748000000000,0.001799000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,53.000000000000000,146.000000000000000,121.000000000000000,0.268513468986649,0.275002624188711,120.000000000000000,0.407003878905728
0.000000000000000,0.000017000000000,0.000084000000000,0.000077000000000,0.000701000000000,0.000777000000000,0.001677000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,76.000000000000000,141.000000000000000,121.000000000000000,0.291779187140528,0.283057832886000,119.000000000000000,0.418925579815900
0.000000000000000,0.000019000000000,0.000215000000000,0.000093000000000,0.000797000000000,0.000821000000000,0.001970000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,96.000000000000000,146.000000000000000,121.000000000000000,0.284752636918227,0.313033086538782,118.000000000000000,0.463288955787851
0.000000000000000,0.000018000000000,0.000115000000000,0.000087000000000,0.000928000000000,0.000752000000000,0.001937000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,108.000000000000000,164.000000000000000,121.000000000000000,0.266984542077384,0.310809089011309,117.000000000000000,0.459997446489587
0.000000000000000,0.000018000000000,0.000145000000000,0.000076000000000,0.000879000000000,0.000667000000000,0.001797000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,172.000000000000000,121.000000000000000,0.281472783188953,0.318106490866299,118.000000000000000,0.470797632399925
0.000000000000000,0.000025000000000,0.000097000000000,0.000092000000000,0.001059000000000,0.001001000000000,0.002299000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,169.000000000000000,121.000000000000000,0.259578545621370,0.296051217193402,118.000000000000000,0.438155829408007
0.000000000000000,0.000023000000000,0.000152000000000,0.000090000000000,0.000987000000000,0.000683000000000,0.001956000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,88.000000000000000,169.000000000000000,121.000000000000000,0.260854021729684,0.463271425215932,119.000000000000000,0.685641725628487
0.000000000000000,0.000012000000000,0.000147000000000,0.000078000000000,0.000828000000000,0.000628000000000,0.001713000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,100.000000000000000,162.000000000000000,121.000000000000000,0.224857463762793,0.241819177256775,120.000000000000000,0.357892398925400
0.000000000000000,0.000015000000000,0.000176000000000,0.000082000000000,0.000976000000000,0.000761000000000,0.002033000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,175.000000000000000,163.000000000000000,121.000000000000000,0.232802573731276,0.307670936583970,121.000000000000000,0.455352982415734
0.000000000000000,0.000015000000000,0.000150000000000,0.000089000000000,0.000914000000000,0.000700000000000,0.001890000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,215.000000000000000,163.000000000000000,121.000000000000000,0.223503209746196,0.245376568533207,121.000000000000000,0.363157337925476
0.000000000000000,0.000014000000000,0.000085000000000,0.000085000000000,0.000830000000000,0.000688000000000,0.001724000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,236.000000000000000,160.000000000000000,121.000000000000000,0.238679764456232,0.260245227551971,121.000000000000000,0.385162944164610
0.000000000000000,0.000017000000000,0.000094000000000,0.000082000000000,0.001027000000000,0.000661000000000,0.001919000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,253.000000000000000,156.000000000000000,121.000000000000000,0.240028080500584,0.347317487309391,119.000000000000000,0.514029864789977
0.000000000000000,0.000017000000000,0.000107000000000,0.000086000000000,0.001095000000000,0.000731000000000,0.002049000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,225.000000000000000,203.000000000000000,121.000000000000000,0.237050681546672,0.342187692298344,118.000000000000000,0.506437791010399
0.000000000000000,0.000027000000000,0.000087000000000,0.000082000000000,0.001031000000000,0.000707000000000,0.001956000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,224.000000000000000,192.000000000000000,121.000000000000000,0.228256268353354,0.255917210772511,118.000000000000000,0.378757460447388
0.000000000000000,0.000028000000000,0.000163000000000,0.000113000000000,0.001066000000000,0.000688000000000,0.002083000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,237.000000000000000,174.000000000000000,121.000000000000000,0.253024524781435,0.270830951731115,117.000000000000000,0.400829834964262
0.000000000000000,0.000024000000000,0.000148000000000,0.000082000000000,0.000924000000000,0.000665000000000,0.001864000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,243.000000000000000,168.000000000000000,121.000000000000000,0.255837794475283,0.309767339197390,117.000000000000000,0.458455660309276
0.000000000000000,0.000024000000000,0.000086000000000,0.000079000000000,0.000949000000000,0.000709000000000,0.001867000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,247.000000000000000,176.000000000000000,121.000000000000000,0.242039138156127,0.295753126320285,120.000000000000000,0.437714633120047
0.000000000000000,0.000017000000000,0.000162000000000,0.000105000000000,0.001033000000000,0.000869000000000,0.002208000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,284.000000000000000,173.000000000000000,121.000000000000000,0.234035640036960,0.262962619721412,120.000000000000000,0.389184684115904
0.000000000000000,0.000017000000000,0.000148000000000,0.000084000000000,0.000934000000000,0.000632000000000,0.001837000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,323.000000000000000,168.000000000000000,121.000000000000000,0.243963350316049,0.254676670136256,118.000000000000000,0.376921484440742
0.000000000000000,0.000020000000000,0.000115000000000,0.000080000000000,0.000954000000000,0.000883000000000,0.002074000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,346.000000000000000,164.000000000000000,121.000000000000000,0.237498578056768,0.242318556788754,120.000000000000000,0.358631474732560
0.000000000000000,0.000026000000000,0.000160000000000,0.000084000000000,0.001068000000000,0.000705000000000,0.002067000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,174.000000000000000,121.000000000000000,0.231204379503806,0.310626553544348,119.000000000000000,0.459727290210615
0.000000000000000,0.000025000000000,0.000107000000000,0.000082000000000,0.000854000000000,0.000734000000000,0.001826000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,371.000000000000000,154.000000000000000,121.000000000000000,0.226372225769396,0.240389889691023,118.000000000000000,0.355777042407619
0.000000000000000,0.000018000000000,0.000109000000000,0.000077000000000,0.000903000000000,0.000645000000000,0.001789000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,383.000000000000000,163.000000000000000,121.000000000000000,0.243430388985134,0.289702067134667,116.000000000000000,0.428759091467402
0.000000000000000,0.000023000000000,0.000162000000000,0.000088000000000,0.001172000000000,0.000728000000000,0.002190000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,189.000000000000000,121.000000000000000,0.238491169554930,0.289513420723387,119.000000000000000,0.428479887859349
0.000000000000000,0.000021000000000,0.000095000000000,0.000089000000000,0.001107000000000,0.000638000000000,0.001974000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,187.000000000000000,121.000000000000000,0.253535525839367,0.299150984189304,119.000000000000000,0.442743431776745
0.000000000000000,0.000012000000000,0.000107000000000,0.000073000000000,0.001079000000000,0.000677000000000,0.001968000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,182.000000000000000,121.000000000000000,0.242747319355396,0.331002407056278,119.000000000000000,0.489883554348514
0.000000000000000,0.000022000000000,0.000152000000000,0.000092000000000,0.001093000000000,0.000806000000000,0.002196000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,420.000000000000000,166.000000000000000,121.000000000000000,0.214060131146887,0.291831223233246,120.000000000000000,0.431910240805571
0.000000000000000,0.000013000000000,0.000086000000000,0.000086000000000,0.001065000000000,0.000781000000000,0.002051000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,467.000000000000000,189.000000000000000,121.000000000000000,0.191542689678233,0.219467905989458,120.000000000000000,0.324812508085401
0.000000000000000,0.000017000000000,0.000087000000000,0.000076000000000,0.001000000000000,0.000696000000000,0.001897000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,491.000000000000000,176.000000000000000,121.000000000000000,0.237287862315746,0.290385534097234,119.000000000000000,0.429770572749869
0.000000000000000,0.000015000000000,0.000098000000000,0.000115000000000,0.001209000000000,0.000852000000000,0.002312000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,503.000000000000000,195.000000000000000,121.000000000000000,0.255419259577233,0.271365864372703,119.000000000000000,0.401621476258818
0.000000000000000,0.000014000000000,0.000106000000000,0.000102000000000,0.001070000000000,0.000640000000000,0.001952000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,519.000000000000000,192.000000000000000,121.000000000000000,0.243354924512186,0.322133979720464,118.000000000000000,0.476758255408191
0.000000000000000,0.000015000000000,0.000087000000000,0.000074000000000,0.001024000000000,0.000658000000000,0.001878000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,537.000000000000000,182.000000000000000,121.000000000000000,0.256839605640840,0.290001626967172,120.000000000000000,0.429202410592353
0.000000000000000,0.000019000000000,0.000171000000000,0.000101000000000,0.001223000000000,0.000747000000000,0.002282000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,551.000000000000000,186.000000000000000,121.000000000000000,0.234920975451741,0.278307444024956,121.000000000000000,0.411895021692308
0.000000000000000,0.000020000000000,0.000085000000000,0.000077000000000,0.001001000000000,0.000765000000000,0.001993000000000,1.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,566.000000000000000,182.000000000000000,121.000000000000000,0.236213272390075,0.243545153610833,120.000000000000000,0.360446828894150
0.000000000000000,0.000019000000000,0.000102000000000,0.000073000000000,0.001140000000000,0.000640000000000,0.001988000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,194.000000000000000,121.000000000000000,0.256770484355549,0.281629623166612,120.000000000000000,0.416811865969600
0.000000000000000,0.000018000000000,0.000099000000000,0.000139000000000,0.001286000000000,0.000761000000000,0.002328000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,193.000000000000000,121.000000000000000,0.238587016371888,0.245006436065432,120.000000000000000,0.362609544894325
0.000000000000000,0.000020000000000,0.000152000000000,0.000079000000000,0.001167000000000,0.000673000000000,0.002111000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,525.000000000000000,207.000000000000000,121.000000000000000,0.272520106859475,0.292688892408874,120.000000000000000,0.433179596238706
0.000000000000000,0.000022000000000,0.000132000000000,0.000074000000000,0.001159000000000,0.000620000000000,0.002036000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,532.000000000000000,193.000000000000000,121.000000000000000,0.300157301652565,0.352279034698071,121.000000000000000,0.521372960116753
0.000000000000000,0.000025000000000,0.000088000000000,0.000085000000000,0.001215000000000,0.000771000000000,0.002209000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,541.000000000000000,192.000000000000000,121.000000000000000,0.262925275260352,0.296134704229152,121.000000000000000,0.438279358303388
0.000000000000000,0.000018000000000,0.000115000000000,0.000090000000000,0.001041000000000,0.000632000000000,0.001918000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,565.000000000000000,171.000000000000000,121.000000000000000,0.281978346961253,0.308467305071063,121.000000000000000,0.456531611839373
0.000000000000000,0.000024000000000,0.000152000000000,0.000078000000000,0.001256000000000,0.000758000000000,0.002299000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,608.000000000000000,201.000000000000000,121.000000000000000,0.262127606608173,0.344179498178872,120.000000000000000,0.509385653434813
0.000000000000000,0.000015000000000,0.000150000000000,0.000084000000000,0.001197000000000,0.000656000000000,0.002124000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,636.000000000000000,196.000000000000000,121.000000000000000,0.243118892914018,0.307363483400101,120.000000000000000,0.454897987687333
0.000000000000000,0.000021000000000,0.000153000000000,0.000080000000000,0.001146000000000,0.000654000000000,0.002075000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,663.000000000000000,198.000000000000000,121.000000000000000,0.308232628226545,0.324201861364523,120.000000000000000,0.479818779824340
0.000000000000000,0.000014000000000,0.000090000000000,0.000080000000000,0.001210000000000,0.000638000000000,0.002083000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,687.000000000000000,198.000000000000000,121.000000000000000,0.262176220963531,0.309537239684066,120.000000000000000,0.458115096603436
0.000000000000000,0.000016000000000,0.000088000000000,0.000083000000000,0.001372000000000,0.000608000000000,0.002184000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,229.000000000000000,121.000000000000000,0.297772564345863,0.357204547667525,120.000000000000000,0.528662731800066
0.000000000000000,0.000019000000000,0.000117000000000,0.000170000000000,0.001186000000000,0.000723000000000,0.002237000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,195.000000000000000,121.000000000000000,0.273843523747758,0.292259687716359,119.000000000000000,0.432544362541022
0.000000000000000,0.000024000000000,0.000153000000000,0.000084000000000,0.001302000000000,0.000641000000000,0.002227000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,668.000000000000000,208.000000000000000,121.000000000000000,0.290286911670839,0.351299244972216,120.000000000000000,0.519922910820696
0.000000000000000,0.000016000000000,0.000088000000000,0.000087000000000,0.001312000000000,0.000624000000000,0.002148000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,681.000000000000000,211.000000000000000,121.000000000000000,0.258959269785308,0.286064925174815,120.000000000000000,0.423376082295636
0.000000000000000,0.000032000000000,0.000153000000000,0.000075000000000,0.001269000000000,0.000651000000000,0.002202000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,695.000000000000000,219.000000000000000,121.000000000000000,0.299842600943275,0.306289515328744,118.000000000000000,0.453308488218462
0.000000000000000,0.000019000000000,0.000193000000000,0.000085000000000,0.001467000000000,0.000596000000000,0.002394000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,704.000000000000000,201.000000000000000,121.000000000000000,0.274895790792161,0.339772965508454,119.000000000000000,0.502863994936072
0.000000000000000,0.000017000000000,0.000154000000000,0.000080000000000,0.001269000000000,0.000617000000000,0.002161000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,712.000000000000000,205.000000000000000,121.000000000000000,0.276261362669988,0.281418870291626,120.000000000000000,0.416499935291270
0.000000000000000,0.000016000000000,0.000086000000000,0.000075000000000,0.001135000000000,0.000709000000000,0.002044000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,743.000000000000000,194.000000000000000,121.000000000000000,0.308428304394499,0.293336201315787,118.000000000000000,0.434137551702758
0.000000000000000,0.000018000000000,0.000171000000000,0.000082000000000,0.001466000000000,0.000783000000000,0.002546000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,769.000000000000000,216.000000000000000,121.000000000000000,0.273449756649530,0.322902720385252,119.000000000000000,0.477896045491622
0.000000000000000,0.000016000000000,0.000087000000000,0.000083000000000,0.001273000000000,0.000696000000000,0.002177000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,791.000000000000000,204.000000000000000,121.000000000000000,0.288082179726881,0.251959464844591,120.000000000000000,0.372900022480006
0.000000000000000,0.000016000000000,0.000085000000000,0.000074000000000,0.001110000000000,0.000812000000000,0.002149000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,190.000000000000000,121.000000000000000,0.282457893847037,0.276453430621912,117.000000000000000,0.409151077978374
0.000000000000000,0.000013000000000,0.000172000000000,0.000092000000000,0.001460000000000,0.000864000000000,0.002617000000000,0.000000000000000,111.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,208.000000000000000,121.000000000000000,0.294189238611744,0.317837647902468,121.000000000000000,0.470399701551591
0.000000000000000,0.000011000000000,0.000108000000000,0.000167000000000,0.001206000000000,0.000875000000000,0.002390000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,194.000000000000000,121.000000000000000,0.243742619771063,0.304182365871834,119.000000000000000,0.450189939591226
0.000000000000000,0.000012000000000,0.000101000000000,0.000079000000000,0.001300000000000,0.000731000000000,0.002246000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,220.000000000000000,121.000000000000000,0.265019461457802,0.270352874765002,119.000000000000000,0.400122247905802
0.000000000000000,0.000020000000000,0.000165000000000,0.000097000000000,0.001379000000000,0.000946000000000,0.002634000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,811.000000000000000,196.000000000000000,121.000000000000000,0.253755606620435,0.269982081521535,119.000000000000000,0.399573494543632
0.000000000000000,0.000016000000000,0.000164000000000,0.000085000000000,0.001383000000000,0.000747000000000,0.002418000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,217.000000000000000,121.000000000000000,0.242158681319397,0.277544822144716,118.000000000000000,0.410766329481695
0.000000000000000,0.000019000000000,0.000149000000000,0.000078000000000,0.001258000000000,0.000689000000000,0.002215000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,826.000000000000000,208.000000000000000,121.000000000000000,0.247027054438406,0.299934280502362,119.000000000000000,0.443902753492513
0.000000000000000,0.000021000000000,0.000089000000000,0.000096000000000,0.001356000000000,0.000776000000000,0.002363000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,835.000000000000000,206.000000000000000,121.000000000000000,0.251693428883974,0.262689374323986,119.000000000000000,0.388780283669121
0.000000000000000,0.000014000000000,0.000148000000000,0.000100000000000,0.001257000000000,0.000655000000000,0.002196000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,858.000000000000000,208.000000000000000,121.000000000000000,0.204260997947582,0.239470873759942,121.000000000000000,0.354416885149381
0.000000000000000,0.000023000000000,0.000162000000000,0.000076000000000,0.001146000000000,0.000648000000000,0.002087000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,182.000000000000000,121.000000000000000,0.288689598171738,0.321106098890548,121.000000000000000,0.475237040529197
0.000000000000000,0.000024000000000,0.000153000000000,0.000091000000000,0.001220000000000,0.000643000000000,0.002186000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,895.000000000000000,169.000000000000000,121.000000000000000,0.226469268954673,0.283662212895001,120.000000000000000,0.419820102888912
0.000000000000000,0.000024000000000,0.000085000000000,0.000080000000000,0.001237000000000,0.000635000000000,0.002077000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,201.000000000000000,121.000000000000000,0.248977774242743,0.301416583242808,121.000000000000000,0.446096553891485
0.000000000000000,0.000021000000000,0.000171000000000,0.000164000000000,0.001414000000000,0.000769000000000,0.002569000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,881.000000000000000,197.000000000000000,121.000000000000000,0.197091396735570,0.215309872281506,121.000000000000000,0.318658631095349
0.000000000000000,0.000020000000000,0.000151000000000,0.000085000000000,0.001385000000000,0.000841000000000,0.002509000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,888.000000000000000,208.000000000000000,121.000000000000000,0.211961816387840,0.217745091845774,121.000000000000000,0.322262753412405
0.000000000000000,0.000018000000000,0.000150000000000,0.000078000000000,0.001242000000000,0.000841000000000,0.002356000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,913.000000000000000,191.000000000000000,121.000000000000000,0.223779838328298,0.264533102759499,119.000000000000000,0.391509013717953
0.000000000000000,0.000022000000000,0.000097000000000,0.000097000000000,0.001450000000000,0.000723000000000,0.002416000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,960.000000000000000,191.000000000000000,121.000000000000000,0.192480719814296,0.242812842123578,120.000000000000000,0.359363019522775
0.000000000000000,0.000020000000000,0.000090000000000,0.000082000000000,0.001223000000000,0.000684000000000,0.002125000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1010.000000000000000,186.000000000000000,121.000000000000000,0.226246256927728,0.246572316756408,121.000000000000000,0.364927025819891
0.000000000000000,0.000018000000000,0.000107000000000,0.000079000000000,0.001240000000000,0.000867000000000,0.002334000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1043.000000000000000,191.000000000000000,121.000000000000000,0.213017300445347,0.226962316320547,121.000000000000000,0.335904230275835
0.000000000000000,0.000016000000000,0.000187000000000,0.000091000000000,0.001459000000000,0.000724000000000,0.002629000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1066.000000000000000,197.000000000000000,121.000000000000000,0.236488382783212,0.308296711179547,118.000000000000000,0.456279145868659
0.000000000000000,0.000016000000000,0.000147000000000,0.000085000000000,0.001434000000000,0.000889000000000,0.002595000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1045.000000000000000,222.000000000000000,121.000000000000000,0.203395568382791,0.322649411912471,119.000000000000000,0.477521162587761
0.000000000000000,0.000019000000000,0.000093000000000,0.000103000000000,0.001526000000000,0.000764000000000,0.002542000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1041.000000000000000,208.000000000000000,121.000000000000000,0.217461375876614,0.242876047600301,119.000000000000000,0.359456575981592
0.000000000000000,0.000012000000000,0.000089000000000,0.000085000000000,0.001381000000000,0.000699000000000,0.002299000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,202.000000000000000,121.000000000000000,0.237082694302344,0.363703604736602,120.000000000000000,0.538281356749968
0.000000000000000,0.000013000000000,0.000106000000000,0.000083000000000,0.001290000000000,0.000760000000000,0.002283000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1051.000000000000000,204.000000000000000,121.000000000000000,0.204040454954466,0.217778632970223,121.000000000000000,0.322312387363004
0.000000000000000,0.000021000000000,0.000089000000000,0.000095000000000,0.001523000000000,0.000740000000000,0.002518000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1092.000000000000000,216.000000000000000,121.000000000000000,0.189496933340522,0.218462243311750,119.000000000000000,0.323324121364161
0.000000000000000,0.000016000000000,0.000114000000000,0.000088000000000,0.001265000000000,0.000870000000000,0.002384000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1150.000000000000000,195.000000000000000,121.000000000000000,0.234911952172899,0.229829985444704,121.000000000000000,0.340148387945673
0.000000000000000,0.000022000000000,0.000165000000000,0.000090000000000,0.001383000000000,0.000870000000000,0.002564000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1188.000000000000000,188.000000000000000,121.000000000000000,0.238956119978159,0.242241004289434,119.000000000000000,0.358516689903833
0.000000000000000,0.000024000000000,0.000125000000000,0.000088000000000,0.001295000000000,0.000732000000000,0.002498000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,181.000000000000000,121.000000000000000,0.231068618034561,0.351821584870681,121.000000000000000,0.520695977291580
0.000000000000000,0.000009000000000,0.000086000000000,0.000086000000000,0.001377000000000,0.000690000000000,0.002304000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,223.000000000000000,121.000000000000000,0.232053202367997,0.243086811406556,121.000000000000000,0.359768506659923
0.000000000000000,0.000015000000000,0.000156000000000,0.000090000000000,0.001467000000000,0.000758000000000,0.002521000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1126.000000000000000,205.000000000000000,121.000000000000000,0.217720599422460,0.232179181942645,120.000000000000000,0.343625190587007
0.000000000000000,0.000022000000000,0.000088000000000,0.000083000000000,0.001420000000000,0.000856000000000,0.002503000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1122.000000000000000,225.000000000000000,121.000000000000000,0.210873068435952,0.231191074937355,120.000000000000000,0.342162783347037
0.000000000000000,0.000010000000000,0.000096000000000,0.000092000000000,0.001468000000000,0.000882000000000,0.002583000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1124.000000000000000,218.000000000000000,121.000000000000000,0.199078575841603,0.231934072485734,121.000000000000000,0.343262412908369
0.000000000000000,0.000014000000000,0.000090000000000,0.000084000000000,0.001429000000000,0.000750000000000,0.002400000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,217.000000000000000,121.000000000000000,0.199480471035799,0.201976563717399,121.000000000000000,0.298925319680116
0.000000000000000,0.000016000000000,0.000086000000000,0.000082000000000,0.001377000000000,0.000677000000000,0.002270000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1170.000000000000000,216.000000000000000,121.000000000000000,0.220678346305332,0.284081275414871,121.000000000000000,0.420440299824576
0.000000000000000,0.000027000000000,0.000102000000000,0.000086000000000,0.001575000000000,0.000893000000000,0.002726000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1207.000000000000000,220.000000000000000,121.000000000000000,0.211162657455426,0.207040156377078,121.000000000000000,0.306419439695680
0.000000000000000,0.000016000000000,0.000108000000000,0.000083000000000,0.001461000000000,0.000764000000000,0.002474000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1249.000000000000000,230.000000000000000,121.000000000000000,0.227293453622315,0.273472763274248,121.000000000000000,0.404739696351758
0.000000000000000,0.000016000000000,0.000088000000000,0.000078000000000,0.001528000000000,0.000670000000000,0.002548000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,235.000000000000000,121.000000000000000,0.230650938262149,0.341719334696620,121.000000000000000,0.505744634189104
0.000000000000000,0.000014000000000,0.000157000000000,0.000097000000000,0.001482000000000,0.000903000000000,0.002719000000000,0.000000000000000,106.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1118.000000000000000,225.000000000000000,121.000000000000000,0.192289115182234,0.284781239296802,121.000000000000000,0.421476244276178
0.000000000000000,0.000009000000000,0.000107000000000,0.000085000000000,0.001393000000000,0.000822000000000,0.002450000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1112.000000000000000,223.000000000000000,121.000000000000000,0.206644050900703,0.414842370987539,120.000000000000000,0.613966712358429
0.000000000000000,0.000013000000000,0.000119000000000,0.000087000000000,0.001684000000000,0.000696000000000,0.002637000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1116.000000000000000,248.000000000000000,121.000000000000000,0.217279659889557,0.386405943424662,121.000000000000000,0.571880811943511
0.000000000000000,0.000013000000000,0.000107000000000,0.000086000000000,0.001569000000000,0.000810000000000,0.002621000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1115.000000000000000,245.000000000000000,121.000000000000000,0.205227319768628,0.260415513728504,121.000000000000000,0.385414955241683
0.000000000000000,0.000017000000000,0.000090000000000,0.000082000000000,0.001607000000000,0.000835000000000,0.002666000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1141.000000000000000,234.000000000000000,121.000000000000000,0.215577332340184,0.253414375214643,120.000000000000000,0.375053286801197
0.000000000000000,0.000018000000000,0.000118000000000,0.000089000000000,0.001476000000000,0.000855000000000,0.002594000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1162.000000000000000,189.000000000000000,121.000000000000000,0.263380285246488,0.344156012084719,121.000000000000000,0.509350879343151
0.000000000000000,0.000019000000000,0.000107000000000,0.000079000000000,0.001350000000000,0.000913000000000,0.002498000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1191.000000000000000,211.000000000000000,121.000000000000000,0.211941539220644,0.302129709054244,121.000000000000000,0.447152008225909
0.000000000000000,0.000017000000000,0.000113000000000,0.000084000000000,0.001500000000000,0.000792000000000,0.002540000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,227.000000000000000,121.000000000000000,0.202379365741516,0.390009847847033,120.000000000000000,0.577214540970843
0.000000000000000,0.000011000000000,0.000089000000000,0.000087000000000,0.001487000000000,0.000793000000000,0.002500000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1231.000000000000000,213.000000000000000,121.000000000000000,0.214175551012714,0.216953676956891,120.000000000000000,0.321091452830759
0.000000000000000,0.000015000000000,0.000086000000000,0.000080000000000,0.001432000000000,0.000687000000000,0.002331000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,223.000000000000000,121.000000000000000,0.199434039847726,0.257284849007253,121.000000000000000,0.380781610790163
0.000000000000000,0.000014000000000,0.000093000000000,0.000084000000000,0.001629000000000,0.000756000000000,0.002612000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,237.000000000000000,121.000000000000000,0.197980635555219,0.261749609075989,121.000000000000000,0.387389446361990
0.000000000000000,0.000017000000000,0.000088000000000,0.000087000000000,0.001493000000000,0.000846000000000,0.002566000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,228.000000000000000,121.000000000000000,0.236508484356981,0.240200224772688,121.000000000000000,0.355496322487429
0.000000000000000,0.000014000000000,0.000164000000000,0.000088000000000,0.001671000000000,0.000811000000000,0.002926000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,248.000000000000000,121.000000000000000,0.184315305243839,0.201680214668134,121.000000000000000,0.298486726394656
0.000000000000000,0.000013000000000,0.000164000000000,0.000099000000000,0.001590000000000,0.000831000000000,0.002766000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1105.000000000000000,230.000000000000000,121.000000000000000,0.190613955848338,0.200507971284196,120.000000000000000,0.296751787320034
0.000000000000000,0.000016000000000,0.000087000000000,0.000085000000000,0.001377000000000,0.000850000000000,0.002444000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1104.000000000000000,218.000000000000000,121.000000000000000,0.200535674470726,0.209035608733197,120.000000000000000,0.309372710300047
0.000000000000000,0.000013000000000,0.000091000000000,0.000089000000000,0.001407000000000,0.000797000000000,0.002429000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,201.000000000000000,121.000000000000000,0.227018445721033,0.269171979187375,121.000000000000000,0.398374546537365
0.000000000000000,0.000024000000000,0.000159000000000,0.000086000000000,0.001427000000000,0.000852000000000,0.002589000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,206.000000000000000,121.000000000000000,0.206458268024795,0.213071493736729,121.000000000000000,0.315345817611568
0.000000000000000,0.000023000000000,0.000096000000000,0.000084000000000,0.001443000000000,0.000848000000000,0.002527000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,197.000000000000000,121.000000000000000,0.195773117170614,0.235723820191720,120.000000000000000,0.348871251500985
0.000000000000000,0.000018000000000,0.000166000000000,0.000097000000000,0.001547000000000,0.000786000000000,0.002646000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,206.000000000000000,121.000000000000000,0.202449512908354,0.259327833750046,121.000000000000000,0.383805213005791
0.000000000000000,0.000015000000000,0.000089000000000,0.000085000000000,0.001354000000000,0.000899000000000,0.002473000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1255.000000000000000,195.000000000000000,121.000000000000000,0.234803205654552,0.251609191002594,120.000000000000000,0.372381595360564
0.000000000000000,0.000015000000000,0.000148000000000,0.000079000000000,0.001434000000000,0.000759000000000,0.002612000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1267.000000000000000,221.000000000000000,121.000000000000000,0.207185371940379,0.272198837560189,121.000000000000000,0.402854263297596
0.000000000000000,0.000014000000000,0.000110000000000,0.000089000000000,0.001471000000000,0.000697000000000,0.002442000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,216.000000000000000,121.000000000000000,0.170542136055104,0.199853580496046,121.000000000000000,0.295783293486634
0.000000000000000,0.000011000000000,0.000140000000000,0.000086000000000,0.001577000000000,0.000769000000000,0.002617000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,234.000000000000000,121.000000000000000,0.189245949418993,0.197736413403659,121.000000000000000,0.292649910272330
0.000000000000000,0.000018000000000,0.000166000000000,0.000094000000000,0.001662000000000,0.000699000000000,0.002677000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1177.000000000000000,225.000000000000000,121.000000000000000,0.198921843902340,0.245387061075502,119.000000000000000,0.363172854853005
0.000000000000000,0.000010000000000,0.000149000000000,0.000107000000000,0.001458000000000,0.000785000000000,0.002541000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,222.000000000000000,121.000000000000000,0.196061687568370,0.262447572567731,121.000000000000000,0.388422408733057
0.000000000000000,0.000013000000000,0.000150000000000,0.000086000000000,0.001461000000000,0.000690000000000,0.002431000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1264.000000000000000,222.000000000000000,121.000000000000000,0.188165247064654,0.226893158617754,120.000000000000000,0.335801879206626
0.000000000000000,0.000011000000000,0.000087000000000,0.000090000000000,0.001543000000000,0.000884000000000,0.002648000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1280.000000000000000,221.000000000000000,121.000000000000000,0.195179134679885,0.208025099223027,119.000000000000000,0.307877146368027
0.000000000000000,0.000014000000000,0.000208000000000,0.000082000000000,0.001576000000000,0.000716000000000,0.002630000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1295.000000000000000,262.000000000000000,121.000000000000000,0.204969117934416,0.216857241486732,119.000000000000000,0.320948742586853
0.000000000000000,0.000024000000000,0.000169000000000,0.000098000000000,0.001343000000000,0.000853000000000,0.002520000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,192.000000000000000,121.000000000000000,0.227441413063451,0.257511508846672,120.000000000000000,0.381117019034739
0.000000000000000,0.000009000000000,0.000087000000000,0.000083000000000,0.001482000000000,0.000867000000000,0.002560000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,234.000000000000000,121.000000000000000,0.206543419238523,0.240601524255942,121.000000000000000,0.356090287223134
0.000000000000000,0.000017000000000,0.000085000000000,0.000090000000000,0.001430000000000,0.000831000000000,0.002586000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1312.000000000000000,207.000000000000000,121.000000000000000,0.208575675835164,0.214150033311086,121.000000000000000,0.316942039397577
0.000000000000000,0.000015000000000,0.000164000000000,0.000102000000000,0.001530000000000,0.000841000000000,0.002712000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1139.000000000000000,223.000000000000000,121.000000000000000,0.209522023711657,0.213144284667314,121.000000000000000,0.315453551588861
0.000000000000000,0.000015000000000,0.000105000000000,0.000078000000000,0.001301000000000,0.000820000000000,0.002350000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1137.000000000000000,212.000000000000000,121.000000000000000,0.214867803164927,0.261245240464500,120.000000000000000,0.386642965897231
0.000000000000000,0.000016000000000,0.000101000000000,0.000080000000000,0.001381000000000,0.000813000000000,0.002424000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,188.000000000000000,121.000000000000000,0.205519375020890,0.194082271824269,121.000000000000000,0.287241755590969
0.000000000000000,0.000013000000000,0.000149000000000,0.000084000000000,0.001317000000000,0.000634000000000,0.002230000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,190.000000000000000,121.000000000000000,0.193630790736426,0.209509974832839,121.000000000000000,0.310074762819209
0.000000000000000,0.000020000000000,0.000095000000000,0.000087000000000,0.001465000000000,0.000730000000000,0.002432000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1251.000000000000000,218.000000000000000,121.000000000000000,0.192820932489665,0.213663301525895,121.000000000000000,0.316221664773433
0.000000000000000,0.000019000000000,0.000163000000000,0.000085000000000,0.001400000000000,0.000794000000000,0.002498000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1258.000000000000000,187.000000000000000,121.000000000000000,0.178472893331951,0.196146108608914,120.000000000000000,0.290296240030441
0.000000000000000,0.000012000000000,0.000108000000000,0.000091000000000,0.001293000000000,0.000751000000000,0.002285000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1274.000000000000000,203.000000000000000,121.000000000000000,0.204380791067604,0.211515445058425,119.000000000000000,0.313042868010643
0.000000000000000,0.000017000000000,0.000161000000000,0.000078000000000,0.001276000000000,0.000745000000000,0.002316000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1284.000000000000000,207.000000000000000,121.000000000000000,0.187808408411530,0.214316116353771,121.000000000000000,0.317187858866760
0.000000000000000,0.000015000000000,0.000093000000000,0.000089000000000,0.001466000000000,0.000749000000000,0.002445000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1286.000000000000000,207.000000000000000,121.000000000000000,0.199989980411572,0.230213140472726,121.000000000000000,0.340715438140956
0.000000000000000,0.000016000000000,0.000150000000000,0.000081000000000,0.001405000000000,0.000699000000000,0.002385000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,213.000000000000000,121.000000000000000,0.206268660764417,0.237247311670881,121.000000000000000,0.351126007647841
0.000000000000000,0.000020000000000,0.000155000000000,0.000079000000000,0.001337000000000,0.000555000000000,0.002180000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1279.000000000000000,203.000000000000000,121.000000000000000,0.183003834372267,0.219788084820077,121.000000000000000,0.325286355628032
0.000000000000000,0.000024000000000,0.000162000000000,0.000089000000000,0.001394000000000,0.000718000000000,0.002565000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1272.000000000000000,191.000000000000000,121.000000000000000,0.186553971804931,0.182751341988290,121.000000000000000,0.270472000430761
0.000000000000000,0.000016000000000,0.000109000000000,0.000097000000000,0.001309000000000,0.000793000000000,0.002379000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1148.000000000000000,206.000000000000000,121.000000000000000,0.182081620850879,0.190648173777097,119.000000000000000,0.282159304756511
0.000000000000000,0.000018000000000,0.000165000000000,0.000085000000000,0.001434000000000,0.001000000000000,0.002736000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1146.000000000000000,198.000000000000000,121.000000000000000,0.161190220960006,0.185797469108689,121.000000000000000,0.274980249131025
0.000000000000000,0.000017000000000,0.000065000000000,0.000084000000000,0.001165000000000,0.000868000000000,0.002227000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1202.000000000000000,183.000000000000000,121.000000000000000,0.182593650165736,0.182593650165736,120.000000000000000,0.270238614721097
0.000000000000000,0.000013000000000,0.000106000000000,0.000081000000000,0.001303000000000,0.000768000000000,0.002301000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1222.000000000000000,185.000000000000000,121.000000000000000,0.166736786826383,0.199418227406995,120.000000000000000,0.295138987188000
0.000000000000000,0.000015000000000,0.000109000000000,0.000095000000000,0.001360000000000,0.000938000000000,0.002546000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1249.000000000000000,189.000000000000000,121.000000000000000,0.191808443828625,0.226302606500564,121.000000000000000,0.334927876891194
0.000000000000000,0.000019000000000,0.000163000000000,0.000087000000000,0.001234000000000,0.000928000000000,0.002463000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,165.000000000000000,121.000000000000000,0.166580449599247,0.180745814173528,121.000000000000000,0.267503794151850
0.000000000000000,0.000023000000000,0.000090000000000,0.000087000000000,0.001327000000000,0.000948000000000,0.002508000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1289.000000000000000,189.000000000000000,121.000000000000000,0.204446978142314,0.213236190122245,120.000000000000000,0.315589564787498
0.000000000000000,0.000012000000000,0.000089000000000,0.000086000000000,0.001424000000000,0.000752000000000,0.002394000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,207.000000000000000,121.000000000000000,0.189969084150173,0.239096854811035,121.000000000000000,0.353863330132112
0.000000000000000,0.000018000000000,0.000151000000000,0.000084000000000,0.001303000000000,0.000702000000000,0.002297000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1309.000000000000000,198.000000000000000,121.000000000000000,0.203248106998635,0.283179509252321,120.000000000000000,0.419105667153969
0.000000000000000,0.000018000000000,0.000121000000000,0.000114000000000,0.001451000000000,0.000873000000000,0.002610000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1309.000000000000000,186.000000000000000,121.000000000000000,0.193450874101625,0.200224929073400,120.000000000000000,0.296332880820479
0.000000000000000,0.000016000000000,0.000099000000000,0.000085000000000,0.001266000000000,0.000850000000000,0.002346000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1307.000000000000000,182.000000000000000,121.000000000000000,0.190247303390951,0.176735411055755,120.000000000000000,0.261568417740656
0.000000000000000,0.000017000000000,0.000148000000000,0.000080000000000,0.001332000000000,0.000666000000000,0.002391000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1339.000000000000000,203.000000000000000,121.000000000000000,0.200414786051663,0.221307330406924,119.000000000000000,0.327534844351391
0.000000000000000,0.000016000000000,0.000157000000000,0.000094000000000,0.001205000000000,0.000724000000000,0.002264000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1203.000000000000000,168.000000000000000,121.000000000000000,0.199743231584850,0.225956586080112,121.000000000000000,0.334415767738987
0.000000000000000,0.000011000000000,0.000087000000000,0.000092000000000,0.001277000000000,0.000769000000000,0.002270000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1198.000000000000000,184.000000000000000,121.000000000000000,0.148096017248236,0.229791243682201,121.000000000000000,0.340091046075047
0.000000000000000,0.000015000000000,0.000098000000000,0.000084000000000,0.001401000000000,0.000839000000000,0.002470000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,207.000000000000000,121.000000000000000,0.171701277226300,0.189474819161897,121.000000000000000,0.280422723010407
0.000000000000000,0.000015000000000,0.000157000000000,0.000086000000000,0.001295000000000,0.000747000000000,0.002332000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1272.000000000000000,197.000000000000000,121.000000000000000,0.154842494097145,0.164895074486922,120.000000000000000,0.244044726914351
0.000000000000000,0.000013000000000,0.000151000000000,0.000078000000000,0.001295000000000,0.000656000000000,0.002225000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1288.000000000000000,190.000000000000000,121.000000000000000,0.171037240946432,0.274189059551541,121.000000000000000,0.405799825981935
0.000000000000000,0.000012000000000,0.000163000000000,0.000096000000000,0.001404000000000,0.000731000000000,0.002442000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1300.000000000000000,186.000000000000000,121.000000000000000,0.183021843966101,0.234105160743232,121.000000000000000,0.346475629956656
0.000000000000000,0.000015000000000,0.000096000000000,0.000083000000000,0.001337000000000,0.000726000000000,0.002288000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1307.000000000000000,185.000000000000000,121.000000000000000,0.183461220328528,0.207991417189789,121.000000000000000,0.307827284970608
0.000000000000000,0.000016000000000,0.000149000000000,0.000080000000000,0.001274000000000,0.000606000000000,0.002156000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1313.000000000000000,200.000000000000000,121.000000000000000,0.203158950593758,0.241305735673881,121.000000000000000,0.357132499098233
0.000000000000000,0.000012000000000,0.000152000000000,0.000083000000000,0.001408000000000,0.000621000000000,0.002310000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1319.000000000000000,207.000000000000000,121.000000000000000,0.172769464465069,0.242423202061487,121.000000000000000,0.358786340745426
0.000000000000000,0.000017000000000,0.000155000000000,0.000087000000000,0.001210000000000,0.000693000000000,0.002332000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1314.000000000000000,179.000000000000000,121.000000000000000,0.189900236210407,0.248224778263959,121.000000000000000,0.367372660392853
0.000000000000000,0.000013000000000,0.000157000000000,0.000092000000000,0.001368000000000,0.000607000000000,0.002301000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1110.000000000000000,204.000000000000000,121.000000000000000,0.167414207784906,0.238891530166090,121.000000000000000,0.353559486451843
0.000000000000000,0.000017000000000,0.000092000000000,0.000082000000000,0.001333000000000,0.000748000000000,0.002306000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1103.000000000000000,197.000000000000000,121.000000000000000,0.164998017179115,0.188230341089860,121.000000000000000,0.278580909202036
0.000000000000000,0.000011000000000,0.000150000000000,0.000082000000000,0.001388000000000,0.000755000000000,0.002418000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1146.000000000000000,219.000000000000000,121.000000000000000,0.173088299071632,0.182167160144569,121.000000000000000,0.269607399250574
0.000000000000000,0.000014000000000,0.000095000000000,0.000091000000000,0.001460000000000,0.000843000000000,0.002540000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1188.000000000000000,199.000000000000000,121.000000000000000,0.159193450711399,0.194342796414950,121.000000000000000,0.287627328495391
0.000000000000000,0.000023000000000,0.000162000000000,0.000084000000000,0.001428000000000,0.000661000000000,0.002402000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1213.000000000000000,213.000000000000000,121.000000000000000,0.163604775275874,0.178874584867471,121.000000000000000,0.264734376394025
0.000000000000000,0.000015000000000,0.000091000000000,0.000079000000000,0.001243000000000,0.000704000000000,0.002165000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1240.000000000000000,184.000000000000000,121.000000000000000,0.179908260850595,0.207929795609648,121.000000000000000,0.307736104067471
0.000000000000000,0.000014000000000,0.000152000000000,0.000086000000000,0.001314000000000,0.000826000000000,0.002424000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1263.000000000000000,189.000000000000000,121.000000000000000,0.159876479501220,0.178421775752828,121.000000000000000,0.264064216973465
0.000000000000000,0.000013000000000,0.000087000000000,0.000080000000000,0.001408000000000,0.000757000000000,0.002377000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,210.000000000000000,121.000000000000000,0.180877939732212,0.181463986212420,121.000000000000000,0.268566703687611
0.000000000000000,0.000011000000000,0.000088000000000,0.000086000000000,0.001449000000000,0.000746000000000,0.002414000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1279.000000000000000,209.000000000000000,121.000000000000000,0.194139433375495,0.249420140021997,121.000000000000000,0.369141792306152
0.000000000000000,0.000013000000000,0.000095000000000,0.000083000000000,0.001315000000000,0.000730000000000,0.002270000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1277.000000000000000,178.000000000000000,121.000000000000000,0.156793757882320,0.178864440219203,120.000000000000000,0.264719390175743
0.000000000000000,0.000012000000000,0.000127000000000,0.000079000000000,0.001313000000000,0.000701000000000,0.002265000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,185.000000000000000,121.000000000000000,0.164883000136156,0.240733177667065,121.000000000000000,0.356285108060795
0.000000000000000,0.000014000000000,0.000166000000000,0.000100000000000,0.001393000000000,0.000725000000000,0.002432000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1318.000000000000000,183.000000000000000,121.000000000000000,0.135728889598211,0.162060045311901,119.000000000000000,0.239848863786058
0.000000000000000,0.000015000000000,0.000155000000000,0.000089000000000,0.001343000000000,0.000750000000000,0.002387000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1356.000000000000000,191.000000000000000,121.000000000000000,0.173788063146282,0.195529217779223,121.000000000000000,0.289383243221234
0.000000000000000,0.000016000000000,0.000150000000000,0.000078000000000,0.001258000000000,0.000644000000000,0.002300000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1395.000000000000000,185.000000000000000,121.000000000000000,0.151475576157850,0.175539257809215,121.000000000000000,0.259798098049517
0.000000000000000,0.000011000000000,0.000151000000000,0.000096000000000,0.001555000000000,0.000651000000000,0.002540000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1235.000000000000000,239.000000000000000,121.000000000000000,0.155311911245058,0.179651409086422,121.000000000000000,0.265884094799596
0.000000000000000,0.000013000000000,0.000088000000000,0.000084000000000,0.001324000000000,0.000729000000000,0.002270000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,203.000000000000000,121.000000000000000,0.164537143262273,0.221612984534127,121.000000000000000,0.327987236077292
0.000000000000000,0.000013000000000,0.000098000000000,0.000087000000000,0.001493000000000,0.000702000000000,0.002430000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1240.000000000000000,208.000000000000000,121.000000000000000,0.175143219815607,0.215695284047881,121.000000000000000,0.319229017177332
0.000000000000000,0.000023000000000,0.000119000000000,0.000088000000000,0.001527000000000,0.000683000000000,0.002476000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,213.000000000000000,121.000000000000000,0.153675591056653,0.182664252108742,121.000000000000000,0.270343088627296
0.000000000000000,0.000014000000000,0.000151000000000,0.000078000000000,0.001233000000000,0.000696000000000,0.002204000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1239.000000000000000,203.000000000000000,121.000000000000000,0.153229115354477,0.193081832581164,121.000000000000000,0.285761102061624
0.000000000000000,0.000021000000000,0.000143000000000,0.000123000000000,0.001403000000000,0.000694000000000,0.002424000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,184.000000000000000,121.000000000000000,0.139965152794629,0.227002185063016,120.000000000000000,0.335963240089810
0.000000000000000,0.000015000000000,0.000110000000000,0.000088000000000,0.001404000000000,0.000704000000000,0.002355000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,209.000000000000000,121.000000000000000,0.137197338848433,0.208531405635090,121.000000000000000,0.308626482553977
0.000000000000000,0.000018000000000,0.000088000000000,0.000079000000000,0.001190000000000,0.000700000000000,0.002107000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,188.000000000000000,121.000000000000000,0.130941105653028,0.146060909721641,121.000000000000000,0.216170158735599
0.000000000000000,0.000021000000000,0.000118000000000,0.000089000000000,0.001415000000000,0.000816000000000,0.002492000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,184.000000000000000,121.000000000000000,0.153353796799625,0.199263806316360,121.000000000000000,0.294910453677173
0.000000000000000,0.000017000000000,0.000151000000000,0.000082000000000,0.001223000000000,0.000698000000000,0.002202000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1228.000000000000000,179.000000000000000,121.000000000000000,0.140213553319483,0.175196026122660,121.000000000000000,0.259290133483817
0.000000000000000,0.000019000000000,0.000155000000000,0.000078000000000,0.001255000000000,0.000811000000000,0.002351000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1229.000000000000000,190.000000000000000,121.000000000000000,0.130014281845411,0.153376137782893,120.000000000000000,0.226996677933372
0.000000000000000,0.000017000000000,0.000119000000000,0.000089000000000,0.001341000000000,0.000705000000000,0.002417000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1228.000000000000000,181.000000000000000,121.000000000000000,0.153048040967559,0.195396547391727,121.000000000000000,0.289186880799568
0.000000000000000,0.000034000000000,0.000168000000000,0.000085000000000,0.001355000000000,0.000876000000000,0.002565000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1098.000000000000000,224.000000000000000,121.000000000000000,0.139772469128747,0.173322524076028,121.000000000000000,0.256517354567422
0.000000000000000,0.000017000000000,0.000164000000000,0.000090000000000,0.001416000000000,0.000893000000000,0.002614000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1096.000000000000000,199.000000000000000,121.000000000000000,0.153304734612976,0.183642468789981,120.000000000000000,0.271790863455149
0.000000000000000,0.000016000000000,0.000151000000000,0.000085000000000,0.001314000000000,0.000712000000000,0.002309000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1169.000000000000000,206.000000000000000,121.000000000000000,0.142897294139319,0.181555340311233,120.000000000000000,0.268701908186442
0.000000000000000,0.000015000000000,0.000117000000000,0.000085000000000,0.001376000000000,0.000780000000000,0.002405000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,222.000000000000000,121.000000000000000,0.126799378274116,0.146656265445468,120.000000000000000,0.217051287718072
0.000000000000000,0.000016000000000,0.000165000000000,0.000106000000000,0.001478000000000,0.000760000000000,0.002565000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1236.000000000000000,202.000000000000000,121.000000000000000,0.140084421158853,0.204486312942259,121.000000000000000,0.302639753705096
0.000000000000000,0.000011000000000,0.000107000000000,0.000080000000000,0.001194000000000,0.000749000000000,0.002169000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1246.000000000000000,201.000000000000000,121.000000000000000,0.139419993339841,0.168128905916729,121.000000000000000,0.248830789269877
0.000000000000000,0.000014000000000,0.000152000000000,0.000092000000000,0.001362000000000,0.000736000000000,0.002390000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1247.000000000000000,196.000000000000000,121.000000000000000,0.145258293429478,0.175683893358186,121.000000000000000,0.260012150779442
0.000000000000000,0.000017000000000,0.000165000000000,0.000083000000000,0.001281000000000,0.000789000000000,0.002367000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1243.000000000000000,182.000000000000000,121.000000000000000,0.130776175712068,0.189705854296339,121.000000000000000,0.280764651397170
0.000000000000000,0.000014000000000,0.000148000000000,0.000080000000000,0.001146000000000,0.000646000000000,0.002066000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1231.000000000000000,185.000000000000000,121.000000000000000,0.160961980060978,0.209744716515705,121.000000000000000,0.310422175201532
0.000000000000000,0.000017000000000,0.000165000000000,0.000099000000000,0.001434000000000,0.000756000000000,0.002649000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1229.000000000000000,192.000000000000000,121.000000000000000,0.148237727429110,0.178425698740388,121.000000000000000,0.264070029503321
0.000000000000000,0.000009000000000,0.000152000000000,0.000088000000000,0.001271000000000,0.000627000000000,0.002203000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1070.000000000000000,202.000000000000000,121.000000000000000,0.145517296057557,0.194221829742895,121.000000000000000,0.287448302575818
0.000000000000000,0.000013000000000,0.000171000000000,0.000110000000000,0.001292000000000,0.000667000000000,0.002296000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1062.000000000000000,207.000000000000000,121.000000000000000,0.144870620749843,0.185268776731061,120.000000000000000,0.274197806796764
0.000000000000000,0.000012000000000,0.000167000000000,0.000090000000000,0.001672000000000,0.000784000000000,0.002761000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1051.000000000000000,240.000000000000000,121.000000000000000,0.147451415472805,0.239110494103313,121.000000000000000,0.353883547627265
0.000000000000000,0.000023000000000,0.000169000000000,0.000084000000000,0.001225000000000,0.000708000000000,0.002240000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1058.000000000000000,193.000000000000000,121.000000000000000,0.139221728219021,0.208258123486744,121.000000000000000,0.308222006291587
0.000000000000000,0.000018000000000,0.000114000000000,0.000095000000000,0.001441000000000,0.000867000000000,0.002569000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1122.000000000000000,195.000000000000000,121.000000000000000,0.144798866574318,0.161179631549979,121.000000000000000,0.238545871495364
0.000000000000000,0.000015000000000,0.000087000000000,0.000084000000000,0.001190000000000,0.000950000000000,0.002357000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1161.000000000000000,181.000000000000000,121.000000000000000,0.156951809426305,0.174642571732918,121.000000000000000,0.258471021667723
0.000000000000000,0.000025000000000,0.000109000000000,0.000081000000000,0.001177000000000,0.000732000000000,0.002154000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1178.000000000000000,188.000000000000000,121.000000000000000,0.143966687089071,0.148286357619255,121.000000000000000,0.219463816142722
0.000000000000000,0.000016000000000,0.000167000000000,0.000091000000000,0.001397000000000,0.000699000000000,0.002561000000000,1.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1185.000000000000000,186.000000000000000,121.000000000000000,0.141904983877111,0.215844079586902,120.000000000000000,0.319449236243278
0.000000000000000,0.000013000000000,0.000107000000000,0.000086000000000,0.001203000000000,0.000597000000000,0.002056000000000,0.000000000000000,109.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1024.000000000000000,198.000000000000000,121.000000000000000,0.158784086059447,0.201694183533691,121.000000000000000,0.298507398783450
0.000000000000000,0.000011000000000,0.000112000000000,0.000087000000000,0.001357000000000,0.000625000000000,0.002227000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1012.000000000000000,200.000000000000000,121.000000000000000,0.142538196203460,0.192228508218454,121.000000000000000,0.284498190955069
0.000000000000000,0.000011000000000,0.000111000000000,0.000090000000000,0.001358000000000,0.000662000000000,0.002264000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1012.000000000000000,207.000000000000000,121.000000000000000,0.168549877781207,0.187821262175372,121.000000000000000,0.277975471037833
0.000000000000000,0.000012000000000,0.000149000000000,0.000117000000000,0.001286000000000,0.000829000000000,0.002423000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1008.000000000000000,214.000000000000000,121.000000000000000,0.143759909717310,0.192182987333624,121.000000000000000,0.284430816152474
0.000000000000000,0.000015000000000,0.000167000000000,0.000090000000000,0.001493000000000,0.000810000000000,0.002612000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1001.000000000000000,212.000000000000000,121.000000000000000,0.174890959675259,0.213312437262072,121.000000000000000,0.315702428954185
0.000000000000000,0.000019000000000,0.000151000000000,0.000086000000000,0.001409000000000,0.000712000000000,0.002407000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1001.000000000000000,207.000000000000000,121.000000000000000,0.152789140763855,0.164105200132453,121.000000000000000,0.242875700800910
0.000000000000000,0.000016000000000,0.000087000000000,0.000081000000000,0.001205000000000,0.000602000000000,0.002152000000000,1.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1043.000000000000000,199.000000000000000,121.000000000000000,0.163939125281343,0.202213714394798,120.000000000000000,0.299276295395959
0.000000000000000,0.000011000000000,0.000111000000000,0.000101000000000,0.001485000000000,0.000606000000000,0.002388000000000,0.000000000000000,105.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,933.000000000000000,228.000000000000000,121.000000000000000,0.162381916777401,0.199296542527132,121.000000000000000,0.294958899849931
0.000000000000000,0.000012000000000,0.000086000000000,0.000086000000000,0.001214000000000,0.000711000000000,0.002139000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,930.000000000000000,188.000000000000000,121.000000000000000,0.126548973306942,0.138574150155498,120.000000000000000,0.205089745706516
0.000000000000000,0.000012000000000,0.000167000000000,0.000087000000000,0.001455000000000,0.000638000000000,0.002392000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,959.000000000000000,226.000000000000000,121.000000000000000,0.166801187642930,0.183010323861591,120.000000000000000,0.270855273595110
0.000000000000000,0.000014000000000,0.000088000000000,0.000087000000000,0.001409000000000,0.000740000000000,0.002371000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,966.000000000000000,208.000000000000000,121.000000000000000,0.151605751950910,0.214754024488413,120.000000000000000,0.317835955945735
0.000000000000000,0.000017000000000,0.000112000000000,0.000085000000000,0.001310000000000,0.000658000000000,0.002217000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,966.000000000000000,202.000000000000000,121.000000000000000,0.149723343863280,0.232974269786109,121.000000000000000,0.344801899348607
0.000000000000000,0.000014000000000,0.000143000000000,0.000095000000000,0.001352000000000,0.001073000000000,0.002710000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,980.000000000000000,184.000000000000000,121.000000000000000,0.155861567427539,0.183956647347986,120.000000000000000,0.272255840571782
0.000000000000000,0.000014000000000,0.000149000000000,0.000084000000000,0.001120000000000,0.000720000000000,0.002120000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,984.000000000000000,177.000000000000000,121.000000000000000,0.150101106525736,0.181285553460186,121.000000000000000,0.268302612657185
0.000000000000000,0.000011000000000,0.000154000000000,0.000077000000000,0.001135000000000,0.000704000000000,0.002114000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1009.000000000000000,162.000000000000000,121.000000000000000,0.164203457795522,0.166918334037277,121.000000000000000,0.247039140121353
0.000000000000000,0.000014000000000,0.000153000000000,0.000087000000000,0.001265000000000,0.000661000000000,0.002212000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1044.000000000000000,184.000000000000000,121.000000000000000,0.166079753752638,0.235937082097295,121.000000000000000,0.349186897144050
0.000000000000000,0.000016000000000,0.000087000000000,0.000079000000000,0.001083000000000,0.000776000000000,0.002071000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1065.000000000000000,179.000000000000000,121.000000000000000,0.140784932440615,0.174109634177219,121.000000000000000,0.257682261366227
0.000000000000000,0.000017000000000,0.000177000000000,0.000085000000000,0.001217000000000,0.000893000000000,0.002614000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1085.000000000000000,173.000000000000000,121.000000000000000,0.136693367503916,0.164873100634404,121.000000000000000,0.244012202019025
0.000000000000000,0.000010000000000,0.000111000000000,0.000098000000000,0.001335000000000,0.000674000000000,0.002290000000000,0.000000000000000,108.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,940.000000000000000,210.000000000000000,121.000000000000000,0.152064884062461,0.157064948938113,121.000000000000000,0.232456109236813
0.000000000000000,0.000009000000000,0.000148000000000,0.000085000000000,0.001199000000000,0.000682000000000,0.002153000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,937.000000000000000,196.000000000000000,121.000000000000000,0.149276999735119,0.172780031999726,121.000000000000000,0.255714442019319
0.000000000000000,0.000016000000000,0.000157000000000,0.000094000000000,0.001300000000000,0.000821000000000,0.002420000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,948.000000000000000,187.000000000000000,121.000000000000000,0.151768200456098,0.155713508180996,121.000000000000000,0.230455992441400
0.000000000000000,0.000019000000000,0.000116000000000,0.000085000000000,0.001196000000000,0.000791000000000,0.002239000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,950.000000000000000,186.000000000000000,121.000000000000000,0.144000321191294,0.179229516503960,121.000000000000000,0.265259702733687
0.000000000000000,0.000013000000000,0.000154000000000,0.000083000000000,0.001139000000000,0.000808000000000,0.002231000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,960.000000000000000,170.000000000000000,121.000000000000000,0.152250251263816,0.176190168633417,121.000000000000000,0.260761436421654
0.000000000000000,0.000015000000000,0.000087000000000,0.000085000000000,0.001127000000000,0.000768000000000,0.002115000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,983.000000000000000,164.000000000000000,121.000000000000000,0.129302901903334,0.143871781002835,121.000000000000000,0.212930229321842
0.000000000000000,0.000013000000000,0.000110000000000,0.000120000000000,0.001152000000000,0.000761000000000,0.002188000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1013.000000000000000,181.000000000000000,121.000000000000000,0.137216294671414,0.165518456926789,121.000000000000000,0.244967327033756
0.000000000000000,0.000022000000000,0.000167000000000,0.000085000000000,0.001236000000000,0.000710000000000,0.002252000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,170.000000000000000,121.000000000000000,0.162107693844438,0.225222383429965,121.000000000000000,0.333329153190160
0.000000000000000,0.000014000000000,0.000108000000000,0.000083000000000,0.001153000000000,0.000703000000000,0.002206000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1073.000000000000000,176.000000000000000,121.000000000000000,0.142487698690500,0.215619270887844,121.000000000000000,0.319116532088679
0.000000000000000,0.000016000000000,0.000149000000000,0.000080000000000,0.001141000000000,0.000754000000000,0.002190000000000,0.000000000000000,109.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,919.000000000000000,194.000000000000000,121.000000000000000,0.144721479076393,0.144784816827571,121.000000000000000,0.214281516154081
0.000000000000000,0.000013000000000,0.000152000000000,0.000093000000000,0.001294000000000,0.000832000000000,0.002420000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,915.000000000000000,177.000000000000000,121.000000000000000,0.149099082449318,0.191998203412644,121.000000000000000,0.284157323986802
0.000000000000000,0.000018000000000,0.000087000000000,0.000081000000000,0.001097000000000,0.000702000000000,0.002015000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,933.000000000000000,176.000000000000000,121.000000000000000,0.155861769273800,0.170207046079660,121.000000000000000,0.251906426263653
0.000000000000000,0.000014000000000,0.000089000000000,0.000078000000000,0.001146000000000,0.000908000000000,0.002268000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,942.000000000000000,168.000000000000000,121.000000000000000,0.138592736529167,0.162826237027208,121.000000000000000,0.240982837817279
0.000000000000000,0.000020000000000,0.000091000000000,0.000094000000000,0.001136000000000,0.000619000000000,0.001992000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,952.000000000000000,158.000000000000000,121.000000000000000,0.145657951253513,0.188004859202689,121.000000000000000,0.278247194172679
0.000000000000000,0.000020000000000,0.000125000000000,0.000078000000000,0.001053000000000,0.000766000000000,0.002073000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,981.000000000000000,173.000000000000000,121.000000000000000,0.128806331282994,0.198803320775784,121.000000000000000,0.294228921915590
0.000000000000000,0.000019000000000,0.000167000000000,0.000089000000000,0.001255000000000,0.000878000000000,0.002445000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1004.000000000000000,174.000000000000000,121.000000000000000,0.118066645939065,0.158172695543332,121.000000000000000,0.234095596462448
0.000000000000000,0.000014000000000,0.000107000000000,0.000083000000000,0.001134000000000,0.000719000000000,0.002090000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1033.000000000000000,174.000000000000000,121.000000000000000,0.120270585655071,0.185382011446256,121.000000000000000,0.274365384559707
0.000000000000000,0.000023000000000,0.000149000000000,0.000076000000000,0.001048000000000,0.000778000000000,0.002104000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1055.000000000000000,177.000000000000000,121.000000000000000,0.111655902944065,0.127479790231990,121.000000000000000,0.188670081746738
0.000000000000000,0.000024000000000,0.000165000000000,0.000093000000000,0.001273000000000,0.000666000000000,0.002422000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1068.000000000000000,161.000000000000000,121.000000000000000,0.137252105053442,0.201665031989628,121.000000000000000,0.298464234431299
0.000000000000000,0.000016000000000,0.000151000000000,0.000085000000000,0.001226000000000,0.000594000000000,0.002131000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,917.000000000000000,199.000000000000000,121.000000000000000,0.109849180876551,0.169849754533253,121.000000000000000,0.251377637677945
0.000000000000000,0.000019000000000,0.000166000000000,0.000083000000000,0.001090000000000,0.000732000000000,0.002128000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,914.000000000000000,161.000000000000000,121.000000000000000,0.094619378225366,0.127736587063336,121.000000000000000,0.189050145383732
0.000000000000000,0.000019000000000,0.000109000000000,0.000094000000000,0.001174000000000,0.000652000000000,0.002081000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,924.000000000000000,172.000000000000000,121.000000000000000,0.095987819870883,0.183985249071388,121.000000000000000,0.272298170952257
0.000000000000000,0.000013000000000,0.000151000000000,0.000081000000000,0.001173000000000,0.000806000000000,0.002255000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,931.000000000000000,188.000000000000000,121.000000000000000,0.103214005987484,0.160102278671277,121.000000000000000,0.236951368196443
0.000000000000000,0.000016000000000,0.000166000000000,0.000090000000000,0.001244000000000,0.000853000000000,0.002405000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,933.000000000000000,176.000000000000000,121.000000000000000,0.107624598314130,0.132136716401229,121.000000000000000,0.195562352203489
0.000000000000000,0.000019000000000,0.000160000000000,0.000083000000000,0.001074000000000,0.000663000000000,0.002029000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,968.000000000000000,161.000000000000000,121.000000000000000,0.105311906661098,0.145668800777700,121.000000000000000,0.215589815537253
0.000000000000000,0.000016000000000,0.000109000000000,0.000079000000000,0.001079000000000,0.000825000000000,0.002137000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,999.000000000000000,164.000000000000000,121.000000000000000,0.115404334777576,0.144024386113689,121.000000000000000,0.213156084014562
0.000000000000000,0.000016000000000,0.000093000000000,0.000099000000000,0.001239000000000,0.000811000000000,0.002291000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1014.000000000000000,171.000000000000000,121.000000000000000,0.102620214117170,0.192238051746968,120.000000000000000,0.284512317929807
0.000000000000000,0.000019000000000,0.000086000000000,0.000082000000000,0.001037000000000,0.000718000000000,0.001972000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,170.000000000000000,121.000000000000000,0.105335250265535,0.144890464932436,121.000000000000000,0.214437873207215
0.000000000000000,0.000017000000000,0.000109000000000,0.000079000000000,0.001050000000000,0.000705000000000,0.001992000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1061.000000000000000,160.000000000000000,121.000000000000000,0.102004773526380,0.101331023468228,121.000000000000000,0.149969916793428
0.000000000000000,0.000019000000000,0.000120000000000,0.000096000000000,0.001236000000000,0.000783000000000,0.002289000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1076.000000000000000,160.000000000000000,121.000000000000000,0.101321819984142,0.125092002453206,121.000000000000000,0.185136164681599
0.000000000000000,0.000017000000000,0.000151000000000,0.000083000000000,0.001093000000000,0.000805000000000,0.002181000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1087.000000000000000,166.000000000000000,121.000000000000000,0.100949350996349,0.133208193438679,121.000000000000000,0.197148136707625
0.000000000000000,0.000016000000000,0.000166000000000,0.000086000000000,0.001225000000000,0.000614000000000,0.002142000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1094.000000000000000,173.000000000000000,121.000000000000000,0.113902102274736,0.263501176586062,121.000000000000000,0.389981758862282
0.000000000000000,0.000017000000000,0.000110000000000,0.000085000000000,0.001057000000000,0.000701000000000,0.002002000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1103.000000000000000,147.000000000000000,121.000000000000000,0.112950870334457,0.159164000744430,121.000000000000000,0.235562729541910
0.000000000000000,0.000015000000000,0.000148000000000,0.000081000000000,0.001005000000000,0.000710000000000,0.002002000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1107.000000000000000,160.000000000000000,121.000000000000000,0.096261721291800,0.169941815822129,121.000000000000000,0.251513903595272
0.000000000000000,0.000014000000000,0.000111000000000,0.000087000000000,0.001226000000000,0.000704000000000,0.002175000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1134.000000000000000,179.000000000000000,121.000000000000000,0.100695432310273,0.149404926005425,121.000000000000000,0.221119300461396
0.000000000000000,0.000017000000000,0.000159000000000,0.000084000000000,0.001069000000000,0.000609000000000,0.001969000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1163.000000000000000,157.000000000000000,121.000000000000000,0.104264909242810,0.168733357468614,121.000000000000000,0.249725362886627
0.000000000000000,0.000016000000000,0.000084000000000,0.000079000000000,0.000994000000000,0.000786000000000,0.001989000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1186.000000000000000,161.000000000000000,121.000000000000000,0.098764835183793,0.142076743187060,121.000000000000000,0.210273574643228
0.000000000000000,0.000014000000000,0.000113000000000,0.000090000000000,0.001119000000000,0.000653000000000,0.002020000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1204.000000000000000,154.000000000000000,121.000000000000000,0.099700583140545,0.115489183389192,121.000000000000000,0.170923985837824
0.000000000000000,0.000015000000000,0.000088000000000,0.000083000000000,0.001096000000000,0.000758000000000,0.002070000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1224.000000000000000,165.000000000000000,121.000000000000000,0.113531975395493,0.159479019440782,121.000000000000000,0.236028944980121
0.000000000000000,0.000016000000000,0.000106000000000,0.000080000000000,0.000950000000000,0.000689000000000,0.001870000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1236.000000000000000,152.000000000000000,121.000000000000000,0.101278477365169,0.121853029810147,121.000000000000000,0.180342482857587
0.000000000000000,0.000026000000000,0.000157000000000,0.000086000000000,0.001250000000000,0.000748000000000,0.002309000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1243.000000000000000,177.000000000000000,121.000000000000000,0.113386397852020,0.206475855003173,121.000000000000000,0.305584280242809
0.000000000000000,0.000018000000000,0.000087000000000,0.000081000000000,0.001080000000000,0.000613000000000,0.001908000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1252.000000000000000,162.000000000000000,121.000000000000000,0.103012888489708,0.192649617277085,121.000000000000000,0.285121445786875
0.000000000000000,0.000014000000000,0.000084000000000,0.000078000000000,0.001104000000000,0.000752000000000,0.002063000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1267.000000000000000,181.000000000000000,121.000000000000000,0.119108801565499,0.146565581346541,121.000000000000000,0.216917068822130
0.000000000000000,0.000017000000000,0.000169000000000,0.000087000000000,0.001262000000000,0.000745000000000,0.002313000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1272.000000000000000,162.000000000000000,121.000000000000000,0.113261864886997,0.130198323725397,121.000000000000000,0.192693514913252
0.000000000000000,0.000013000000000,0.000147000000000,0.000081000000000,0.001101000000000,0.000691000000000,0.002065000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1269.000000000000000,173.000000000000000,121.000000000000000,0.115292532476552,0.143763553384774,121.000000000000000,0.212770056216498
0.000000000000000,0.000017000000000,0.000089000000000,0.000077000000000,0.001052000000000,0.000711000000000,0.001978000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1264.000000000000000,159.000000000000000,121.000000000000000,0.101382577834875,0.107355247718934,121.000000000000000,0.158885781611837
0.000000000000000,0.000016000000000,0.000118000000000,0.000092000000000,0.001316000000000,0.000678000000000,0.002256000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1260.000000000000000,177.000000000000000,121.000000000000000,0.124156248079858,0.184101498216806,121.000000000000000,0.272470221836003
0.000000000000000,0.000018000000000,0.000151000000000,0.000083000000000,0.001038000000000,0.000700000000000,0.002018000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1259.000000000000000,160.000000000000000,121.000000000000000,0.124874729225317,0.136317718156052,121.000000000000000,0.201750219857265
0.000000000000000,0.000019000000000,0.000118000000000,0.000088000000000,0.001213000000000,0.000732000000000,0.002204000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1256.000000000000000,175.000000000000000,121.000000000000000,0.137395115258524,0.272990416298521,121.000000000000000,0.404025816597931
0.000000000000000,0.000033000000000,0.000166000000000,0.000098000000000,0.001332000000000,0.000676000000000,0.002341000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1256.000000000000000,175.000000000000000,121.000000000000000,0.125440559596022,0.192594450198696,121.000000000000000,0.285039792378329
0.000000000000000,0.000011000000000,0.000147000000000,0.000080000000000,0.001052000000000,0.000650000000000,0.001970000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1254.000000000000000,161.000000000000000,121.000000000000000,0.135853494715468,0.174263471494220,121.000000000000000,0.257909935633510
0.000000000000000,0.000016000000000,0.000118000000000,0.000078000000000,0.001158000000000,0.000816000000000,0.002217000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1253.000000000000000,167.000000000000000,121.000000000000000,0.122720689566236,0.207186929273245,121.000000000000000,0.306636651409222
0.000000000000000,0.000015000000000,0.000117000000000,0.000091000000000,0.001177000000000,0.000833000000000,0.002267000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1249.000000000000000,162.000000000000000,121.000000000000000,0.131197397185608,0.147375515585527,121.000000000000000,0.218115764109708
0.000000000000000,0.000011000000000,0.000150000000000,0.000081000000000,0.001038000000000,0.000708000000000,0.002171000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,166.000000000000000,121.000000000000000,0.137378655081558,0.217187593785681,121.000000000000000,0.321437652163372
0.000000000000000,0.000014000000000,0.000152000000000,0.000092000000000,0.001138000000000,0.000700000000000,0.002154000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1068.000000000000000,186.000000000000000,121.000000000000000,0.145478356636836,0.211190875078835,121.000000000000000,0.312562500325826
0.000000000000000,0.000014000000000,0.000151000000000,0.000087000000000,0.001380000000000,0.000695000000000,0.002361000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1066.000000000000000,208.000000000000000,121.000000000000000,0.145910261187635,0.270725437624101,121.000000000000000,0.400673629542342
0.000000000000000,0.000010000000000,0.000106000000000,0.000080000000000,0.001226000000000,0.000670000000000,0.002123000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1063.000000000000000,202.000000000000000,121.000000000000000,0.142580547861068,0.222707058106822,121.000000000000000,0.329606455264036
0.000000000000000,0.000016000000000,0.000091000000000,0.000082000000000,0.001339000000000,0.000780000000000,0.002343000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1056.000000000000000,206.000000000000000,121.000000000000000,0.127026409458953,0.138078812827686,121.000000000000000,0.204356634060426
0.000000000000000,0.000015000000000,0.000112000000000,0.000084000000000,0.001327000000000,0.000760000000000,0.002333000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1046.000000000000000,203.000000000000000,121.000000000000000,0.143390011522010,0.189299220864586,121.000000000000000,0.280162852382094
0.000000000000000,0.000012000000000,0.000153000000000,0.000080000000000,0.001259000000000,0.000815000000000,0.002350000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1041.000000000000000,190.000000000000000,121.000000000000000,0.146470888134392,0.170045702350971,121.000000000000000,0.251667632374036
0.000000000000000,0.000016000000000,0.000109000000000,0.000082000000000,0.001303000000000,0.000864000000000,0.002410000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1035.000000000000000,192.000000000000000,121.000000000000000,0.145288024491270,0.152085543829851,120.000000000000000,0.225086604806994
0.000000000000000,0.000015000000000,0.000130000000000,0.000082000000000,0.001210000000000,0.000564000000000,0.002031000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1033.000000000000000,195.000000000000000,121.000000000000000,0.150132403510209,0.284875350032339,121.000000000000000,0.421615517545908
0.000000000000000,0.000017000000000,0.000142000000000,0.000087000000000,0.001179000000000,0.000822000000000,0.002279000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1032.000000000000000,177.000000000000000,121.000000000000000,0.150560599418679,0.158144787063228,121.000000000000000,0.234054276956730
0.000000000000000,0.000014000000000,0.000166000000000,0.000090000000000,0.001346000000000,0.000786000000000,0.002438000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1025.000000000000000,195.000000000000000,121.000000000000000,0.143981650642650,0.191284534828251,121.000000000000000,0.283101111496310
0.000000000000000,0.000011000000000,0.000086000000000,0.000079000000000,0.001157000000000,0.000713000000000,0.002195000000000,1.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1022.000000000000000,190.000000000000000,121.000000000000000,0.154353905338326,0.194713119296172,121.000000000000000,0.288175424788969
0.000000000000000,0.000015000000000,0.000156000000000,0.000097000000000,0.001181000000000,0.000713000000000,0.002220000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,838.000000000000000,198.000000000000000,121.000000000000000,0.139070972107019,0.141121163279108,121.000000000000000,0.208859322949524
0.000000000000000,0.000015000000000,0.000151000000000,0.000089000000000,0.001225000000000,0.000800000000000,0.002315000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,835.000000000000000,186.000000000000000,121.000000000000000,0.138520459895391,0.192364562878898,121.000000000000000,0.284699557206785
0.000000000000000,0.000016000000000,0.000166000000000,0.000083000000000,0.001375000000000,0.000800000000000,0.002476000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,831.000000000000000
1 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
2 0.000000000000000 0.000000000000000 0.000000000000000 0.000203000000000 0.000000000000000 0.000000000000000 0.006850000000000 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
3 0.000000000000000 0.000000000000000 0.000000000000000 0.000109000000000 0.000000000000000 0.000000000000000 0.015619000000000 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
4 0.000000000000000 0.000000000000000 0.000000000000000 0.000085000000000 0.000000000000000 0.000000000000000 0.015440000000000 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
5 0.000000000000000 0.000000000000000 0.000000000000000 0.000096000000000 0.000000000000000 0.000000000000000 0.016054000000000 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
6 0.000000000000000 0.000000000000000 0.000000000000000 0.000104000000000 0.000000000000000 0.000000000000000 0.022404000000000 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
7 0.000000000000000 0.000000000000000 0.000000000000000 0.000086000000000 0.000000000000000 0.000000000000000 0.027997000000000 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
8 0.000000000000000 0.000000000000000 0.000000000000000 0.000120000000000 0.000000000000000 0.000000000000000 0.031598000000000 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
9 0.000000000000000 0.000000000000000 0.000000000000000 0.000085000000000 0.000000000000000 0.000000000000000 0.030167000000000 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
10 0.000000000000000 0.000000000000000 0.000000000000000 0.000115000000000 0.000000000000000 0.000000000000000 0.029319000000000 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
11 0.000000000000000 0.000000000000000 0.000000000000000 0.000085000000000 0.000000000000000 0.000000000000000 0.030061000000000 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
12 0.000000000000000 0.000000000000000 0.000000000000000 0.000118000000000 0.000000000000000 0.000000000000000 0.034386000000000 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
13 0.000000000000000 0.000000000000000 0.000000000000000 0.000086000000000 0.000000000000000 0.000000000000000 0.089886000000000 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
14 0.000000000000000 0.000031000000000 0.000123000000000 0.000083000000000 0.000787000000000 0.001478000000000 0.002580000000000 0.000000000000000 279.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 148.000000000000000 121.000000000000000 0.170175036288459 0.209622100274688 121.000000000000000 0.310240723182540
15 0.000000000000000 0.000021000000000 0.000169000000000 0.000094000000000 0.000742000000000 0.000759000000000 0.001820000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 151.000000000000000 121.000000000000000 0.200086249073657 0.226929968841294 119.000000000000000 0.335856340084193
16 0.000000000000000 0.000025000000000 0.000161000000000 0.000087000000000 0.000786000000000 0.000849000000000 0.001929000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 150.000000000000000 121.000000000000000 0.220021652685224 0.250579787227198 120.000000000000000 0.370858080741496
17 0.000000000000000 0.000022000000000 0.000154000000000 0.000080000000000 0.000833000000000 0.000799000000000 0.001908000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 153.000000000000000 121.000000000000000 0.241933925519325 0.315090765827434 119.000000000000000 0.466334367626374
18 0.000000000000000 0.000022000000000 0.000147000000000 0.000078000000000 0.000733000000000 0.000857000000000 0.001878000000000 1.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 145.000000000000000 121.000000000000000 0.272951477661557 0.278442109473506 118.000000000000000 0.412094340922640
19 0.000000000000000 0.000028000000000 0.000163000000000 0.000105000000000 0.000998000000000 0.000769000000000 0.002090000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 176.000000000000000 121.000000000000000 0.295685270342958 0.345204254144622 120.000000000000000 0.510902319377397
20 0.000000000000000 0.000026000000000 0.000096000000000 0.000108000000000 0.000884000000000 0.000849000000000 0.001987000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 154.000000000000000 121.000000000000000 0.280726298758396 0.351937246615374 119.000000000000000 0.520867118388044
21 0.000000000000000 0.000027000000000 0.000145000000000 0.000077000000000 0.000795000000000 0.000757000000000 0.001822000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 160.000000000000000 121.000000000000000 0.299712701755874 0.325868377142253 119.000000000000000 0.482285213145440
22 0.000000000000000 0.000017000000000 0.000088000000000 0.000082000000000 0.000819000000000 0.000723000000000 0.001753000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 0.000000000000000 157.000000000000000 121.000000000000000 0.266414412346027 0.325724036228748 121.000000000000000 0.482071564765419
23 0.000000000000000 0.000018000000000 0.000117000000000 0.000090000000000 0.000805000000000 0.000748000000000 0.001799000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 53.000000000000000 146.000000000000000 121.000000000000000 0.268513468986649 0.275002624188711 120.000000000000000 0.407003878905728
24 0.000000000000000 0.000017000000000 0.000084000000000 0.000077000000000 0.000701000000000 0.000777000000000 0.001677000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 76.000000000000000 141.000000000000000 121.000000000000000 0.291779187140528 0.283057832886000 119.000000000000000 0.418925579815900
25 0.000000000000000 0.000019000000000 0.000215000000000 0.000093000000000 0.000797000000000 0.000821000000000 0.001970000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 96.000000000000000 146.000000000000000 121.000000000000000 0.284752636918227 0.313033086538782 118.000000000000000 0.463288955787851
26 0.000000000000000 0.000018000000000 0.000115000000000 0.000087000000000 0.000928000000000 0.000752000000000 0.001937000000000 1.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 108.000000000000000 164.000000000000000 121.000000000000000 0.266984542077384 0.310809089011309 117.000000000000000 0.459997446489587
27 0.000000000000000 0.000018000000000 0.000145000000000 0.000076000000000 0.000879000000000 0.000667000000000 0.001797000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 82.000000000000000 172.000000000000000 121.000000000000000 0.281472783188953 0.318106490866299 118.000000000000000 0.470797632399925
28 0.000000000000000 0.000025000000000 0.000097000000000 0.000092000000000 0.001059000000000 0.001001000000000 0.002299000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 82.000000000000000 169.000000000000000 121.000000000000000 0.259578545621370 0.296051217193402 118.000000000000000 0.438155829408007
29 0.000000000000000 0.000023000000000 0.000152000000000 0.000090000000000 0.000987000000000 0.000683000000000 0.001956000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 88.000000000000000 169.000000000000000 121.000000000000000 0.260854021729684 0.463271425215932 119.000000000000000 0.685641725628487
30 0.000000000000000 0.000012000000000 0.000147000000000 0.000078000000000 0.000828000000000 0.000628000000000 0.001713000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 100.000000000000000 162.000000000000000 121.000000000000000 0.224857463762793 0.241819177256775 120.000000000000000 0.357892398925400
31 0.000000000000000 0.000015000000000 0.000176000000000 0.000082000000000 0.000976000000000 0.000761000000000 0.002033000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 175.000000000000000 163.000000000000000 121.000000000000000 0.232802573731276 0.307670936583970 121.000000000000000 0.455352982415734
32 0.000000000000000 0.000015000000000 0.000150000000000 0.000089000000000 0.000914000000000 0.000700000000000 0.001890000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 215.000000000000000 163.000000000000000 121.000000000000000 0.223503209746196 0.245376568533207 121.000000000000000 0.363157337925476
33 0.000000000000000 0.000014000000000 0.000085000000000 0.000085000000000 0.000830000000000 0.000688000000000 0.001724000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 236.000000000000000 160.000000000000000 121.000000000000000 0.238679764456232 0.260245227551971 121.000000000000000 0.385162944164610
34 0.000000000000000 0.000017000000000 0.000094000000000 0.000082000000000 0.001027000000000 0.000661000000000 0.001919000000000 1.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 253.000000000000000 156.000000000000000 121.000000000000000 0.240028080500584 0.347317487309391 119.000000000000000 0.514029864789977
35 0.000000000000000 0.000017000000000 0.000107000000000 0.000086000000000 0.001095000000000 0.000731000000000 0.002049000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 225.000000000000000 203.000000000000000 121.000000000000000 0.237050681546672 0.342187692298344 118.000000000000000 0.506437791010399
36 0.000000000000000 0.000027000000000 0.000087000000000 0.000082000000000 0.001031000000000 0.000707000000000 0.001956000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 224.000000000000000 192.000000000000000 121.000000000000000 0.228256268353354 0.255917210772511 118.000000000000000 0.378757460447388
37 0.000000000000000 0.000028000000000 0.000163000000000 0.000113000000000 0.001066000000000 0.000688000000000 0.002083000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 237.000000000000000 174.000000000000000 121.000000000000000 0.253024524781435 0.270830951731115 117.000000000000000 0.400829834964262
38 0.000000000000000 0.000024000000000 0.000148000000000 0.000082000000000 0.000924000000000 0.000665000000000 0.001864000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 243.000000000000000 168.000000000000000 121.000000000000000 0.255837794475283 0.309767339197390 117.000000000000000 0.458455660309276
39 0.000000000000000 0.000024000000000 0.000086000000000 0.000079000000000 0.000949000000000 0.000709000000000 0.001867000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 247.000000000000000 176.000000000000000 121.000000000000000 0.242039138156127 0.295753126320285 120.000000000000000 0.437714633120047
40 0.000000000000000 0.000017000000000 0.000162000000000 0.000105000000000 0.001033000000000 0.000869000000000 0.002208000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 284.000000000000000 173.000000000000000 121.000000000000000 0.234035640036960 0.262962619721412 120.000000000000000 0.389184684115904
41 0.000000000000000 0.000017000000000 0.000148000000000 0.000084000000000 0.000934000000000 0.000632000000000 0.001837000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 323.000000000000000 168.000000000000000 121.000000000000000 0.243963350316049 0.254676670136256 118.000000000000000 0.376921484440742
42 0.000000000000000 0.000020000000000 0.000115000000000 0.000080000000000 0.000954000000000 0.000883000000000 0.002074000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 346.000000000000000 164.000000000000000 121.000000000000000 0.237498578056768 0.242318556788754 120.000000000000000 0.358631474732560
43 0.000000000000000 0.000026000000000 0.000160000000000 0.000084000000000 0.001068000000000 0.000705000000000 0.002067000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 360.000000000000000 174.000000000000000 121.000000000000000 0.231204379503806 0.310626553544348 119.000000000000000 0.459727290210615
44 0.000000000000000 0.000025000000000 0.000107000000000 0.000082000000000 0.000854000000000 0.000734000000000 0.001826000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 371.000000000000000 154.000000000000000 121.000000000000000 0.226372225769396 0.240389889691023 118.000000000000000 0.355777042407619
45 0.000000000000000 0.000018000000000 0.000109000000000 0.000077000000000 0.000903000000000 0.000645000000000 0.001789000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 383.000000000000000 163.000000000000000 121.000000000000000 0.243430388985134 0.289702067134667 116.000000000000000 0.428759091467402
46 0.000000000000000 0.000023000000000 0.000162000000000 0.000088000000000 0.001172000000000 0.000728000000000 0.002190000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 351.000000000000000 189.000000000000000 121.000000000000000 0.238491169554930 0.289513420723387 119.000000000000000 0.428479887859349
47 0.000000000000000 0.000021000000000 0.000095000000000 0.000089000000000 0.001107000000000 0.000638000000000 0.001974000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 351.000000000000000 187.000000000000000 121.000000000000000 0.253535525839367 0.299150984189304 119.000000000000000 0.442743431776745
48 0.000000000000000 0.000012000000000 0.000107000000000 0.000073000000000 0.001079000000000 0.000677000000000 0.001968000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 360.000000000000000 182.000000000000000 121.000000000000000 0.242747319355396 0.331002407056278 119.000000000000000 0.489883554348514
49 0.000000000000000 0.000022000000000 0.000152000000000 0.000092000000000 0.001093000000000 0.000806000000000 0.002196000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 420.000000000000000 166.000000000000000 121.000000000000000 0.214060131146887 0.291831223233246 120.000000000000000 0.431910240805571
50 0.000000000000000 0.000013000000000 0.000086000000000 0.000086000000000 0.001065000000000 0.000781000000000 0.002051000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 467.000000000000000 189.000000000000000 121.000000000000000 0.191542689678233 0.219467905989458 120.000000000000000 0.324812508085401
51 0.000000000000000 0.000017000000000 0.000087000000000 0.000076000000000 0.001000000000000 0.000696000000000 0.001897000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 491.000000000000000 176.000000000000000 121.000000000000000 0.237287862315746 0.290385534097234 119.000000000000000 0.429770572749869
52 0.000000000000000 0.000015000000000 0.000098000000000 0.000115000000000 0.001209000000000 0.000852000000000 0.002312000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 503.000000000000000 195.000000000000000 121.000000000000000 0.255419259577233 0.271365864372703 119.000000000000000 0.401621476258818
53 0.000000000000000 0.000014000000000 0.000106000000000 0.000102000000000 0.001070000000000 0.000640000000000 0.001952000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 519.000000000000000 192.000000000000000 121.000000000000000 0.243354924512186 0.322133979720464 118.000000000000000 0.476758255408191
54 0.000000000000000 0.000015000000000 0.000087000000000 0.000074000000000 0.001024000000000 0.000658000000000 0.001878000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 537.000000000000000 182.000000000000000 121.000000000000000 0.256839605640840 0.290001626967172 120.000000000000000 0.429202410592353
55 0.000000000000000 0.000019000000000 0.000171000000000 0.000101000000000 0.001223000000000 0.000747000000000 0.002282000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 551.000000000000000 186.000000000000000 121.000000000000000 0.234920975451741 0.278307444024956 121.000000000000000 0.411895021692308
56 0.000000000000000 0.000020000000000 0.000085000000000 0.000077000000000 0.001001000000000 0.000765000000000 0.001993000000000 1.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 566.000000000000000 182.000000000000000 121.000000000000000 0.236213272390075 0.243545153610833 120.000000000000000 0.360446828894150
57 0.000000000000000 0.000019000000000 0.000102000000000 0.000073000000000 0.001140000000000 0.000640000000000 0.001988000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 523.000000000000000 194.000000000000000 121.000000000000000 0.256770484355549 0.281629623166612 120.000000000000000 0.416811865969600
58 0.000000000000000 0.000018000000000 0.000099000000000 0.000139000000000 0.001286000000000 0.000761000000000 0.002328000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 523.000000000000000 193.000000000000000 121.000000000000000 0.238587016371888 0.245006436065432 120.000000000000000 0.362609544894325
59 0.000000000000000 0.000020000000000 0.000152000000000 0.000079000000000 0.001167000000000 0.000673000000000 0.002111000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 525.000000000000000 207.000000000000000 121.000000000000000 0.272520106859475 0.292688892408874 120.000000000000000 0.433179596238706
60 0.000000000000000 0.000022000000000 0.000132000000000 0.000074000000000 0.001159000000000 0.000620000000000 0.002036000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 532.000000000000000 193.000000000000000 121.000000000000000 0.300157301652565 0.352279034698071 121.000000000000000 0.521372960116753
61 0.000000000000000 0.000025000000000 0.000088000000000 0.000085000000000 0.001215000000000 0.000771000000000 0.002209000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 541.000000000000000 192.000000000000000 121.000000000000000 0.262925275260352 0.296134704229152 121.000000000000000 0.438279358303388
62 0.000000000000000 0.000018000000000 0.000115000000000 0.000090000000000 0.001041000000000 0.000632000000000 0.001918000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 565.000000000000000 171.000000000000000 121.000000000000000 0.281978346961253 0.308467305071063 121.000000000000000 0.456531611839373
63 0.000000000000000 0.000024000000000 0.000152000000000 0.000078000000000 0.001256000000000 0.000758000000000 0.002299000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 608.000000000000000 201.000000000000000 121.000000000000000 0.262127606608173 0.344179498178872 120.000000000000000 0.509385653434813
64 0.000000000000000 0.000015000000000 0.000150000000000 0.000084000000000 0.001197000000000 0.000656000000000 0.002124000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 636.000000000000000 196.000000000000000 121.000000000000000 0.243118892914018 0.307363483400101 120.000000000000000 0.454897987687333
65 0.000000000000000 0.000021000000000 0.000153000000000 0.000080000000000 0.001146000000000 0.000654000000000 0.002075000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 663.000000000000000 198.000000000000000 121.000000000000000 0.308232628226545 0.324201861364523 120.000000000000000 0.479818779824340
66 0.000000000000000 0.000014000000000 0.000090000000000 0.000080000000000 0.001210000000000 0.000638000000000 0.002083000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 687.000000000000000 198.000000000000000 121.000000000000000 0.262176220963531 0.309537239684066 120.000000000000000 0.458115096603436
67 0.000000000000000 0.000016000000000 0.000088000000000 0.000083000000000 0.001372000000000 0.000608000000000 0.002184000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 661.000000000000000 229.000000000000000 121.000000000000000 0.297772564345863 0.357204547667525 120.000000000000000 0.528662731800066
68 0.000000000000000 0.000019000000000 0.000117000000000 0.000170000000000 0.001186000000000 0.000723000000000 0.002237000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 661.000000000000000 195.000000000000000 121.000000000000000 0.273843523747758 0.292259687716359 119.000000000000000 0.432544362541022
69 0.000000000000000 0.000024000000000 0.000153000000000 0.000084000000000 0.001302000000000 0.000641000000000 0.002227000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 668.000000000000000 208.000000000000000 121.000000000000000 0.290286911670839 0.351299244972216 120.000000000000000 0.519922910820696
70 0.000000000000000 0.000016000000000 0.000088000000000 0.000087000000000 0.001312000000000 0.000624000000000 0.002148000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 681.000000000000000 211.000000000000000 121.000000000000000 0.258959269785308 0.286064925174815 120.000000000000000 0.423376082295636
71 0.000000000000000 0.000032000000000 0.000153000000000 0.000075000000000 0.001269000000000 0.000651000000000 0.002202000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 695.000000000000000 219.000000000000000 121.000000000000000 0.299842600943275 0.306289515328744 118.000000000000000 0.453308488218462
72 0.000000000000000 0.000019000000000 0.000193000000000 0.000085000000000 0.001467000000000 0.000596000000000 0.002394000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 704.000000000000000 201.000000000000000 121.000000000000000 0.274895790792161 0.339772965508454 119.000000000000000 0.502863994936072
73 0.000000000000000 0.000017000000000 0.000154000000000 0.000080000000000 0.001269000000000 0.000617000000000 0.002161000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 712.000000000000000 205.000000000000000 121.000000000000000 0.276261362669988 0.281418870291626 120.000000000000000 0.416499935291270
74 0.000000000000000 0.000016000000000 0.000086000000000 0.000075000000000 0.001135000000000 0.000709000000000 0.002044000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 743.000000000000000 194.000000000000000 121.000000000000000 0.308428304394499 0.293336201315787 118.000000000000000 0.434137551702758
75 0.000000000000000 0.000018000000000 0.000171000000000 0.000082000000000 0.001466000000000 0.000783000000000 0.002546000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 769.000000000000000 216.000000000000000 121.000000000000000 0.273449756649530 0.322902720385252 119.000000000000000 0.477896045491622
76 0.000000000000000 0.000016000000000 0.000087000000000 0.000083000000000 0.001273000000000 0.000696000000000 0.002177000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 791.000000000000000 204.000000000000000 121.000000000000000 0.288082179726881 0.251959464844591 120.000000000000000 0.372900022480006
77 0.000000000000000 0.000016000000000 0.000085000000000 0.000074000000000 0.001110000000000 0.000812000000000 0.002149000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 803.000000000000000 190.000000000000000 121.000000000000000 0.282457893847037 0.276453430621912 117.000000000000000 0.409151077978374
78 0.000000000000000 0.000013000000000 0.000172000000000 0.000092000000000 0.001460000000000 0.000864000000000 0.002617000000000 0.000000000000000 111.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 781.000000000000000 208.000000000000000 121.000000000000000 0.294189238611744 0.317837647902468 121.000000000000000 0.470399701551591
79 0.000000000000000 0.000011000000000 0.000108000000000 0.000167000000000 0.001206000000000 0.000875000000000 0.002390000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 781.000000000000000 194.000000000000000 121.000000000000000 0.243742619771063 0.304182365871834 119.000000000000000 0.450189939591226
80 0.000000000000000 0.000012000000000 0.000101000000000 0.000079000000000 0.001300000000000 0.000731000000000 0.002246000000000 0.000000000000000 114.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 803.000000000000000 220.000000000000000 121.000000000000000 0.265019461457802 0.270352874765002 119.000000000000000 0.400122247905802
81 0.000000000000000 0.000020000000000 0.000165000000000 0.000097000000000 0.001379000000000 0.000946000000000 0.002634000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 811.000000000000000 196.000000000000000 121.000000000000000 0.253755606620435 0.269982081521535 119.000000000000000 0.399573494543632
82 0.000000000000000 0.000016000000000 0.000164000000000 0.000085000000000 0.001383000000000 0.000747000000000 0.002418000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 821.000000000000000 217.000000000000000 121.000000000000000 0.242158681319397 0.277544822144716 118.000000000000000 0.410766329481695
83 0.000000000000000 0.000019000000000 0.000149000000000 0.000078000000000 0.001258000000000 0.000689000000000 0.002215000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 826.000000000000000 208.000000000000000 121.000000000000000 0.247027054438406 0.299934280502362 119.000000000000000 0.443902753492513
84 0.000000000000000 0.000021000000000 0.000089000000000 0.000096000000000 0.001356000000000 0.000776000000000 0.002363000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 835.000000000000000 206.000000000000000 121.000000000000000 0.251693428883974 0.262689374323986 119.000000000000000 0.388780283669121
85 0.000000000000000 0.000014000000000 0.000148000000000 0.000100000000000 0.001257000000000 0.000655000000000 0.002196000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 858.000000000000000 208.000000000000000 121.000000000000000 0.204260997947582 0.239470873759942 121.000000000000000 0.354416885149381
86 0.000000000000000 0.000023000000000 0.000162000000000 0.000076000000000 0.001146000000000 0.000648000000000 0.002087000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 887.000000000000000 182.000000000000000 121.000000000000000 0.288689598171738 0.321106098890548 121.000000000000000 0.475237040529197
87 0.000000000000000 0.000024000000000 0.000153000000000 0.000091000000000 0.001220000000000 0.000643000000000 0.002186000000000 1.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 895.000000000000000 169.000000000000000 121.000000000000000 0.226469268954673 0.283662212895001 120.000000000000000 0.419820102888912
88 0.000000000000000 0.000024000000000 0.000085000000000 0.000080000000000 0.001237000000000 0.000635000000000 0.002077000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 887.000000000000000 201.000000000000000 121.000000000000000 0.248977774242743 0.301416583242808 121.000000000000000 0.446096553891485
89 0.000000000000000 0.000021000000000 0.000171000000000 0.000164000000000 0.001414000000000 0.000769000000000 0.002569000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 881.000000000000000 197.000000000000000 121.000000000000000 0.197091396735570 0.215309872281506 121.000000000000000 0.318658631095349
90 0.000000000000000 0.000020000000000 0.000151000000000 0.000085000000000 0.001385000000000 0.000841000000000 0.002509000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 888.000000000000000 208.000000000000000 121.000000000000000 0.211961816387840 0.217745091845774 121.000000000000000 0.322262753412405
91 0.000000000000000 0.000018000000000 0.000150000000000 0.000078000000000 0.001242000000000 0.000841000000000 0.002356000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 913.000000000000000 191.000000000000000 121.000000000000000 0.223779838328298 0.264533102759499 119.000000000000000 0.391509013717953
92 0.000000000000000 0.000022000000000 0.000097000000000 0.000097000000000 0.001450000000000 0.000723000000000 0.002416000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 960.000000000000000 191.000000000000000 121.000000000000000 0.192480719814296 0.242812842123578 120.000000000000000 0.359363019522775
93 0.000000000000000 0.000020000000000 0.000090000000000 0.000082000000000 0.001223000000000 0.000684000000000 0.002125000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1010.000000000000000 186.000000000000000 121.000000000000000 0.226246256927728 0.246572316756408 121.000000000000000 0.364927025819891
94 0.000000000000000 0.000018000000000 0.000107000000000 0.000079000000000 0.001240000000000 0.000867000000000 0.002334000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1043.000000000000000 191.000000000000000 121.000000000000000 0.213017300445347 0.226962316320547 121.000000000000000 0.335904230275835
95 0.000000000000000 0.000016000000000 0.000187000000000 0.000091000000000 0.001459000000000 0.000724000000000 0.002629000000000 1.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1066.000000000000000 197.000000000000000 121.000000000000000 0.236488382783212 0.308296711179547 118.000000000000000 0.456279145868659
96 0.000000000000000 0.000016000000000 0.000147000000000 0.000085000000000 0.001434000000000 0.000889000000000 0.002595000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1045.000000000000000 222.000000000000000 121.000000000000000 0.203395568382791 0.322649411912471 119.000000000000000 0.477521162587761
97 0.000000000000000 0.000019000000000 0.000093000000000 0.000103000000000 0.001526000000000 0.000764000000000 0.002542000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1041.000000000000000 208.000000000000000 121.000000000000000 0.217461375876614 0.242876047600301 119.000000000000000 0.359456575981592
98 0.000000000000000 0.000012000000000 0.000089000000000 0.000085000000000 0.001381000000000 0.000699000000000 0.002299000000000 0.000000000000000 114.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1048.000000000000000 202.000000000000000 121.000000000000000 0.237082694302344 0.363703604736602 120.000000000000000 0.538281356749968
99 0.000000000000000 0.000013000000000 0.000106000000000 0.000083000000000 0.001290000000000 0.000760000000000 0.002283000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1051.000000000000000 204.000000000000000 121.000000000000000 0.204040454954466 0.217778632970223 121.000000000000000 0.322312387363004
100 0.000000000000000 0.000021000000000 0.000089000000000 0.000095000000000 0.001523000000000 0.000740000000000 0.002518000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1092.000000000000000 216.000000000000000 121.000000000000000 0.189496933340522 0.218462243311750 119.000000000000000 0.323324121364161
101 0.000000000000000 0.000016000000000 0.000114000000000 0.000088000000000 0.001265000000000 0.000870000000000 0.002384000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1150.000000000000000 195.000000000000000 121.000000000000000 0.234911952172899 0.229829985444704 121.000000000000000 0.340148387945673
102 0.000000000000000 0.000022000000000 0.000165000000000 0.000090000000000 0.001383000000000 0.000870000000000 0.002564000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1188.000000000000000 188.000000000000000 121.000000000000000 0.238956119978159 0.242241004289434 119.000000000000000 0.358516689903833
103 0.000000000000000 0.000024000000000 0.000125000000000 0.000088000000000 0.001295000000000 0.000732000000000 0.002498000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1209.000000000000000 181.000000000000000 121.000000000000000 0.231068618034561 0.351821584870681 121.000000000000000 0.520695977291580
104 0.000000000000000 0.000009000000000 0.000086000000000 0.000086000000000 0.001377000000000 0.000690000000000 0.002304000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1131.000000000000000 223.000000000000000 121.000000000000000 0.232053202367997 0.243086811406556 121.000000000000000 0.359768506659923
105 0.000000000000000 0.000015000000000 0.000156000000000 0.000090000000000 0.001467000000000 0.000758000000000 0.002521000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1126.000000000000000 205.000000000000000 121.000000000000000 0.217720599422460 0.232179181942645 120.000000000000000 0.343625190587007
106 0.000000000000000 0.000022000000000 0.000088000000000 0.000083000000000 0.001420000000000 0.000856000000000 0.002503000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1122.000000000000000 225.000000000000000 121.000000000000000 0.210873068435952 0.231191074937355 120.000000000000000 0.342162783347037
107 0.000000000000000 0.000010000000000 0.000096000000000 0.000092000000000 0.001468000000000 0.000882000000000 0.002583000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1124.000000000000000 218.000000000000000 121.000000000000000 0.199078575841603 0.231934072485734 121.000000000000000 0.343262412908369
108 0.000000000000000 0.000014000000000 0.000090000000000 0.000084000000000 0.001429000000000 0.000750000000000 0.002400000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1131.000000000000000 217.000000000000000 121.000000000000000 0.199480471035799 0.201976563717399 121.000000000000000 0.298925319680116
109 0.000000000000000 0.000016000000000 0.000086000000000 0.000082000000000 0.001377000000000 0.000677000000000 0.002270000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1170.000000000000000 216.000000000000000 121.000000000000000 0.220678346305332 0.284081275414871 121.000000000000000 0.420440299824576
110 0.000000000000000 0.000027000000000 0.000102000000000 0.000086000000000 0.001575000000000 0.000893000000000 0.002726000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1207.000000000000000 220.000000000000000 121.000000000000000 0.211162657455426 0.207040156377078 121.000000000000000 0.306419439695680
111 0.000000000000000 0.000016000000000 0.000108000000000 0.000083000000000 0.001461000000000 0.000764000000000 0.002474000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1249.000000000000000 230.000000000000000 121.000000000000000 0.227293453622315 0.273472763274248 121.000000000000000 0.404739696351758
112 0.000000000000000 0.000016000000000 0.000088000000000 0.000078000000000 0.001528000000000 0.000670000000000 0.002548000000000 1.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1276.000000000000000 235.000000000000000 121.000000000000000 0.230650938262149 0.341719334696620 121.000000000000000 0.505744634189104
113 0.000000000000000 0.000014000000000 0.000157000000000 0.000097000000000 0.001482000000000 0.000903000000000 0.002719000000000 0.000000000000000 106.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1118.000000000000000 225.000000000000000 121.000000000000000 0.192289115182234 0.284781239296802 121.000000000000000 0.421476244276178
114 0.000000000000000 0.000009000000000 0.000107000000000 0.000085000000000 0.001393000000000 0.000822000000000 0.002450000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1112.000000000000000 223.000000000000000 121.000000000000000 0.206644050900703 0.414842370987539 120.000000000000000 0.613966712358429
115 0.000000000000000 0.000013000000000 0.000119000000000 0.000087000000000 0.001684000000000 0.000696000000000 0.002637000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1116.000000000000000 248.000000000000000 121.000000000000000 0.217279659889557 0.386405943424662 121.000000000000000 0.571880811943511
116 0.000000000000000 0.000013000000000 0.000107000000000 0.000086000000000 0.001569000000000 0.000810000000000 0.002621000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1115.000000000000000 245.000000000000000 121.000000000000000 0.205227319768628 0.260415513728504 121.000000000000000 0.385414955241683
117 0.000000000000000 0.000017000000000 0.000090000000000 0.000082000000000 0.001607000000000 0.000835000000000 0.002666000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1141.000000000000000 234.000000000000000 121.000000000000000 0.215577332340184 0.253414375214643 120.000000000000000 0.375053286801197
118 0.000000000000000 0.000018000000000 0.000118000000000 0.000089000000000 0.001476000000000 0.000855000000000 0.002594000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1162.000000000000000 189.000000000000000 121.000000000000000 0.263380285246488 0.344156012084719 121.000000000000000 0.509350879343151
119 0.000000000000000 0.000019000000000 0.000107000000000 0.000079000000000 0.001350000000000 0.000913000000000 0.002498000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1191.000000000000000 211.000000000000000 121.000000000000000 0.211941539220644 0.302129709054244 121.000000000000000 0.447152008225909
120 0.000000000000000 0.000017000000000 0.000113000000000 0.000084000000000 0.001500000000000 0.000792000000000 0.002540000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1216.000000000000000 227.000000000000000 121.000000000000000 0.202379365741516 0.390009847847033 120.000000000000000 0.577214540970843
121 0.000000000000000 0.000011000000000 0.000089000000000 0.000087000000000 0.001487000000000 0.000793000000000 0.002500000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1231.000000000000000 213.000000000000000 121.000000000000000 0.214175551012714 0.216953676956891 120.000000000000000 0.321091452830759
122 0.000000000000000 0.000015000000000 0.000086000000000 0.000080000000000 0.001432000000000 0.000687000000000 0.002331000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1244.000000000000000 223.000000000000000 121.000000000000000 0.199434039847726 0.257284849007253 121.000000000000000 0.380781610790163
123 0.000000000000000 0.000014000000000 0.000093000000000 0.000084000000000 0.001629000000000 0.000756000000000 0.002612000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1244.000000000000000 237.000000000000000 121.000000000000000 0.197980635555219 0.261749609075989 121.000000000000000 0.387389446361990
124 0.000000000000000 0.000017000000000 0.000088000000000 0.000087000000000 0.001493000000000 0.000846000000000 0.002566000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1233.000000000000000 228.000000000000000 121.000000000000000 0.236508484356981 0.240200224772688 121.000000000000000 0.355496322487429
125 0.000000000000000 0.000014000000000 0.000164000000000 0.000088000000000 0.001671000000000 0.000811000000000 0.002926000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1223.000000000000000 248.000000000000000 121.000000000000000 0.184315305243839 0.201680214668134 121.000000000000000 0.298486726394656
126 0.000000000000000 0.000013000000000 0.000164000000000 0.000099000000000 0.001590000000000 0.000831000000000 0.002766000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1105.000000000000000 230.000000000000000 121.000000000000000 0.190613955848338 0.200507971284196 120.000000000000000 0.296751787320034
127 0.000000000000000 0.000016000000000 0.000087000000000 0.000085000000000 0.001377000000000 0.000850000000000 0.002444000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1104.000000000000000 218.000000000000000 121.000000000000000 0.200535674470726 0.209035608733197 120.000000000000000 0.309372710300047
128 0.000000000000000 0.000013000000000 0.000091000000000 0.000089000000000 0.001407000000000 0.000797000000000 0.002429000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1144.000000000000000 201.000000000000000 121.000000000000000 0.227018445721033 0.269171979187375 121.000000000000000 0.398374546537365
129 0.000000000000000 0.000024000000000 0.000159000000000 0.000086000000000 0.001427000000000 0.000852000000000 0.002589000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1182.000000000000000 206.000000000000000 121.000000000000000 0.206458268024795 0.213071493736729 121.000000000000000 0.315345817611568
130 0.000000000000000 0.000023000000000 0.000096000000000 0.000084000000000 0.001443000000000 0.000848000000000 0.002527000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1214.000000000000000 197.000000000000000 121.000000000000000 0.195773117170614 0.235723820191720 120.000000000000000 0.348871251500985
131 0.000000000000000 0.000018000000000 0.000166000000000 0.000097000000000 0.001547000000000 0.000786000000000 0.002646000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1234.000000000000000 206.000000000000000 121.000000000000000 0.202449512908354 0.259327833750046 121.000000000000000 0.383805213005791
132 0.000000000000000 0.000015000000000 0.000089000000000 0.000085000000000 0.001354000000000 0.000899000000000 0.002473000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1255.000000000000000 195.000000000000000 121.000000000000000 0.234803205654552 0.251609191002594 120.000000000000000 0.372381595360564
133 0.000000000000000 0.000015000000000 0.000148000000000 0.000079000000000 0.001434000000000 0.000759000000000 0.002612000000000 1.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1267.000000000000000 221.000000000000000 121.000000000000000 0.207185371940379 0.272198837560189 121.000000000000000 0.402854263297596
134 0.000000000000000 0.000014000000000 0.000110000000000 0.000089000000000 0.001471000000000 0.000697000000000 0.002442000000000 0.000000000000000 112.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1147.000000000000000 216.000000000000000 121.000000000000000 0.170542136055104 0.199853580496046 121.000000000000000 0.295783293486634
135 0.000000000000000 0.000011000000000 0.000140000000000 0.000086000000000 0.001577000000000 0.000769000000000 0.002617000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1147.000000000000000 234.000000000000000 121.000000000000000 0.189245949418993 0.197736413403659 121.000000000000000 0.292649910272330
136 0.000000000000000 0.000018000000000 0.000166000000000 0.000094000000000 0.001662000000000 0.000699000000000 0.002677000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1177.000000000000000 225.000000000000000 121.000000000000000 0.198921843902340 0.245387061075502 119.000000000000000 0.363172854853005
137 0.000000000000000 0.000010000000000 0.000149000000000 0.000107000000000 0.001458000000000 0.000785000000000 0.002541000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1221.000000000000000 222.000000000000000 121.000000000000000 0.196061687568370 0.262447572567731 121.000000000000000 0.388422408733057
138 0.000000000000000 0.000013000000000 0.000150000000000 0.000086000000000 0.001461000000000 0.000690000000000 0.002431000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1264.000000000000000 222.000000000000000 121.000000000000000 0.188165247064654 0.226893158617754 120.000000000000000 0.335801879206626
139 0.000000000000000 0.000011000000000 0.000087000000000 0.000090000000000 0.001543000000000 0.000884000000000 0.002648000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1280.000000000000000 221.000000000000000 121.000000000000000 0.195179134679885 0.208025099223027 119.000000000000000 0.307877146368027
140 0.000000000000000 0.000014000000000 0.000208000000000 0.000082000000000 0.001576000000000 0.000716000000000 0.002630000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1295.000000000000000 262.000000000000000 121.000000000000000 0.204969117934416 0.216857241486732 119.000000000000000 0.320948742586853
141 0.000000000000000 0.000024000000000 0.000169000000000 0.000098000000000 0.001343000000000 0.000853000000000 0.002520000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1303.000000000000000 192.000000000000000 121.000000000000000 0.227441413063451 0.257511508846672 120.000000000000000 0.381117019034739
142 0.000000000000000 0.000009000000000 0.000087000000000 0.000083000000000 0.001482000000000 0.000867000000000 0.002560000000000 0.000000000000000 114.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1303.000000000000000 234.000000000000000 121.000000000000000 0.206543419238523 0.240601524255942 121.000000000000000 0.356090287223134
143 0.000000000000000 0.000017000000000 0.000085000000000 0.000090000000000 0.001430000000000 0.000831000000000 0.002586000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1312.000000000000000 207.000000000000000 121.000000000000000 0.208575675835164 0.214150033311086 121.000000000000000 0.316942039397577
144 0.000000000000000 0.000015000000000 0.000164000000000 0.000102000000000 0.001530000000000 0.000841000000000 0.002712000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1139.000000000000000 223.000000000000000 121.000000000000000 0.209522023711657 0.213144284667314 121.000000000000000 0.315453551588861
145 0.000000000000000 0.000015000000000 0.000105000000000 0.000078000000000 0.001301000000000 0.000820000000000 0.002350000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1137.000000000000000 212.000000000000000 121.000000000000000 0.214867803164927 0.261245240464500 120.000000000000000 0.386642965897231
146 0.000000000000000 0.000016000000000 0.000101000000000 0.000080000000000 0.001381000000000 0.000813000000000 0.002424000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1182.000000000000000 188.000000000000000 121.000000000000000 0.205519375020890 0.194082271824269 121.000000000000000 0.287241755590969
147 0.000000000000000 0.000013000000000 0.000149000000000 0.000084000000000 0.001317000000000 0.000634000000000 0.002230000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1230.000000000000000 190.000000000000000 121.000000000000000 0.193630790736426 0.209509974832839 121.000000000000000 0.310074762819209
148 0.000000000000000 0.000020000000000 0.000095000000000 0.000087000000000 0.001465000000000 0.000730000000000 0.002432000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1251.000000000000000 218.000000000000000 121.000000000000000 0.192820932489665 0.213663301525895 121.000000000000000 0.316221664773433
149 0.000000000000000 0.000019000000000 0.000163000000000 0.000085000000000 0.001400000000000 0.000794000000000 0.002498000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1258.000000000000000 187.000000000000000 121.000000000000000 0.178472893331951 0.196146108608914 120.000000000000000 0.290296240030441
150 0.000000000000000 0.000012000000000 0.000108000000000 0.000091000000000 0.001293000000000 0.000751000000000 0.002285000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1274.000000000000000 203.000000000000000 121.000000000000000 0.204380791067604 0.211515445058425 119.000000000000000 0.313042868010643
151 0.000000000000000 0.000017000000000 0.000161000000000 0.000078000000000 0.001276000000000 0.000745000000000 0.002316000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1284.000000000000000 207.000000000000000 121.000000000000000 0.187808408411530 0.214316116353771 121.000000000000000 0.317187858866760
152 0.000000000000000 0.000015000000000 0.000093000000000 0.000089000000000 0.001466000000000 0.000749000000000 0.002445000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1286.000000000000000 207.000000000000000 121.000000000000000 0.199989980411572 0.230213140472726 121.000000000000000 0.340715438140956
153 0.000000000000000 0.000016000000000 0.000150000000000 0.000081000000000 0.001405000000000 0.000699000000000 0.002385000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1278.000000000000000 213.000000000000000 121.000000000000000 0.206268660764417 0.237247311670881 121.000000000000000 0.351126007647841
154 0.000000000000000 0.000020000000000 0.000155000000000 0.000079000000000 0.001337000000000 0.000555000000000 0.002180000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1279.000000000000000 203.000000000000000 121.000000000000000 0.183003834372267 0.219788084820077 121.000000000000000 0.325286355628032
155 0.000000000000000 0.000024000000000 0.000162000000000 0.000089000000000 0.001394000000000 0.000718000000000 0.002565000000000 1.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1272.000000000000000 191.000000000000000 121.000000000000000 0.186553971804931 0.182751341988290 121.000000000000000 0.270472000430761
156 0.000000000000000 0.000016000000000 0.000109000000000 0.000097000000000 0.001309000000000 0.000793000000000 0.002379000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1148.000000000000000 206.000000000000000 121.000000000000000 0.182081620850879 0.190648173777097 119.000000000000000 0.282159304756511
157 0.000000000000000 0.000018000000000 0.000165000000000 0.000085000000000 0.001434000000000 0.001000000000000 0.002736000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1146.000000000000000 198.000000000000000 121.000000000000000 0.161190220960006 0.185797469108689 121.000000000000000 0.274980249131025
158 0.000000000000000 0.000017000000000 0.000065000000000 0.000084000000000 0.001165000000000 0.000868000000000 0.002227000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1202.000000000000000 183.000000000000000 121.000000000000000 0.182593650165736 0.182593650165736 120.000000000000000 0.270238614721097
159 0.000000000000000 0.000013000000000 0.000106000000000 0.000081000000000 0.001303000000000 0.000768000000000 0.002301000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1222.000000000000000 185.000000000000000 121.000000000000000 0.166736786826383 0.199418227406995 120.000000000000000 0.295138987188000
160 0.000000000000000 0.000015000000000 0.000109000000000 0.000095000000000 0.001360000000000 0.000938000000000 0.002546000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1249.000000000000000 189.000000000000000 121.000000000000000 0.191808443828625 0.226302606500564 121.000000000000000 0.334927876891194
161 0.000000000000000 0.000019000000000 0.000163000000000 0.000087000000000 0.001234000000000 0.000928000000000 0.002463000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1276.000000000000000 165.000000000000000 121.000000000000000 0.166580449599247 0.180745814173528 121.000000000000000 0.267503794151850
162 0.000000000000000 0.000023000000000 0.000090000000000 0.000087000000000 0.001327000000000 0.000948000000000 0.002508000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1289.000000000000000 189.000000000000000 121.000000000000000 0.204446978142314 0.213236190122245 120.000000000000000 0.315589564787498
163 0.000000000000000 0.000012000000000 0.000089000000000 0.000086000000000 0.001424000000000 0.000752000000000 0.002394000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1303.000000000000000 207.000000000000000 121.000000000000000 0.189969084150173 0.239096854811035 121.000000000000000 0.353863330132112
164 0.000000000000000 0.000018000000000 0.000151000000000 0.000084000000000 0.001303000000000 0.000702000000000 0.002297000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1309.000000000000000 198.000000000000000 121.000000000000000 0.203248106998635 0.283179509252321 120.000000000000000 0.419105667153969
165 0.000000000000000 0.000018000000000 0.000121000000000 0.000114000000000 0.001451000000000 0.000873000000000 0.002610000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1309.000000000000000 186.000000000000000 121.000000000000000 0.193450874101625 0.200224929073400 120.000000000000000 0.296332880820479
166 0.000000000000000 0.000016000000000 0.000099000000000 0.000085000000000 0.001266000000000 0.000850000000000 0.002346000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1307.000000000000000 182.000000000000000 121.000000000000000 0.190247303390951 0.176735411055755 120.000000000000000 0.261568417740656
167 0.000000000000000 0.000017000000000 0.000148000000000 0.000080000000000 0.001332000000000 0.000666000000000 0.002391000000000 1.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1339.000000000000000 203.000000000000000 121.000000000000000 0.200414786051663 0.221307330406924 119.000000000000000 0.327534844351391
168 0.000000000000000 0.000016000000000 0.000157000000000 0.000094000000000 0.001205000000000 0.000724000000000 0.002264000000000 0.000000000000000 112.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1203.000000000000000 168.000000000000000 121.000000000000000 0.199743231584850 0.225956586080112 121.000000000000000 0.334415767738987
169 0.000000000000000 0.000011000000000 0.000087000000000 0.000092000000000 0.001277000000000 0.000769000000000 0.002270000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1198.000000000000000 184.000000000000000 121.000000000000000 0.148096017248236 0.229791243682201 121.000000000000000 0.340091046075047
170 0.000000000000000 0.000015000000000 0.000098000000000 0.000084000000000 0.001401000000000 0.000839000000000 0.002470000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1234.000000000000000 207.000000000000000 121.000000000000000 0.171701277226300 0.189474819161897 121.000000000000000 0.280422723010407
171 0.000000000000000 0.000015000000000 0.000157000000000 0.000086000000000 0.001295000000000 0.000747000000000 0.002332000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1272.000000000000000 197.000000000000000 121.000000000000000 0.154842494097145 0.164895074486922 120.000000000000000 0.244044726914351
172 0.000000000000000 0.000013000000000 0.000151000000000 0.000078000000000 0.001295000000000 0.000656000000000 0.002225000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1288.000000000000000 190.000000000000000 121.000000000000000 0.171037240946432 0.274189059551541 121.000000000000000 0.405799825981935
173 0.000000000000000 0.000012000000000 0.000163000000000 0.000096000000000 0.001404000000000 0.000731000000000 0.002442000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1300.000000000000000 186.000000000000000 121.000000000000000 0.183021843966101 0.234105160743232 121.000000000000000 0.346475629956656
174 0.000000000000000 0.000015000000000 0.000096000000000 0.000083000000000 0.001337000000000 0.000726000000000 0.002288000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1307.000000000000000 185.000000000000000 121.000000000000000 0.183461220328528 0.207991417189789 121.000000000000000 0.307827284970608
175 0.000000000000000 0.000016000000000 0.000149000000000 0.000080000000000 0.001274000000000 0.000606000000000 0.002156000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1313.000000000000000 200.000000000000000 121.000000000000000 0.203158950593758 0.241305735673881 121.000000000000000 0.357132499098233
176 0.000000000000000 0.000012000000000 0.000152000000000 0.000083000000000 0.001408000000000 0.000621000000000 0.002310000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1319.000000000000000 207.000000000000000 121.000000000000000 0.172769464465069 0.242423202061487 121.000000000000000 0.358786340745426
177 0.000000000000000 0.000017000000000 0.000155000000000 0.000087000000000 0.001210000000000 0.000693000000000 0.002332000000000 1.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1314.000000000000000 179.000000000000000 121.000000000000000 0.189900236210407 0.248224778263959 121.000000000000000 0.367372660392853
178 0.000000000000000 0.000013000000000 0.000157000000000 0.000092000000000 0.001368000000000 0.000607000000000 0.002301000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1110.000000000000000 204.000000000000000 121.000000000000000 0.167414207784906 0.238891530166090 121.000000000000000 0.353559486451843
179 0.000000000000000 0.000017000000000 0.000092000000000 0.000082000000000 0.001333000000000 0.000748000000000 0.002306000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1103.000000000000000 197.000000000000000 121.000000000000000 0.164998017179115 0.188230341089860 121.000000000000000 0.278580909202036
180 0.000000000000000 0.000011000000000 0.000150000000000 0.000082000000000 0.001388000000000 0.000755000000000 0.002418000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1146.000000000000000 219.000000000000000 121.000000000000000 0.173088299071632 0.182167160144569 121.000000000000000 0.269607399250574
181 0.000000000000000 0.000014000000000 0.000095000000000 0.000091000000000 0.001460000000000 0.000843000000000 0.002540000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1188.000000000000000 199.000000000000000 121.000000000000000 0.159193450711399 0.194342796414950 121.000000000000000 0.287627328495391
182 0.000000000000000 0.000023000000000 0.000162000000000 0.000084000000000 0.001428000000000 0.000661000000000 0.002402000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1213.000000000000000 213.000000000000000 121.000000000000000 0.163604775275874 0.178874584867471 121.000000000000000 0.264734376394025
183 0.000000000000000 0.000015000000000 0.000091000000000 0.000079000000000 0.001243000000000 0.000704000000000 0.002165000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1240.000000000000000 184.000000000000000 121.000000000000000 0.179908260850595 0.207929795609648 121.000000000000000 0.307736104067471
184 0.000000000000000 0.000014000000000 0.000152000000000 0.000086000000000 0.001314000000000 0.000826000000000 0.002424000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1263.000000000000000 189.000000000000000 121.000000000000000 0.159876479501220 0.178421775752828 121.000000000000000 0.264064216973465
185 0.000000000000000 0.000013000000000 0.000087000000000 0.000080000000000 0.001408000000000 0.000757000000000 0.002377000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1276.000000000000000 210.000000000000000 121.000000000000000 0.180877939732212 0.181463986212420 121.000000000000000 0.268566703687611
186 0.000000000000000 0.000011000000000 0.000088000000000 0.000086000000000 0.001449000000000 0.000746000000000 0.002414000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1279.000000000000000 209.000000000000000 121.000000000000000 0.194139433375495 0.249420140021997 121.000000000000000 0.369141792306152
187 0.000000000000000 0.000013000000000 0.000095000000000 0.000083000000000 0.001315000000000 0.000730000000000 0.002270000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1277.000000000000000 178.000000000000000 121.000000000000000 0.156793757882320 0.178864440219203 120.000000000000000 0.264719390175743
188 0.000000000000000 0.000012000000000 0.000127000000000 0.000079000000000 0.001313000000000 0.000701000000000 0.002265000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1276.000000000000000 185.000000000000000 121.000000000000000 0.164883000136156 0.240733177667065 121.000000000000000 0.356285108060795
189 0.000000000000000 0.000014000000000 0.000166000000000 0.000100000000000 0.001393000000000 0.000725000000000 0.002432000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1318.000000000000000 183.000000000000000 121.000000000000000 0.135728889598211 0.162060045311901 119.000000000000000 0.239848863786058
190 0.000000000000000 0.000015000000000 0.000155000000000 0.000089000000000 0.001343000000000 0.000750000000000 0.002387000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1356.000000000000000 191.000000000000000 121.000000000000000 0.173788063146282 0.195529217779223 121.000000000000000 0.289383243221234
191 0.000000000000000 0.000016000000000 0.000150000000000 0.000078000000000 0.001258000000000 0.000644000000000 0.002300000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1395.000000000000000 185.000000000000000 121.000000000000000 0.151475576157850 0.175539257809215 121.000000000000000 0.259798098049517
192 0.000000000000000 0.000011000000000 0.000151000000000 0.000096000000000 0.001555000000000 0.000651000000000 0.002540000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1235.000000000000000 239.000000000000000 121.000000000000000 0.155311911245058 0.179651409086422 121.000000000000000 0.265884094799596
193 0.000000000000000 0.000013000000000 0.000088000000000 0.000084000000000 0.001324000000000 0.000729000000000 0.002270000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1230.000000000000000 203.000000000000000 121.000000000000000 0.164537143262273 0.221612984534127 121.000000000000000 0.327987236077292
194 0.000000000000000 0.000013000000000 0.000098000000000 0.000087000000000 0.001493000000000 0.000702000000000 0.002430000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1240.000000000000000 208.000000000000000 121.000000000000000 0.175143219815607 0.215695284047881 121.000000000000000 0.319229017177332
195 0.000000000000000 0.000023000000000 0.000119000000000 0.000088000000000 0.001527000000000 0.000683000000000 0.002476000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1244.000000000000000 213.000000000000000 121.000000000000000 0.153675591056653 0.182664252108742 121.000000000000000 0.270343088627296
196 0.000000000000000 0.000014000000000 0.000151000000000 0.000078000000000 0.001233000000000 0.000696000000000 0.002204000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1239.000000000000000 203.000000000000000 121.000000000000000 0.153229115354477 0.193081832581164 121.000000000000000 0.285761102061624
197 0.000000000000000 0.000021000000000 0.000143000000000 0.000123000000000 0.001403000000000 0.000694000000000 0.002424000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1238.000000000000000 184.000000000000000 121.000000000000000 0.139965152794629 0.227002185063016 120.000000000000000 0.335963240089810
198 0.000000000000000 0.000015000000000 0.000110000000000 0.000088000000000 0.001404000000000 0.000704000000000 0.002355000000000 0.000000000000000 114.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1234.000000000000000 209.000000000000000 121.000000000000000 0.137197338848433 0.208531405635090 121.000000000000000 0.308626482553977
199 0.000000000000000 0.000018000000000 0.000088000000000 0.000079000000000 0.001190000000000 0.000700000000000 0.002107000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1230.000000000000000 188.000000000000000 121.000000000000000 0.130941105653028 0.146060909721641 121.000000000000000 0.216170158735599
200 0.000000000000000 0.000021000000000 0.000118000000000 0.000089000000000 0.001415000000000 0.000816000000000 0.002492000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1230.000000000000000 184.000000000000000 121.000000000000000 0.153353796799625 0.199263806316360 121.000000000000000 0.294910453677173
201 0.000000000000000 0.000017000000000 0.000151000000000 0.000082000000000 0.001223000000000 0.000698000000000 0.002202000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1228.000000000000000 179.000000000000000 121.000000000000000 0.140213553319483 0.175196026122660 121.000000000000000 0.259290133483817
202 0.000000000000000 0.000019000000000 0.000155000000000 0.000078000000000 0.001255000000000 0.000811000000000 0.002351000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1229.000000000000000 190.000000000000000 121.000000000000000 0.130014281845411 0.153376137782893 120.000000000000000 0.226996677933372
203 0.000000000000000 0.000017000000000 0.000119000000000 0.000089000000000 0.001341000000000 0.000705000000000 0.002417000000000 1.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1228.000000000000000 181.000000000000000 121.000000000000000 0.153048040967559 0.195396547391727 121.000000000000000 0.289186880799568
204 0.000000000000000 0.000034000000000 0.000168000000000 0.000085000000000 0.001355000000000 0.000876000000000 0.002565000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1098.000000000000000 224.000000000000000 121.000000000000000 0.139772469128747 0.173322524076028 121.000000000000000 0.256517354567422
205 0.000000000000000 0.000017000000000 0.000164000000000 0.000090000000000 0.001416000000000 0.000893000000000 0.002614000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1096.000000000000000 199.000000000000000 121.000000000000000 0.153304734612976 0.183642468789981 120.000000000000000 0.271790863455149
206 0.000000000000000 0.000016000000000 0.000151000000000 0.000085000000000 0.001314000000000 0.000712000000000 0.002309000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1169.000000000000000 206.000000000000000 121.000000000000000 0.142897294139319 0.181555340311233 120.000000000000000 0.268701908186442
207 0.000000000000000 0.000015000000000 0.000117000000000 0.000085000000000 0.001376000000000 0.000780000000000 0.002405000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1216.000000000000000 222.000000000000000 121.000000000000000 0.126799378274116 0.146656265445468 120.000000000000000 0.217051287718072
208 0.000000000000000 0.000016000000000 0.000165000000000 0.000106000000000 0.001478000000000 0.000760000000000 0.002565000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1236.000000000000000 202.000000000000000 121.000000000000000 0.140084421158853 0.204486312942259 121.000000000000000 0.302639753705096
209 0.000000000000000 0.000011000000000 0.000107000000000 0.000080000000000 0.001194000000000 0.000749000000000 0.002169000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1246.000000000000000 201.000000000000000 121.000000000000000 0.139419993339841 0.168128905916729 121.000000000000000 0.248830789269877
210 0.000000000000000 0.000014000000000 0.000152000000000 0.000092000000000 0.001362000000000 0.000736000000000 0.002390000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1247.000000000000000 196.000000000000000 121.000000000000000 0.145258293429478 0.175683893358186 121.000000000000000 0.260012150779442
211 0.000000000000000 0.000017000000000 0.000165000000000 0.000083000000000 0.001281000000000 0.000789000000000 0.002367000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1243.000000000000000 182.000000000000000 121.000000000000000 0.130776175712068 0.189705854296339 121.000000000000000 0.280764651397170
212 0.000000000000000 0.000014000000000 0.000148000000000 0.000080000000000 0.001146000000000 0.000646000000000 0.002066000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1231.000000000000000 185.000000000000000 121.000000000000000 0.160961980060978 0.209744716515705 121.000000000000000 0.310422175201532
213 0.000000000000000 0.000017000000000 0.000165000000000 0.000099000000000 0.001434000000000 0.000756000000000 0.002649000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1229.000000000000000 192.000000000000000 121.000000000000000 0.148237727429110 0.178425698740388 121.000000000000000 0.264070029503321
214 0.000000000000000 0.000009000000000 0.000152000000000 0.000088000000000 0.001271000000000 0.000627000000000 0.002203000000000 0.000000000000000 114.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1070.000000000000000 202.000000000000000 121.000000000000000 0.145517296057557 0.194221829742895 121.000000000000000 0.287448302575818
215 0.000000000000000 0.000013000000000 0.000171000000000 0.000110000000000 0.001292000000000 0.000667000000000 0.002296000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1062.000000000000000 207.000000000000000 121.000000000000000 0.144870620749843 0.185268776731061 120.000000000000000 0.274197806796764
216 0.000000000000000 0.000012000000000 0.000167000000000 0.000090000000000 0.001672000000000 0.000784000000000 0.002761000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1051.000000000000000 240.000000000000000 121.000000000000000 0.147451415472805 0.239110494103313 121.000000000000000 0.353883547627265
217 0.000000000000000 0.000023000000000 0.000169000000000 0.000084000000000 0.001225000000000 0.000708000000000 0.002240000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1058.000000000000000 193.000000000000000 121.000000000000000 0.139221728219021 0.208258123486744 121.000000000000000 0.308222006291587
218 0.000000000000000 0.000018000000000 0.000114000000000 0.000095000000000 0.001441000000000 0.000867000000000 0.002569000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1122.000000000000000 195.000000000000000 121.000000000000000 0.144798866574318 0.161179631549979 121.000000000000000 0.238545871495364
219 0.000000000000000 0.000015000000000 0.000087000000000 0.000084000000000 0.001190000000000 0.000950000000000 0.002357000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1161.000000000000000 181.000000000000000 121.000000000000000 0.156951809426305 0.174642571732918 121.000000000000000 0.258471021667723
220 0.000000000000000 0.000025000000000 0.000109000000000 0.000081000000000 0.001177000000000 0.000732000000000 0.002154000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1178.000000000000000 188.000000000000000 121.000000000000000 0.143966687089071 0.148286357619255 121.000000000000000 0.219463816142722
221 0.000000000000000 0.000016000000000 0.000167000000000 0.000091000000000 0.001397000000000 0.000699000000000 0.002561000000000 1.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1185.000000000000000 186.000000000000000 121.000000000000000 0.141904983877111 0.215844079586902 120.000000000000000 0.319449236243278
222 0.000000000000000 0.000013000000000 0.000107000000000 0.000086000000000 0.001203000000000 0.000597000000000 0.002056000000000 0.000000000000000 109.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1024.000000000000000 198.000000000000000 121.000000000000000 0.158784086059447 0.201694183533691 121.000000000000000 0.298507398783450
223 0.000000000000000 0.000011000000000 0.000112000000000 0.000087000000000 0.001357000000000 0.000625000000000 0.002227000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1012.000000000000000 200.000000000000000 121.000000000000000 0.142538196203460 0.192228508218454 121.000000000000000 0.284498190955069
224 0.000000000000000 0.000011000000000 0.000111000000000 0.000090000000000 0.001358000000000 0.000662000000000 0.002264000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1012.000000000000000 207.000000000000000 121.000000000000000 0.168549877781207 0.187821262175372 121.000000000000000 0.277975471037833
225 0.000000000000000 0.000012000000000 0.000149000000000 0.000117000000000 0.001286000000000 0.000829000000000 0.002423000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1008.000000000000000 214.000000000000000 121.000000000000000 0.143759909717310 0.192182987333624 121.000000000000000 0.284430816152474
226 0.000000000000000 0.000015000000000 0.000167000000000 0.000090000000000 0.001493000000000 0.000810000000000 0.002612000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1001.000000000000000 212.000000000000000 121.000000000000000 0.174890959675259 0.213312437262072 121.000000000000000 0.315702428954185
227 0.000000000000000 0.000019000000000 0.000151000000000 0.000086000000000 0.001409000000000 0.000712000000000 0.002407000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1001.000000000000000 207.000000000000000 121.000000000000000 0.152789140763855 0.164105200132453 121.000000000000000 0.242875700800910
228 0.000000000000000 0.000016000000000 0.000087000000000 0.000081000000000 0.001205000000000 0.000602000000000 0.002152000000000 1.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1043.000000000000000 199.000000000000000 121.000000000000000 0.163939125281343 0.202213714394798 120.000000000000000 0.299276295395959
229 0.000000000000000 0.000011000000000 0.000111000000000 0.000101000000000 0.001485000000000 0.000606000000000 0.002388000000000 0.000000000000000 105.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 933.000000000000000 228.000000000000000 121.000000000000000 0.162381916777401 0.199296542527132 121.000000000000000 0.294958899849931
230 0.000000000000000 0.000012000000000 0.000086000000000 0.000086000000000 0.001214000000000 0.000711000000000 0.002139000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 930.000000000000000 188.000000000000000 121.000000000000000 0.126548973306942 0.138574150155498 120.000000000000000 0.205089745706516
231 0.000000000000000 0.000012000000000 0.000167000000000 0.000087000000000 0.001455000000000 0.000638000000000 0.002392000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 959.000000000000000 226.000000000000000 121.000000000000000 0.166801187642930 0.183010323861591 120.000000000000000 0.270855273595110
232 0.000000000000000 0.000014000000000 0.000088000000000 0.000087000000000 0.001409000000000 0.000740000000000 0.002371000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 966.000000000000000 208.000000000000000 121.000000000000000 0.151605751950910 0.214754024488413 120.000000000000000 0.317835955945735
233 0.000000000000000 0.000017000000000 0.000112000000000 0.000085000000000 0.001310000000000 0.000658000000000 0.002217000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 966.000000000000000 202.000000000000000 121.000000000000000 0.149723343863280 0.232974269786109 121.000000000000000 0.344801899348607
234 0.000000000000000 0.000014000000000 0.000143000000000 0.000095000000000 0.001352000000000 0.001073000000000 0.002710000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 980.000000000000000 184.000000000000000 121.000000000000000 0.155861567427539 0.183956647347986 120.000000000000000 0.272255840571782
235 0.000000000000000 0.000014000000000 0.000149000000000 0.000084000000000 0.001120000000000 0.000720000000000 0.002120000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 984.000000000000000 177.000000000000000 121.000000000000000 0.150101106525736 0.181285553460186 121.000000000000000 0.268302612657185
236 0.000000000000000 0.000011000000000 0.000154000000000 0.000077000000000 0.001135000000000 0.000704000000000 0.002114000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1009.000000000000000 162.000000000000000 121.000000000000000 0.164203457795522 0.166918334037277 121.000000000000000 0.247039140121353
237 0.000000000000000 0.000014000000000 0.000153000000000 0.000087000000000 0.001265000000000 0.000661000000000 0.002212000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1044.000000000000000 184.000000000000000 121.000000000000000 0.166079753752638 0.235937082097295 121.000000000000000 0.349186897144050
238 0.000000000000000 0.000016000000000 0.000087000000000 0.000079000000000 0.001083000000000 0.000776000000000 0.002071000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1065.000000000000000 179.000000000000000 121.000000000000000 0.140784932440615 0.174109634177219 121.000000000000000 0.257682261366227
239 0.000000000000000 0.000017000000000 0.000177000000000 0.000085000000000 0.001217000000000 0.000893000000000 0.002614000000000 1.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1085.000000000000000 173.000000000000000 121.000000000000000 0.136693367503916 0.164873100634404 121.000000000000000 0.244012202019025
240 0.000000000000000 0.000010000000000 0.000111000000000 0.000098000000000 0.001335000000000 0.000674000000000 0.002290000000000 0.000000000000000 108.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 940.000000000000000 210.000000000000000 121.000000000000000 0.152064884062461 0.157064948938113 121.000000000000000 0.232456109236813
241 0.000000000000000 0.000009000000000 0.000148000000000 0.000085000000000 0.001199000000000 0.000682000000000 0.002153000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 937.000000000000000 196.000000000000000 121.000000000000000 0.149276999735119 0.172780031999726 121.000000000000000 0.255714442019319
242 0.000000000000000 0.000016000000000 0.000157000000000 0.000094000000000 0.001300000000000 0.000821000000000 0.002420000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 948.000000000000000 187.000000000000000 121.000000000000000 0.151768200456098 0.155713508180996 121.000000000000000 0.230455992441400
243 0.000000000000000 0.000019000000000 0.000116000000000 0.000085000000000 0.001196000000000 0.000791000000000 0.002239000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 950.000000000000000 186.000000000000000 121.000000000000000 0.144000321191294 0.179229516503960 121.000000000000000 0.265259702733687
244 0.000000000000000 0.000013000000000 0.000154000000000 0.000083000000000 0.001139000000000 0.000808000000000 0.002231000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 960.000000000000000 170.000000000000000 121.000000000000000 0.152250251263816 0.176190168633417 121.000000000000000 0.260761436421654
245 0.000000000000000 0.000015000000000 0.000087000000000 0.000085000000000 0.001127000000000 0.000768000000000 0.002115000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 983.000000000000000 164.000000000000000 121.000000000000000 0.129302901903334 0.143871781002835 121.000000000000000 0.212930229321842
246 0.000000000000000 0.000013000000000 0.000110000000000 0.000120000000000 0.001152000000000 0.000761000000000 0.002188000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1013.000000000000000 181.000000000000000 121.000000000000000 0.137216294671414 0.165518456926789 121.000000000000000 0.244967327033756
247 0.000000000000000 0.000022000000000 0.000167000000000 0.000085000000000 0.001236000000000 0.000710000000000 0.002252000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1048.000000000000000 170.000000000000000 121.000000000000000 0.162107693844438 0.225222383429965 121.000000000000000 0.333329153190160
248 0.000000000000000 0.000014000000000 0.000108000000000 0.000083000000000 0.001153000000000 0.000703000000000 0.002206000000000 1.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1073.000000000000000 176.000000000000000 121.000000000000000 0.142487698690500 0.215619270887844 121.000000000000000 0.319116532088679
249 0.000000000000000 0.000016000000000 0.000149000000000 0.000080000000000 0.001141000000000 0.000754000000000 0.002190000000000 0.000000000000000 109.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 919.000000000000000 194.000000000000000 121.000000000000000 0.144721479076393 0.144784816827571 121.000000000000000 0.214281516154081
250 0.000000000000000 0.000013000000000 0.000152000000000 0.000093000000000 0.001294000000000 0.000832000000000 0.002420000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 915.000000000000000 177.000000000000000 121.000000000000000 0.149099082449318 0.191998203412644 121.000000000000000 0.284157323986802
251 0.000000000000000 0.000018000000000 0.000087000000000 0.000081000000000 0.001097000000000 0.000702000000000 0.002015000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 933.000000000000000 176.000000000000000 121.000000000000000 0.155861769273800 0.170207046079660 121.000000000000000 0.251906426263653
252 0.000000000000000 0.000014000000000 0.000089000000000 0.000078000000000 0.001146000000000 0.000908000000000 0.002268000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 942.000000000000000 168.000000000000000 121.000000000000000 0.138592736529167 0.162826237027208 121.000000000000000 0.240982837817279
253 0.000000000000000 0.000020000000000 0.000091000000000 0.000094000000000 0.001136000000000 0.000619000000000 0.001992000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 952.000000000000000 158.000000000000000 121.000000000000000 0.145657951253513 0.188004859202689 121.000000000000000 0.278247194172679
254 0.000000000000000 0.000020000000000 0.000125000000000 0.000078000000000 0.001053000000000 0.000766000000000 0.002073000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 981.000000000000000 173.000000000000000 121.000000000000000 0.128806331282994 0.198803320775784 121.000000000000000 0.294228921915590
255 0.000000000000000 0.000019000000000 0.000167000000000 0.000089000000000 0.001255000000000 0.000878000000000 0.002445000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1004.000000000000000 174.000000000000000 121.000000000000000 0.118066645939065 0.158172695543332 121.000000000000000 0.234095596462448
256 0.000000000000000 0.000014000000000 0.000107000000000 0.000083000000000 0.001134000000000 0.000719000000000 0.002090000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1033.000000000000000 174.000000000000000 121.000000000000000 0.120270585655071 0.185382011446256 121.000000000000000 0.274365384559707
257 0.000000000000000 0.000023000000000 0.000149000000000 0.000076000000000 0.001048000000000 0.000778000000000 0.002104000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1055.000000000000000 177.000000000000000 121.000000000000000 0.111655902944065 0.127479790231990 121.000000000000000 0.188670081746738
258 0.000000000000000 0.000024000000000 0.000165000000000 0.000093000000000 0.001273000000000 0.000666000000000 0.002422000000000 1.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1068.000000000000000 161.000000000000000 121.000000000000000 0.137252105053442 0.201665031989628 121.000000000000000 0.298464234431299
259 0.000000000000000 0.000016000000000 0.000151000000000 0.000085000000000 0.001226000000000 0.000594000000000 0.002131000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 917.000000000000000 199.000000000000000 121.000000000000000 0.109849180876551 0.169849754533253 121.000000000000000 0.251377637677945
260 0.000000000000000 0.000019000000000 0.000166000000000 0.000083000000000 0.001090000000000 0.000732000000000 0.002128000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 914.000000000000000 161.000000000000000 121.000000000000000 0.094619378225366 0.127736587063336 121.000000000000000 0.189050145383732
261 0.000000000000000 0.000019000000000 0.000109000000000 0.000094000000000 0.001174000000000 0.000652000000000 0.002081000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 924.000000000000000 172.000000000000000 121.000000000000000 0.095987819870883 0.183985249071388 121.000000000000000 0.272298170952257
262 0.000000000000000 0.000013000000000 0.000151000000000 0.000081000000000 0.001173000000000 0.000806000000000 0.002255000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 931.000000000000000 188.000000000000000 121.000000000000000 0.103214005987484 0.160102278671277 121.000000000000000 0.236951368196443
263 0.000000000000000 0.000016000000000 0.000166000000000 0.000090000000000 0.001244000000000 0.000853000000000 0.002405000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 933.000000000000000 176.000000000000000 121.000000000000000 0.107624598314130 0.132136716401229 121.000000000000000 0.195562352203489
264 0.000000000000000 0.000019000000000 0.000160000000000 0.000083000000000 0.001074000000000 0.000663000000000 0.002029000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 968.000000000000000 161.000000000000000 121.000000000000000 0.105311906661098 0.145668800777700 121.000000000000000 0.215589815537253
265 0.000000000000000 0.000016000000000 0.000109000000000 0.000079000000000 0.001079000000000 0.000825000000000 0.002137000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 999.000000000000000 164.000000000000000 121.000000000000000 0.115404334777576 0.144024386113689 121.000000000000000 0.213156084014562
266 0.000000000000000 0.000016000000000 0.000093000000000 0.000099000000000 0.001239000000000 0.000811000000000 0.002291000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1014.000000000000000 171.000000000000000 121.000000000000000 0.102620214117170 0.192238051746968 120.000000000000000 0.284512317929807
267 0.000000000000000 0.000019000000000 0.000086000000000 0.000082000000000 0.001037000000000 0.000718000000000 0.001972000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1040.000000000000000 170.000000000000000 121.000000000000000 0.105335250265535 0.144890464932436 121.000000000000000 0.214437873207215
268 0.000000000000000 0.000017000000000 0.000109000000000 0.000079000000000 0.001050000000000 0.000705000000000 0.001992000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1061.000000000000000 160.000000000000000 121.000000000000000 0.102004773526380 0.101331023468228 121.000000000000000 0.149969916793428
269 0.000000000000000 0.000019000000000 0.000120000000000 0.000096000000000 0.001236000000000 0.000783000000000 0.002289000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1076.000000000000000 160.000000000000000 121.000000000000000 0.101321819984142 0.125092002453206 121.000000000000000 0.185136164681599
270 0.000000000000000 0.000017000000000 0.000151000000000 0.000083000000000 0.001093000000000 0.000805000000000 0.002181000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1087.000000000000000 166.000000000000000 121.000000000000000 0.100949350996349 0.133208193438679 121.000000000000000 0.197148136707625
271 0.000000000000000 0.000016000000000 0.000166000000000 0.000086000000000 0.001225000000000 0.000614000000000 0.002142000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1094.000000000000000 173.000000000000000 121.000000000000000 0.113902102274736 0.263501176586062 121.000000000000000 0.389981758862282
272 0.000000000000000 0.000017000000000 0.000110000000000 0.000085000000000 0.001057000000000 0.000701000000000 0.002002000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1103.000000000000000 147.000000000000000 121.000000000000000 0.112950870334457 0.159164000744430 121.000000000000000 0.235562729541910
273 0.000000000000000 0.000015000000000 0.000148000000000 0.000081000000000 0.001005000000000 0.000710000000000 0.002002000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1107.000000000000000 160.000000000000000 121.000000000000000 0.096261721291800 0.169941815822129 121.000000000000000 0.251513903595272
274 0.000000000000000 0.000014000000000 0.000111000000000 0.000087000000000 0.001226000000000 0.000704000000000 0.002175000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1134.000000000000000 179.000000000000000 121.000000000000000 0.100695432310273 0.149404926005425 121.000000000000000 0.221119300461396
275 0.000000000000000 0.000017000000000 0.000159000000000 0.000084000000000 0.001069000000000 0.000609000000000 0.001969000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1163.000000000000000 157.000000000000000 121.000000000000000 0.104264909242810 0.168733357468614 121.000000000000000 0.249725362886627
276 0.000000000000000 0.000016000000000 0.000084000000000 0.000079000000000 0.000994000000000 0.000786000000000 0.001989000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1186.000000000000000 161.000000000000000 121.000000000000000 0.098764835183793 0.142076743187060 121.000000000000000 0.210273574643228
277 0.000000000000000 0.000014000000000 0.000113000000000 0.000090000000000 0.001119000000000 0.000653000000000 0.002020000000000 0.000000000000000 113.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1204.000000000000000 154.000000000000000 121.000000000000000 0.099700583140545 0.115489183389192 121.000000000000000 0.170923985837824
278 0.000000000000000 0.000015000000000 0.000088000000000 0.000083000000000 0.001096000000000 0.000758000000000 0.002070000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1224.000000000000000 165.000000000000000 121.000000000000000 0.113531975395493 0.159479019440782 121.000000000000000 0.236028944980121
279 0.000000000000000 0.000016000000000 0.000106000000000 0.000080000000000 0.000950000000000 0.000689000000000 0.001870000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1236.000000000000000 152.000000000000000 121.000000000000000 0.101278477365169 0.121853029810147 121.000000000000000 0.180342482857587
280 0.000000000000000 0.000026000000000 0.000157000000000 0.000086000000000 0.001250000000000 0.000748000000000 0.002309000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1243.000000000000000 177.000000000000000 121.000000000000000 0.113386397852020 0.206475855003173 121.000000000000000 0.305584280242809
281 0.000000000000000 0.000018000000000 0.000087000000000 0.000081000000000 0.001080000000000 0.000613000000000 0.001908000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1252.000000000000000 162.000000000000000 121.000000000000000 0.103012888489708 0.192649617277085 121.000000000000000 0.285121445786875
282 0.000000000000000 0.000014000000000 0.000084000000000 0.000078000000000 0.001104000000000 0.000752000000000 0.002063000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1267.000000000000000 181.000000000000000 121.000000000000000 0.119108801565499 0.146565581346541 121.000000000000000 0.216917068822130
283 0.000000000000000 0.000017000000000 0.000169000000000 0.000087000000000 0.001262000000000 0.000745000000000 0.002313000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1272.000000000000000 162.000000000000000 121.000000000000000 0.113261864886997 0.130198323725397 121.000000000000000 0.192693514913252
284 0.000000000000000 0.000013000000000 0.000147000000000 0.000081000000000 0.001101000000000 0.000691000000000 0.002065000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1269.000000000000000 173.000000000000000 121.000000000000000 0.115292532476552 0.143763553384774 121.000000000000000 0.212770056216498
285 0.000000000000000 0.000017000000000 0.000089000000000 0.000077000000000 0.001052000000000 0.000711000000000 0.001978000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1264.000000000000000 159.000000000000000 121.000000000000000 0.101382577834875 0.107355247718934 121.000000000000000 0.158885781611837
286 0.000000000000000 0.000016000000000 0.000118000000000 0.000092000000000 0.001316000000000 0.000678000000000 0.002256000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1260.000000000000000 177.000000000000000 121.000000000000000 0.124156248079858 0.184101498216806 121.000000000000000 0.272470221836003
287 0.000000000000000 0.000018000000000 0.000151000000000 0.000083000000000 0.001038000000000 0.000700000000000 0.002018000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1259.000000000000000 160.000000000000000 121.000000000000000 0.124874729225317 0.136317718156052 121.000000000000000 0.201750219857265
288 0.000000000000000 0.000019000000000 0.000118000000000 0.000088000000000 0.001213000000000 0.000732000000000 0.002204000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1256.000000000000000 175.000000000000000 121.000000000000000 0.137395115258524 0.272990416298521 121.000000000000000 0.404025816597931
289 0.000000000000000 0.000033000000000 0.000166000000000 0.000098000000000 0.001332000000000 0.000676000000000 0.002341000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1256.000000000000000 175.000000000000000 121.000000000000000 0.125440559596022 0.192594450198696 121.000000000000000 0.285039792378329
290 0.000000000000000 0.000011000000000 0.000147000000000 0.000080000000000 0.001052000000000 0.000650000000000 0.001970000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1254.000000000000000 161.000000000000000 121.000000000000000 0.135853494715468 0.174263471494220 121.000000000000000 0.257909935633510
291 0.000000000000000 0.000016000000000 0.000118000000000 0.000078000000000 0.001158000000000 0.000816000000000 0.002217000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1253.000000000000000 167.000000000000000 121.000000000000000 0.122720689566236 0.207186929273245 121.000000000000000 0.306636651409222
292 0.000000000000000 0.000015000000000 0.000117000000000 0.000091000000000 0.001177000000000 0.000833000000000 0.002267000000000 0.000000000000000 118.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1249.000000000000000 162.000000000000000 121.000000000000000 0.131197397185608 0.147375515585527 121.000000000000000 0.218115764109708
293 0.000000000000000 0.000011000000000 0.000150000000000 0.000081000000000 0.001038000000000 0.000708000000000 0.002171000000000 1.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1238.000000000000000 166.000000000000000 121.000000000000000 0.137378655081558 0.217187593785681 121.000000000000000 0.321437652163372
294 0.000000000000000 0.000014000000000 0.000152000000000 0.000092000000000 0.001138000000000 0.000700000000000 0.002154000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1068.000000000000000 186.000000000000000 121.000000000000000 0.145478356636836 0.211190875078835 121.000000000000000 0.312562500325826
295 0.000000000000000 0.000014000000000 0.000151000000000 0.000087000000000 0.001380000000000 0.000695000000000 0.002361000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1066.000000000000000 208.000000000000000 121.000000000000000 0.145910261187635 0.270725437624101 121.000000000000000 0.400673629542342
296 0.000000000000000 0.000010000000000 0.000106000000000 0.000080000000000 0.001226000000000 0.000670000000000 0.002123000000000 0.000000000000000 120.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1063.000000000000000 202.000000000000000 121.000000000000000 0.142580547861068 0.222707058106822 121.000000000000000 0.329606455264036
297 0.000000000000000 0.000016000000000 0.000091000000000 0.000082000000000 0.001339000000000 0.000780000000000 0.002343000000000 0.000000000000000 116.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1056.000000000000000 206.000000000000000 121.000000000000000 0.127026409458953 0.138078812827686 121.000000000000000 0.204356634060426
298 0.000000000000000 0.000015000000000 0.000112000000000 0.000084000000000 0.001327000000000 0.000760000000000 0.002333000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1046.000000000000000 203.000000000000000 121.000000000000000 0.143390011522010 0.189299220864586 121.000000000000000 0.280162852382094
299 0.000000000000000 0.000012000000000 0.000153000000000 0.000080000000000 0.001259000000000 0.000815000000000 0.002350000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1041.000000000000000 190.000000000000000 121.000000000000000 0.146470888134392 0.170045702350971 121.000000000000000 0.251667632374036
300 0.000000000000000 0.000016000000000 0.000109000000000 0.000082000000000 0.001303000000000 0.000864000000000 0.002410000000000 0.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1035.000000000000000 192.000000000000000 121.000000000000000 0.145288024491270 0.152085543829851 120.000000000000000 0.225086604806994
301 0.000000000000000 0.000015000000000 0.000130000000000 0.000082000000000 0.001210000000000 0.000564000000000 0.002031000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1033.000000000000000 195.000000000000000 121.000000000000000 0.150132403510209 0.284875350032339 121.000000000000000 0.421615517545908
302 0.000000000000000 0.000017000000000 0.000142000000000 0.000087000000000 0.001179000000000 0.000822000000000 0.002279000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1032.000000000000000 177.000000000000000 121.000000000000000 0.150560599418679 0.158144787063228 121.000000000000000 0.234054276956730
303 0.000000000000000 0.000014000000000 0.000166000000000 0.000090000000000 0.001346000000000 0.000786000000000 0.002438000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1025.000000000000000 195.000000000000000 121.000000000000000 0.143981650642650 0.191284534828251 121.000000000000000 0.283101111496310
304 0.000000000000000 0.000011000000000 0.000086000000000 0.000079000000000 0.001157000000000 0.000713000000000 0.002195000000000 1.000000000000000 121.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 1022.000000000000000 190.000000000000000 121.000000000000000 0.154353905338326 0.194713119296172 121.000000000000000 0.288175424788969
305 0.000000000000000 0.000015000000000 0.000156000000000 0.000097000000000 0.001181000000000 0.000713000000000 0.002220000000000 0.000000000000000 115.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 838.000000000000000 198.000000000000000 121.000000000000000 0.139070972107019 0.141121163279108 121.000000000000000 0.208859322949524
306 0.000000000000000 0.000015000000000 0.000151000000000 0.000089000000000 0.001225000000000 0.000800000000000 0.002315000000000 0.000000000000000 117.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 835.000000000000000 186.000000000000000 121.000000000000000 0.138520459895391 0.192364562878898 121.000000000000000 0.284699557206785
307 0.000000000000000 0.000016000000000 0.000166000000000 0.000083000000000 0.001375000000000 0.000800000000000 0.002476000000000 0.000000000000000 119.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 -1.000000000000000 831.000000000000000
svo/test/data/.gitignore
+2
View File
@@ -0,0 +1,2 @@
*
!.gitignore
svo/test/results/.gitignore
+2
View File
@@ -0,0 +1,2 @@
*
!.gitignore
svo/test/test_depth_filter.cpp
+200
View File
@@ -0,0 +1,200 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <list>
#include <vector>
#include <string>
#include <boost/bind.hpp>
#include <vikit/pinhole_camera.h>
#include <vikit/abstract_camera.h>
#include <vikit/atan_camera.h>
#include <vikit/math_utils.h>
#include <vikit/file_reader.h>
#include <vikit/blender_utils.h>
#include <vikit/timer.h>
#include <iostream>
#include <svo/feature_detection.h>
#include <svo/depth_filter.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.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(const svo::Seed& seed, const Eigen::Vector3d& xyz_world);
void testReconstruction(std::string dataset_dir, std::string experiment_name);
std::list<ConvergedSeed> results_;
std::vector<double> errors_;
double error_sum_;
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, 217.083701215, 217.083701215, 376, 240)),
depth_filter_(NULL)
{
errors_.reserve(1000);
}
DepthFilterTest::~DepthFilterTest()
{
delete cam_;
}
void DepthFilterTest::depthFilterCb(
const svo::Seed& seed,
const Eigen::Vector3d& xyz_world)
{
double depth = (frame_ref_->pos() - xyz_world).norm();
double error = fabs(depth - depth_ref_.at<float>(seed.ftr->px[1], seed.ftr->px[0]));
results_.push_back(ConvergedSeed(seed.ftr->px[0], seed.ftr->px[1], depth, error));
errors_.push_back(error);
delete seed.ftr;
++n_converged_seeds_;
}
void DepthFilterTest::testReconstruction(
std::string dataset_dir,
std::string experiment_name)
{
vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
std::vector<vk::blender_utils::file_format::ImageNameAndPose> 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<vk::blender_utils::file_format::ImageNameAndPose>::iterator it = sequence.begin();
vk::Timer t;
std::list<size_t> n_converged_per_iteration;
svo::feature_detection::DetectorPtr feature_detector(new svo::feature_detection::FastDetector());
svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2);
depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb);
for(int i=0; it != sequence.end() && i<50; ++it, ++i)
{
std::string img_name(dataset_dir+"/img/" + (*it).image_name_ + "_0.png");
if(i==0)
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 = Sophus::SE3(it->q_.toRotationMatrix(), it->t_);
if(i == 0)
{
// create reference frame and load ground truth depthmap
frame_ref_.reset(new svo::Frame(cam_, img));
frame_ref_->T_f_w_ = T_w_f.inverse();
depth_filter_->addKeyframe(frame_ref_, 3, 1);
vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/" + (*it).image_name_ + "_0.depth", *cam_, depth_ref_);
continue;
}
n_converged_seeds_=0;
frame_cur_ = svo::FramePtr(new svo::Frame(cam_, img));
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: 349)\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: 1.510507)\n", sum_error*100/errors_.size());
std::vector<double>::iterator it = errors_.begin()+0.5*errors_.size();
std::nth_element(errors_.begin(), it, errors_.end());
printf("50-percentile: \t %f cm (ref: 0.809308)\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: 1.580725)\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: 2.557531)\n", *it*100);
}
// trace error
std::string trace_name(std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + ".txt");
std::ofstream ofs(trace_name.c_str());
for(std::list<ConvergedSeed>::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 = std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + "_convergence.txt";
ofs.open(trace_name.c_str());
for(std::list<size_t>::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 = std::string(TEST_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<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
{
cv::Vec3b c = frame_ref_->img_pyr_[0].at<cv::Vec3b>(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(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
std::string experiment_name("synth_fast_2d");
test.testReconstruction(dataset_dir, experiment_name);
return 0;
}
svo/test/test_feature_alignment.cpp
+122
View File
@@ -0,0 +1,122 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <string.h>
#include <svo/global.h>
#include <svo/feature_alignment.h>
#include <vikit/timer.h>
#include <vikit/aligned_mem.h>
#if __ARM_NEON__
#include <arm_neon.h>
#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(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_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<uint8_t>(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<float>();
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.637000ms, Odroid-U2: ?, 0.008788px) \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.147000ms, Odroid-U2: ?, 0.009632px)\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.548000ms, 0.023032px)\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;
}
svo/test/test_feature_detection.cpp
+80
View File
@@ -0,0 +1,80 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <string.h>
#include <svo/global.h>
#include <svo/config.h>
#include <svo/feature_detection.h>
#include <svo/depth_filter.h>
#include <vikit/timer.h>
#include <vikit/vision.h>
#include <vikit/abstract_camera.h>
#include <vikit/atan_camera.h>
namespace {
using namespace Eigen;
using namespace std;
void testCornerDetector()
{
std::string img_name(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_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());
svo::ImgPyr img_pyr;
svo::frame_utils::createImgPyramid(
img, max(svo::Config::nPyrLevels(), svo::Config::kltMaxLevel()+1), img_pyr);
svo::Features fts;
// Corner detection
vk::Timer t;
svo::feature_detection::Corners corners;
svo::feature_detection::FastDetector fast_detector;
for(int i=0; i<100; ++i)
{
fast_detector.detect(img_pyr, fts, svo::Config::gridSize(), svo::Config::nPyrLevels(),
svo::Config::triangMinCornerScore(), &corners);
}
printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, corners.size());
if(false)
{
cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
cv::cvtColor(img, img_rgb, CV_GRAY2RGB);
for(svo::feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
{
// we must check again if the score is above the threshold because the Corners
// vector is initialized with dummy corners
if(it->score > svo::Config::triangMinCornerScore())
{
cv::circle(img_rgb, cv::Point2f(it->x, it->y), 4*(it->level+1), cv::Scalar(0,255,0), 1);
}
}
cv::imshow("ref_img", img_rgb);
cv::waitKey(0);
}
}
} // namespace
int main(int argc, char **argv)
{
testCornerDetector();
return 0;
}
svo/test/test_matcher.cpp
+195
View File
@@ -0,0 +1,195 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <vikit/pinhole_camera.h>
#include <vikit/math_utils.h>
#include <vikit/blender_utils.h>
#include <svo/matcher.h>
#include <svo/frame.h>
#include <svo/config.h>
#include <svo/feature.h>
namespace {
/// Matcher Test-Fixture
class MatcherTest {
public:
MatcherTest()
{
cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
// load images
std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
std::string img_name(dataset_dir+"/img/frame_000071_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_000097_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(1.0054399967193604, 1.75655996799469, 4.610799789428711);
Eigen::Vector3d t_w_cur(-0.046640001237392426, 2.163439989089966, 4.6279802322387695);
Eigen::Quaterniond q_w_ref(0.015279541723430157, 0.7095794081687927, 0.7044594287872314, 0.0006494877743534744);
Eigen::Quaterniond q_w_cur(0.004395846277475357, -0.7776520252227783, -0.6286740899085999, -0.0026246386114507914);
frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref.toRotationMatrix(), t_w_ref).inverse();
frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur.toRotationMatrix(), t_w_cur).inverse();
// load ground-truth depth
vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000071_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()
{
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<double> errors; errors.reserve(depthmap.cols*depthmap.rows);
for(int y=4; y<depthmap.rows-4; ++y)
{
for(int x=4; x<depthmap.cols-4; ++x)
{
svo::Feature ftr(frame_ref_, Eigen::Vector2d(x,y), 0);
double depth_gt = depth_ref_.at<float>(y, x);
double depth_estimate, h_inv;
int res = svo::matcher::findEpipolarMatchDirect(
*frame_ref_, *frame_cur_, ftr,
depth_gt, fmax(depth_gt-depth_range,0.0), depth_gt+depth_range, false, depth_estimate, h_inv);
if(res)
{
depthmap.at<float>(y,x) = depth_estimate;
error_map.at<float>(y,x) = fabsf(depth_estimate-depth_gt);
++n_converged;
sum_error += error_map.at<float>(y,x);
errors.push_back(error_map.at<float>(y,x));
}
else
mask.at<uint8_t>(y,x) = 0;
}
}
// compute mean, median and variance of error in converged area
printf("n converged: \t %zu (ref: 194472)\n", n_converged);
printf("mean error: \t %f (ref: 1.0848)\n", sum_error*100/n_converged);
std::vector<double>::iterator it = errors.begin()+0.5*errors.size();
std::nth_element(errors.begin(), it, errors.end());
printf("50-percentile: \t %f (ref: 0.1343)\n", *it*100);
it = errors.begin()+0.8*errors.size();
std::nth_element(errors.begin(), it, errors.end());
printf("80-percentile: \t %f (ref: 0.3151)\n", *it*100);
it = errors.begin()+0.95*errors.size();
std::nth_element(errors.begin(), it, errors.end());
printf("95-percentile: \t %f (ref: 1.3753)\n", *it*100);
// save results to file
std::ofstream output_stream;
std::string output_filename(std::string(TEST_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(std::string(TEST_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(std::string(TEST_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::PatchData md(patch_size);
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(patch_size, patch_size, CV_8U, md.patch_with_border);
Eigen::Matrix2d A_cur_ref;
svo::matcher::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::matcher::warp::getBestSearchLevel(A_cur_ref, svo::Config::nPyrLevels()-1);
svo::matcher::warp::warpAffine(
A_cur_ref, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, ref_ftr_->level, level_cur, md);
// copy current patch
Eigen::Vector2i pxi(px_cur[0]/(1<<level_cur)+0.5, px_cur[1]/(1<<level_cur)+0.5);
cv::Mat cur_patch(patch_size, patch_size, CV_8U);
frame_cur_->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<<ref_ftr_->level), ref_ftr_->px[1]/(1<<ref_ftr_->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;
}
svo/test/test_pipeline.cpp
+94
View File
@@ -0,0 +1,94 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <svo/config.h>
#include <svo/frame_handler_mono.h>
#include <svo/map.h>
#include <svo/frame.h>
#include <vector>
#include <string>
#include <vikit/math_utils.h>
#include <vikit/vision.h>
#include <vikit/abstract_camera.h>
#include <vikit/atan_camera.h>
#include <vikit/pinhole_camera.h>
#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <iostream>
namespace svo {
class BenchmarkNode
{
vk::AbstractCamera* cam_;
svo::FrameHandlerMono* vo_;
public:
BenchmarkNode();
~BenchmarkNode();
void runFromFolder();
};
BenchmarkNode::BenchmarkNode()
{
Config::traceName() = "benchmark";
Config::traceDir() = std::string(TEST_TRACE_DIR)+"/..";
cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
vo_ = new svo::FrameHandlerMono(cam_);
vo_->start();
}
BenchmarkNode::~BenchmarkNode()
{
delete vo_;
delete cam_;
}
void BenchmarkNode::runFromFolder()
{
for(int img_id = 1; img_id < 310; ++img_id)
{
// load image
std::stringstream ss;
ss << std::string(TEST_DATA_DIR) << "/flying_room_1_rig_1/img/frame_" << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
if(img_id == 1)
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";
}
}
} // namespace svo
int main(int argc, char** argv)
{
{
svo::BenchmarkNode benchmark;
benchmark.runFromFolder();
}
printf("BenchmarkNode finished.\n");
return 0;
}
svo/test/test_pose_optimizer.cpp
+119
View File
@@ -0,0 +1,119 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <vikit/pinhole_camera.h>
#include <vikit/math_utils.h>
#include <vikit/sample.h>
#include <vikit/blender_utils.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/config.h>
#include <svo/feature_detection.h>
#include <svo/pose_optimizer.h>
namespace {
using namespace svo;
using namespace Eigen;
/// Pose Optimizer Test-Fixture
class PoseOptimizerTest {
public:
PoseOptimizerTest()
{
cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
// load image
std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
std::string img_name(dataset_dir+"/img/frame_000001_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(2.139359951019287, -1.9544399976730347, 2.8595199584960938 );
Eigen::Quaterniond q_w_ref(0.0027939670253545046, 0.8382523059844971, 0.5443645715713501, -0.03150530532002449);
frame_->T_f_w_ = Sophus::SE3(q_w_ref.toRotationMatrix(), t_w_ref).inverse();
// load ground-truth depth
vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000001_0.depth", *cam_, depthmap_);
// detect features
feature_detection::Corners corners;
feature_detection::FastDetector detector;
detector.detect(frame_->img_pyr_, frame_->fts_, Config::gridSize(),
Config::nPyrLevels(), Config::triangMinCornerScore(), &corners);
size_t n_fts = 0;
for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
{
// we must check again if the score is above the threshold because the Corners
// vector is initialized with dummy corners
if(it->score > Config::triangMinCornerScore())
{
Feature* ftr(new Feature(frame_.get(), Vector2d(it->x, it->y), it->level));
Point* point(new Point(frame_->f2w(ftr->f*depthmap_.at<float>(it->y, it->x)), Point::DEPTH));
ftr->point = point;
ftr->frame = frame_.get();
frame_->addFeature(ftr);
++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;
}
svo/test/test_sparse_img_align.cpp
+165
View File
@@ -0,0 +1,165 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <vikit/pinhole_camera.h>
#include <vikit/math_utils.h>
#include <vikit/file_reader.h>
#include <vikit/timer.h>
#include <vikit/blender_utils.h>
#include <iostream>
#include <svo/feature_detection.h>
#include <svo/sparse_img_align.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/config.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, 217.083701215, 217.083701215, 376, 240))
{}
SparseImgAlignTest::~SparseImgAlignTest()
{}
void SparseImgAlignTest::testSequence(
const std::string& dataset_dir,
const std::string& experiment_name,
svo::feature_detection::AbstractDetector* feature_detector)
{
vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
std::vector<vk::blender_utils::file_format::ImageNameAndPose> 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<vk::blender_utils::file_format::ImageNameAndPose>::iterator iter = sequence.begin();
std::list<double> translation_error;
Sophus::SE3 T_prev_w, T_prevgt_w;
std::string trace_name(std::string(TEST_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 = Sophus::SE3(iter->q_.toRotationMatrix(), 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));
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
svo::feature_detection::Corners corners;
feature_detector->detect(
frame_ref_->img_pyr_, frame_ref_->fts_, svo::Config::gridSize(),
svo::Config::nPyrLevels(), svo::Config::triangMinCornerScore(), &corners);
for(svo::feature_detection::Corners::iterator corner=corners.begin(); corner!=corners.end(); ++corner)
{
svo::Feature* ftr = new svo::Feature(frame_ref_.get(), Eigen::Vector2d(corner->x, corner->y), corner->level);
Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(corner->y, corner->x);
Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur;
svo::Point* pt = new svo::Point(pt_pos_w, svo::Point::DEPTH);
ftr->point = pt;
frame_ref_->addFeature(ftr);
}
printf("Added %zu 3d pts to the reference frame.\n", corners.size());
T_prev_w = frame_ref_->T_f_w_;
T_prevgt_w = T_gt_w;
continue;
}
frame_cur_.reset(new svo::Frame(cam_, img));
//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 = std::string(TEST_TRACE_DIR) + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
ofs.open(trace_name.c_str());
for(std::list<double>::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(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
svo::feature_detection::FastDetector detector;
svo::Config::triangMinCornerScore() = 20;
svo::Config::kltMinLevel() = 0;
SparseImgAlignTest test;
test.testSequence(dataset_dir, experiment_name, &detector);
return 0;
}
svo_analysis/benchmark.py
Executable
+67
View File
@@ -0,0 +1,67 @@
#!/usr/bin/python
"""
Created on Sat Aug 10 18:21:47 2013
@author: Christian Forster
"""
import os
import yaml
import time
class RosNode:
def __init__(self, package, executable):
self.package = package
self.executable = executable
def run(self, parameter_dictionary):
parameter_string = ''
for key in parameter_dictionary.keys():
parameter_string = parameter_string + ' _' + key + ':=' + str(parameter_dictionary[key])
os.system('rosrun ' + self.package + ' ' + self.executable + ' ' + parameter_string)
if __name__=="__main__":
# config:
home_dir = os.getenv("HOME")
dataset_dir = home_dir+'/Datasets/SlamBenchmark/'
algo_dir = home_dir+'/workspace/rpg_slam/svo_ros'
trace_dir = home_dir+'/workspace/rpg_slam/svo_analysis/results'
platform = 'i7'
# load parameters
experiment_name = 'asl2_fast'
experiment_file = 'experiments/'+experiment_name+'.yaml'
experiment_file_stream = open(experiment_file,'r')
experiment_dict = yaml.load(experiment_file_stream)
dataset_param_file = dataset_dir+experiment_dict['dataset']+'/dataset_params.yaml'
dataset_param_file_stream = open(dataset_param_file,'r')
dataset_param_dict = yaml.load(dataset_param_file_stream)
algo_param_file = algo_dir+'/param/'+experiment_dict['param_settings']+'.param'
algo_param_file_stream = open(algo_param_file,'r')
algo_param_dict = yaml.load(algo_param_file_stream)
params = dict(dataset_param_dict.items() + algo_param_dict.items() + experiment_dict.items())
localtime = time.localtime()
params['trace_name'] = time.strftime("%Y%m%d_%H%M", localtime)+'_nslam_'+platform+'_'+experiment_name
params['trace_dir'] = trace_dir+'/'+params['trace_name']
params['run_id'] = 0
# create folder for experiment results
if not os.path.exists(params['trace_dir']):
os.makedirs(params['trace_dir'])
# dump experiment params to file:
params_dump_file = params['trace_dir']+'/params.yaml'
with open(params_dump_file,'w') as outfile:
outfile.write( yaml.dump(params, default_flow_style=False) )
# start ros node
print 'starting ros node'
node = RosNode('svo_ros','benchmark')
node.run(params)
print 'ros node finished processing'
svo_analysis/evaluate.py
Executable
+47
View File
@@ -0,0 +1,47 @@
#!/usr/bin/python
"""
Created on Sat Aug 10 18:21:47 2013
@author: Christian Forster
"""
import argparse
import csv
import numpy as np
from matplotlib import rc
import scripts.analyse_logs as analyse_logs
import scripts.analyse_timing as analyse_timing
if __name__=="__main__":
# parse command line
parser = argparse.ArgumentParser()
parser.add_argument('--trace_name', help='svo tracefile with timings and logs', default='20131121_0930_px7')
args = parser.parse_args()
# config
results_dir = 'results/'+args.trace_name
trace_file = results_dir+'/'+args.trace_name+'.csv'
# tracefile
data = csv.reader(open(trace_file))
fields = data.next()
D = dict()
for field in fields:
D[field] = list()
# fill dictionary with column values
for row in data:
for (field, value) in zip(fields, row):
D[field].append(float(value))
# change dictionary values from list to numpy array for easier manipulation
for field, value in D.items():
D[field] = np.array(D[field])
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
analyse_logs.analyse_logs(D, results_dir+'/'+args.trace_name)
analyse_timing.analyse_timing(D, results_dir+'/'+args.trace_name)
svo_analysis/experiments/asl2_acc.yaml
+2
View File
@@ -0,0 +1,2 @@
dataset: asl_vicon_d2
param_settings: vo_accurate
svo_analysis/experiments/asl2_fast.yaml
+2
View File
@@ -0,0 +1,2 @@
dataset: asl_vicon_d2
param_settings: vo_fast
svo_analysis/results/.gitignore
+2
View File
@@ -0,0 +1,2 @@
*
!.gitignore
svo_analysis/scripts/__init__.py
+2
View File
@@ -0,0 +1,2 @@
# -*- coding: utf-8 -*-
svo_analysis/scripts/analyse_dataset.py
+42
View File
@@ -0,0 +1,42 @@
# -*- coding: utf-8 -*-
import associate
import numpy as np
import matplotlib.pyplot as plt
import yaml
import transformations
def loadDataset(filename):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
D = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
return D
dataset_dir = '/home/cforster/Datasets/SlamBenchmark/px4_r2'
trajectory_data = dataset_dir+'/groundtruth.txt'
stepsize = 10
# load dataset
data = loadDataset(trajectory_data)
n = data.shape[0]
steps = np.arange(0,n,stepsize)
# compute trajectory length
last_pos = data[0,1:4]
trajectory_length = 0
for i in steps[1:]:
new_pos = data[i,1:4]
trajectory_length += np.linalg.norm(new_pos-last_pos)
last_pos = new_pos
print 'trajectory lenght = ' + str(trajectory_length) + 'm'
print 'height mean = ' + str(np.mean(data[:,3])) + 'm'
print 'height median = ' + str(np.median(data[:,3])) + 'm'
print 'height std = ' + str(np.std(data[:,3])) + 'm'
print 'duration = ' + str(data[-1,0]-data[0,0]) + 's'
print 'speed = ' + str(trajectory_length/(data[-1,0]-data[0,0])) + 'm/s'
svo_analysis/scripts/analyse_logs.py
Executable
+132
View File
@@ -0,0 +1,132 @@
#!/usr/bin/python
import argparse
import csv
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rc
def analyse_logs(D, trace_name):
# identify measurements which result from normal frames and which from keyframes
is_kf = np.argwhere( (D['dropout'] == 1) & (D['repr_n_mps'] >= 0))
is_frame = np.argwhere(D['repr_n_mps'] >= 0)
is_nokf = np.argwhere( (D['dropout'] == 0) & (D['repr_n_mps'] >= 0))
n_frames = len(is_frame)
n_kfs = len(is_kf)
n_nokf = len(is_nokf)
fps = 25.0
# plot number of reprojected points
mean_n_reproj_points = np.mean(D['repr_n_mps'][is_frame]);
mean_n_reproj_matches = np.mean(D['repr_n_new_references'][is_frame]);
plt.figure()
plt.plot(np.arange(n_frames), D['repr_n_mps'][is_frame], 'r-', label='reprojected points, avg = %(i).2f'%{'i':mean_n_reproj_points})
plt.plot(np.arange(n_frames), D['repr_n_new_references'][is_frame], 'b-', label='reprojected matches, avg = %(i).2f'%{'i':mean_n_reproj_matches})
plt.legend()
plt.tight_layout()
plt.title('Reprojected points asdf')
plt.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
# plot number of triangulations
# plt.figure(3)
# plt.plot(np.arange(n_kfs), D['triang_n_points'][is_kf], 'g-', label='triangulated points, avg %0.2f'%np.mean(D['triang_n_points'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_refs'][is_kf], 'b-', label='new references, avg %0.2f'%np.mean(D['triang_n_refs'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_fails'][is_kf], 'r-', label='failures, avg %0.2f'%np.mean(D['triang_n_fails'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_add_refs'][is_kf], 'm-', label='additional refs, avg %0.2f'%np.mean(D['triang_n_add_refs'][is_kf]))
# plt.legend()
# plt.title('Triangulation')
# plt.savefig(trace_name+'_img_triangulation.png', bbox_inches="tight")
# plot number of klt tracked and matched points
fig = plt.figure(figsize=(8,2))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features')
ax.plot(np.arange(n_frames)/fps, D['repr_n_new_references'][is_frame], 'b-', label='number of successful matches')
fig.tight_layout()
fig.savefig(trace_name+'number_of_features.pdf', bbox_inches="tight")
# plot median error before and after pose-optimzation and bundle adjustment
init_error_avg = np.mean(D['sfba_error_init'][is_frame])
opt1_avg = np.mean(D['sfba_error_final'][is_frame])
fig = plt.figure(figsize=(8,2))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='error [px]')
t_max = n_frames
ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_init'][is_frame][0:t_max], 'r-', label='Initial error')
ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_final'][is_frame][0:t_max], 'b-', label='Final error')
ax.legend(ncol=2)
ax.set_xlim([0, t_max/fps+4])
#ax.set_ylim([0, 0.8])
fig.tight_layout()
fig.savefig(trace_name+'reprojection_error.pdf', bbox_inches="tight")
print 'average reprojection error improvement: ' + str(init_error_avg - opt1_avg)
# plot number of references
plt.figure()
plt.fill(np.arange(n_frames+1), np.append(D['repr_n_new_references'][is_frame]+D['klt_n_matches'][is_frame],0), 'g-', label='Reprojection')
plt.fill(np.arange(n_frames+1), np.append(D['klt_n_tracked'][is_frame],0), 'b-', label='KLT Matches')
plt.plot(np.arange(n_frames), D['sfba_n_edges_final'][is_frame], 'r-', label='References after Optimization', linewidth=2.0)
plt.legend()
plt.tight_layout()
plt.title('Pose Optimization')
plt.savefig(trace_name+'_img_n_references.pdf', bbox_inches="tight")
# plot number of seeds
plt.figure()
has_seed = np.argwhere(D['seeds_n_tot'] >= 0)
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_tot'][has_seed],0), label='nSeeds', edgecolor='none')
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed]+D['seeds_n_failed_matches'][has_seed],0), label='nFailedMatches', edgecolor='none')
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed],0), label='nUpdates', edgecolor='none')
plt.legend()
plt.tight_layout()
plt.title('Seeds')
plt.savefig(trace_name+'_seeds.pdf', bbox_inches="tight")
plt.figure()
plt.plot(D['n_candidates'])
plt.title('Candidates')
plt.show()
# plt.figure()
# plt.plot())
if __name__=="__main__":
dataset='20131003_1937_nslam_i7_asl2_fast'
n_align_frames = 10
save = True
results_dir = '../results/'+dataset
logfile = results_dir+'/'+dataset+'_0.csv'
# parse command line
parser = argparse.ArgumentParser(description='''
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
''')
parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=logfile)
parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir+'/')
args = parser.parse_args()
# tracefile
data = csv.reader(open(args.trace_file))
fields = data.next()
D = dict()
for field in fields:
D[field] = list()
# fill dictionary with column values
for row in data:
for (field, value) in zip(fields, row):
D[field].append(float(value))
# change dictionary values from list to numpy array for easier manipulation
for field, value in D.items():
D[field] = np.array(D[field])
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
analyse_logs(D, args.trace_name)
svo_analysis/scripts/analyse_timing.py
Executable
+97
View File
@@ -0,0 +1,97 @@
#!/usr/bin/python
import argparse
import csv
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rc
def analyse_timing(D, trace_name):
# identify measurements which result from normal frames and which from keyframes
is_frame = np.argwhere(D['repr_n_mps'] >= 0)
n_frames = len(is_frame)
# plot total time for frame processing
avg_time = np.mean(D['t_tot_time'][is_frame])*1000;
fig = plt.figure(figsize=(8, 3))
ax = fig.add_subplot(111, ylabel='processing time [ms]', xlabel='time [s]')
ax.plot(np.arange(n_frames), D['t_tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
ax.plot(np.arange(n_frames), np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
ax.legend()
fig.tight_layout()
fig.savefig(trace_name+'_img_tot_processing_time.pdf', bbox_inches="tight")
fig = plt.figure(figsize=(6,2))
ax = fig.add_subplot(111, xlabel='Processing time [ms]')
ax.boxplot([
D['t_tot_time'][is_frame]*1000,
# D['t_local_ba'][is_kf]*1000,
D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000,
D['t_reproject'][is_frame]*1000,
D['t_sparse_img_align'][is_frame]*1000,
D['t_pyramid_creation'][is_frame]*1000
], 0,'', vert=0) #
boxplot_labels = [
r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['t_tot_time'][is_frame]*1000),
# 'Local BA (KF only): %.2fms ' % np.median(D['t_local_ba'][is_kf]*1000),
'Refinement: %2.2fms' % np.median(D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000),
'Feature Alignment: %2.2fms' % np.median(D['t_reproject'][is_frame]*1000),
'Sparse Image Alignment: %2.2fms' % np.median(D['t_sparse_img_align'][is_frame]*1000),
'Pyramid Creation: %2.2fms' % np.median(D['t_pyramid_creation'][is_frame]*1000) ]
ax.set_yticks(np.arange(len(boxplot_labels))+1)
ax.set_yticklabels(boxplot_labels)
fig.tight_layout()
fig.savefig(trace_name+'_timing_boxplot.pdf', bbox_inches="tight")
fig = plt.figure(figsize=(8, 5))
ax = fig.add_subplot(111, ylabel='time [ms]')
ax.plot(D['t_reproject']*1000, 'r', label='reproject')
ax.plot(D['t_reproject_find_pts']*1000, 'g', label='reproject find pts') # ok!
ax.plot(D['t_reproject_best']*1000, 'm', label='reproject best')
ax.plot(D['t_reproject_candidates']*1000, 'b', label='reproject candidates')
ax.legend()
fig.tight_layout()
fig.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
if __name__=="__main__":
# config
dataset='20131003_1937_nslam_i7_asl2_fast'
results_dir = '../results/'+dataset
tracefile = results_dir + '/'+dataset+'_0.csv'
# parse command line
parser = argparse.ArgumentParser(description='''
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
''')
parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=tracefile)
parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir)
args = parser.parse_args()
results_dir = args.trace_name
# tracefile
data = csv.reader(open(args.trace_file))
fields = data.next()
D = dict()
for field in fields:
D[field] = list()
# fill dictionary with column values
for row in data:
for (field, value) in zip(fields, row):
D[field].append(float(value))
# change dictionary values from list to numpy array for easier manipulation
for field, value in D.items():
D[field] = np.array(D[field])
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
svo_analysis/scripts/analyse_trajectory.py
+187
View File
@@ -0,0 +1,187 @@
# -*- coding: utf-8 -*-
import associate
import numpy as np
import matplotlib.pyplot as plt
import yaml
import transformations
from matplotlib import rc
def alignSim3(model, data, n):
# select the first n datapoints and remove its mean so their center is zero
M = model[0:n,:]
D = data[0:n,:]
# substract mean
mu_M = M.mean(0)
mu_D = D.mean(0)
M_zerocentered = M - mu_M
D_zerocentered = D - mu_D
# correlation
C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
D_svd = np.diag(D_svd)
V_svd = np.transpose(V_svd)
S = np.eye(3)
if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
S[2,2] = -1
R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
t = mu_M-s*np.dot(R,mu_D)
return s, R, t
def getRigidBodyTrafo(quat,trans):
T = transformations.quaternion_matrix(quat)
T[0:3,3] = trans
return T
if __name__ == '__main__':
# config
dataset='20131003_1937_nslam_i7_asl2_fast'
n_align_frames = 100
save = True
results_dir = '../results/'+dataset
# plot options
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
# load dataset parameters
params_stream = open(results_dir+'/params.yaml')
params = yaml.load(params_stream)
# set trajectory groundtruth and estimate file
traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
traj_estimate = results_dir+'/traj_estimate.txt'
traj_estimate_rotated_filename = results_dir+'/traj_estimate_rotated.txt'
translation_error_filename = results_dir+'/translation_error.txt'
orientation_error_filename = results_dir+'/orientation_error.txt'
scale_drift_filename = results_dir+'/scale_drift.txt'
# load hand-eye calib: trafo between (m)arker and (c)amera.
dataset_param_file = params['dataset_directory']+'/dataset_params.yaml'
dataset_param_file_stream = open(dataset_param_file,'r')
P = yaml.load(dataset_param_file_stream)
T_cm_quat = np.array([P['calib_Tcm_qx'], P['calib_Tcm_qy'], P['calib_Tcm_qz'], P['calib_Tcm_qw']])
T_cm_tran = np.array([P['calib_Tcm_tx'], P['calib_Tcm_ty'], P['calib_Tcm_tz']])
T_cm = getRigidBodyTrafo(T_cm_quat, T_cm_tran)
T_mc = transformations.inverse_matrix(T_cm)
# load data
data_gt = associate.read_file_list(traj_groundtruth)
data_es = associate.read_file_list(traj_estimate)
# find matches
matches = associate.associate(data_gt, data_es, -P['cam_delay'], 0.02)
p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches])
q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches])
p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches])
t_gt = np.array([float(a) for a,b in matches])
t_es = np.array([float(b) for a,b in matches])
start_time = min(t_es[0], t_gt[0])
t_es -= start_time
t_gt -= start_time
# align Sim3 to get scale
if(n_align_frames > np.shape(matches)[0]):
raise NameError('n_matches_for_scale is too large')
scale,rot,trans = alignSim3(p_gt, p_es, n_align_frames)
print 'scale = '+str(scale)
# get trafo between (v)ision and (o)ptitrack frame
T_om = getRigidBodyTrafo(q_gt[0,:], p_gt[0,:])
T_vc = getRigidBodyTrafo(q_es[0,:], scale*p_es[0,:])
T_cv = transformations.inverse_matrix(T_vc)
T_ov = np.dot(T_om, np.dot(T_mc, T_cv))
print 'T_ov = ' + str(T_ov)
p_es_aligned = np.zeros(np.shape(p_es))
q_es_aligned = np.zeros(np.shape(q_es))
rpy_es_aligned = np.zeros(np.shape(p_es))
rpy_gt = np.zeros(np.shape(p_es))
# rotate trajectory
for i in range(np.shape(matches)[0]):
T_vc = getRigidBodyTrafo(q_es[i,:],p_es[i,:])
T_vc[0:3,3] *= scale
T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
p_es_aligned[i,:] = T_om[0:3,3]
q_es_aligned[i,:] = transformations.quaternion_from_matrix(T_om)
rpy_es_aligned[i,:] = transformations.euler_from_quaternion(q_es_aligned[i,:], 'rzyx')
rpy_gt[i,:] = transformations.euler_from_quaternion(q_gt[i,:], 'rzyx')
if save:
f = open(traj_estimate_rotated_filename,'w')
for i in range(np.shape(p_es_aligned)[0]):
f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (matches[i][1]-P['cam_delay'], p_es_aligned[i,0], p_es_aligned[i,1], p_es_aligned[i,2], q_es_aligned[i,0], q_es_aligned[i,1], q_es_aligned[i,2], q_es_aligned[i,3]))
f.close()
# plot trajectory
fig = plt.figure()
ax = fig.add_subplot(111, title='trajectory', aspect='equal', xlabel='x [m]', ylabel='Y [m]')
ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'b-', label='estimate')
ax.plot(p_gt[:,0], p_gt[:,1], 'r-', label='groundtruth')
ax.plot(p_es_aligned[0:n_align_frames,0], p_es_aligned[0:n_align_frames,1], 'g-', linewidth=2, label='aligned')
fig.tight_layout()
fig.savefig(results_dir+'/trajectory.pdf')
# plot position error (drift)
translation_error = (p_gt-p_es_aligned)
fig = plt.figure(figsize=(8, 2.5))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [m]', xlim=[0,t_es[-1]+4])
ax.plot(t_es, translation_error[:,0], 'r-', label='x')
ax.plot(t_es, translation_error[:,1], 'g-', label='y')
ax.plot(t_es, translation_error[:,2], 'b-', label='z')
fig.tight_layout()
if save:
fig.savefig(results_dir+'/translation_error.pdf')
f = open(translation_error_filename,'w')
for i in range(translation_error.shape[0]):
f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], translation_error[i,0], translation_error[i,1], translation_error[i,2]))
f.close()
# plot orientation error (drift)
orientation_error = (rpy_gt - rpy_es_aligned)
fig = plt.figure(figsize=(8, 2.5))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,t_es[-1]+4])
ax.plot(t_es, orientation_error[:,0], 'r--', label='yaw')
ax.plot(t_es, orientation_error[:,1], 'g--', label='pitch')
ax.plot(t_es, orientation_error[:,2], 'b--', label='roll')
ax.legend()
fig.tight_layout()
if save:
fig.savefig(results_dir+'/orientation_error.pdf')
f = open(orientation_error_filename,'w')
for i in range(orientation_error.shape[0]):
f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], orientation_error[i,0], orientation_error[i,1], orientation_error[i,2]))
f.close()
# plot scale drift
motion_gt = np.diff(p_gt, 0)
motion_es = np.diff(p_es_aligned, 0)
dist_gt = np.sqrt(np.sum(np.multiply(motion_gt,motion_gt),1))
dist_es = np.sqrt(np.sum(np.multiply(motion_es,motion_es),1))
fig = plt.figure(figsize=(8,2.5))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]', xlim=[0,t_es[-1]+4])
scale_drift = np.divide(dist_es,dist_gt)*100-100
ax.plot(t_es, scale_drift, 'b-')
fig.tight_layout()
if save:
fig.savefig(results_dir+'/scale_drift.pdf')
f = open(scale_drift_filename,'w')
for i in range(orientation_error.shape[0]):
f.write('%.7f %.5f\n'%(t_es[i], scale_drift[i]))
f.close()
svo_analysis/scripts/associate.py
+67
View File
@@ -0,0 +1,67 @@
#!/usr/bin/python
#
# Requirements:
# sudo apt-get install python-argparse
import argparse
import sys
import os
import numpy
def read_file_list(filename):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
return dict(list)
def associate(first_list, second_list,offset,max_difference):
# first_list = groundtruth
# second_list = estimate
first_keys = first_list.keys()
second_keys = second_list.keys()
potential_matches = [(abs(a - (b + offset)), a, b)
for a in first_keys
for b in second_keys
if abs(a - (b + offset)) < max_difference]
potential_matches.sort()
matches = []
for diff, a, b in potential_matches:
if a in first_keys and b in second_keys:
first_keys.remove(a)
second_keys.remove(b)
matches.append((a, b))
matches.sort()
return matches
if __name__ == '__main__':
# parse command line
parser = argparse.ArgumentParser(description='''
This script takes two data files with timestamps and associates them
''')
parser.add_argument('first_file', help='first text file (format: timestamp data)')
parser.add_argument('second_file', help='second text file (format: timestamp data)')
parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
args = parser.parse_args()
first_list = read_file_list(args.first_file)
second_list = read_file_list(args.second_file)
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
if args.first_only:
for a,b in matches:
print("%f %s"%(a," ".join(first_list[a])))
else:
for a,b in matches:
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
svo_analysis/scripts/evaluate_rpe.py
Executable
+355
View File
@@ -0,0 +1,355 @@
#!/usr/bin/python
import yaml
import argparse
import random
import numpy
import sys
import transformations
import matplotlib
import matplotlib.pyplot as plt
import matplotlib.pylab as pylab
from matplotlib import rc
def transform44(l):
t = l[1:4]
q = l[4:8]
return numpy.dot(transformations.translation_matrix(t),transformations.quaternion_matrix(q))
def read_trajectory(filename, matrix=True):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
list = [[float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
list_ok = []
for i,l in enumerate(list):
if l[4:8]==[0,0,0,0]:
continue
isnan = False
for v in l:
if numpy.isnan(v):
isnan = True
break
if isnan:
sys.stderr.write("Warning: line %d of file '%s' has NaNs, skipping line\n"%(i,filename))
continue
list_ok.append(l)
if matrix :
traj = dict([(l[0],transform44(l[0:])) for l in list_ok])
else:
traj = dict([(l[0],l[1:8]) for l in list_ok])
return traj
def find_closest_index(L,t):
beginning = 0
difference = abs(L[0] - t)
best = 0
end = len(L)
while beginning < end:
middle = int((end+beginning)/2)
if abs(L[middle] - t) < difference:
difference = abs(L[middle] - t)
best = middle
if t == L[middle]:
return middle
elif L[middle] > t:
end = middle
else:
beginning = middle + 1
return best
def ominus(a,b):
return numpy.dot(numpy.linalg.inv(a),b)
def scale(a,scalar):
return numpy.array(
[[a[0,0], a[0,1], a[0,2], a[0,3]*scalar],
[a[1,0], a[1,1], a[1,2], a[1,3]*scalar],
[a[2,0], a[2,1], a[2,2], a[2,3]*scalar],
[a[3,0], a[3,1], a[3,2], a[3,3]]]
)
def compute_distance(transform):
return numpy.linalg.norm(transform[0:3,3])
def compute_angle(transform):
# an invitation to 3-d vision, p 27
return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) ))
def distances_along_trajectory(traj):
keys = traj.keys()
keys.sort()
motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
distances = [0]
sum = 0
for t in motion:
sum += compute_distance(t)
distances.append(sum)
return distances
def rotations_along_trajectory(traj,scale):
keys = traj.keys()
keys.sort()
motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
distances = [0]
sum = 0
for t in motion:
sum += compute_angle(t)*scale
distances.append(sum)
return distances
def interpolate_transformation(alpha,a,b):
qa,ta = transformations.quaternion_from_matrix(a),transformations.translation_from_matrix(a)
qb,tb = transformations.quaternion_from_matrix(b),transformations.translation_from_matrix(b)
qr = transformations.quaternion_slerp(qa,qb,alpha)
tr = (1-alpha) * ta + (alpha) * tb
r = numpy.dot(transformations.translation_matrix(tr),transformations.quaternion_matrix(qr))
return r
def interpolate_and_resample(traj_gt,stamps_est,param_offset,gt_max_time_difference):
traj_new = {}
stamps_gt = list(traj_gt.keys())
stamps_gt.sort()
for t_est in stamps_est:
t_new = t_est + param_offset
i_nearest = find_closest_index(stamps_gt,t_est + param_offset)
if stamps_gt[i_nearest] >= t_new and i_nearest>0:
i_low = i_nearest-1
i_high = i_nearest
else:
i_low = i_nearest
i_high = i_nearest+1
if i_low<0 or i_high>=len(stamps_gt):
continue
t_low = stamps_gt[i_low]
t_high = stamps_gt[i_high]
if(abs( t_new - (t_low + param_offset) ) > gt_max_time_difference or
abs( t_new - (t_high + param_offset) ) > gt_max_time_difference):
continue
alpha = (t_new - (t_low + param_offset)) / (t_high - t_low)
transform = interpolate_transformation(alpha,traj_gt[t_low],traj_gt[t_high])
traj_new[t_new] = transform
return traj_new
def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00,param_interpolation=True):
stamps_est = list(traj_est.keys())
stamps_est.sort()
stamps_gt = list(traj_gt.keys())
stamps_gt.sort()
gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])])
gt_max_time_difference = 2*gt_interval
if param_interpolation:
traj_gt = interpolate_and_resample(traj_gt,stamps_est,param_offset,gt_max_time_difference)
stamps_gt = list(traj_gt.keys())
stamps_gt.sort()
stamps_est_return = []
for t_est in stamps_est:
t_gt = stamps_gt[find_closest_index(stamps_gt,t_est + param_offset)]
t_est_return = stamps_est[find_closest_index(stamps_est,t_gt - param_offset)]
t_gt_return = stamps_gt[find_closest_index(stamps_gt,t_est_return + param_offset)]
if not t_est_return in stamps_est_return:
stamps_est_return.append(t_est_return)
if(len(stamps_est_return)<2):
raise Exception("Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?")
if param_delta_unit=="s":
index_est = list(traj_est.keys())
index_est.sort()
elif param_delta_unit=="m":
index_est = distances_along_trajectory(traj_est)
elif param_delta_unit=="rad":
index_est = rotations_along_trajectory(traj_est,1)
elif param_delta_unit=="deg":
index_est = rotations_along_trajectory(traj_est,180/numpy.pi)
elif param_delta_unit=="f":
index_est = range(len(traj_est))
else:
raise Exception("Unknown unit for delta: '%s'"%param_delta_unit)
if not param_fixed_delta:
if(param_max_pairs==0 or len(traj_est)<numpy.sqrt(param_max_pairs)):
pairs = [(i,j) for i in range(len(traj_est)) for j in range(len(traj_est))]
else:
pairs = [(random.randint(0,len(traj_est)-1),random.randint(0,len(traj_est)-1)) for i in range(param_max_pairs)]
else:
pairs = []
for i in range(len(traj_est)):
j = find_closest_index(index_est,index_est[i] + param_delta)
if j!=len(traj_est)-1:
pairs.append((i,j))
if(param_max_pairs!=0 and len(pairs)>param_max_pairs):
pairs = random.sample(pairs,param_max_pairs)
result = []
for i,j in pairs:
stamp_est_0 = stamps_est[i]
stamp_est_1 = stamps_est[j]
stamp_gt_0 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_0 + param_offset) ]
stamp_gt_1 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_1 + param_offset) ]
if(abs(stamp_gt_0 - (stamp_est_0 + param_offset)) > gt_max_time_difference or
abs(stamp_gt_1 - (stamp_est_1 + param_offset)) > gt_max_time_difference):
continue
error44 = ominus( scale(
ominus( traj_est[stamp_est_1], traj_est[stamp_est_0] ),param_scale),
ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] ) )
trans = compute_distance(error44)
rot = compute_angle(error44)
motion_gt = ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] )
trans_gt = compute_distance(motion_gt)
rot_gt = compute_angle(motion_gt)
if(rot < 0.07):
result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot,trans_gt,rot_gt])
if len(result)<2:
raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!")
return result
def percentile(seq,q):
seq_sorted = list(seq)
seq_sorted.sort()
return seq_sorted[int((len(seq_sorted)-1)*q)]
if __name__ == '__main__':
random.seed(0)
# config -------------------------------------------------------------------------
dataset='20131003_1937_nslam_i7_asl2_fast'
dataset_dir = '../results/'+dataset
#---------------------------------------------------------------------------------
# load dataset parameters
params_stream = open(dataset_dir+'/params.yaml')
params = yaml.load(params_stream)
# set trajectory groundtruth and estimate file
traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
traj_estimate_rotated = dataset_dir+'/traj_estimate_rotated.txt'
parser = argparse.ArgumentParser(description='''
This script computes the relative pose error from the ground truth trajectory and the estimated trajectory.
''')
parser.add_argument('--groundtruth_file', help='ground-truth trajectory file (format: "timestamp tx ty tz qx qy qz qw")', default=traj_groundtruth)
parser.add_argument('--estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")', default=traj_estimate_rotated)
parser.add_argument('--interpolate', help='interpolate and resample the ground truth trajectory', default=True)
parser.add_argument('--max_pairs', help='maximum number of pose comparisons (default: 10000, set to zero to disable downsampling)', default=10000)
parser.add_argument('--fixed_delta', help='only consider pose pairs that have a distance of delta delta_unit (e.g., for evaluating the drift per second/meter/radian)', default=True)
parser.add_argument('--delta', help='delta for evaluation (default: 1.0)',default=1.0)
parser.add_argument('--delta_unit', help='unit of delta (options: \'s\' for seconds, \'m\' for meters, \'rad\' for radians, \'f\' for frames; default: \'s\')',default='s')
parser.add_argument('--offset', help='time offset between ground-truth and estimated trajectory (default: 0.0)',default=0.0)
parser.add_argument('--scale', help='scaling factor for the estimated trajectory (default: 1.0)',default=1.0)
parser.add_argument('--save', help='text file to which the evaluation will be saved (format: stamp_est0 stamp_est1 stamp_gt0 stamp_gt1 trans_error rot_error)')
parser.add_argument('--plot', help='plot the result to a file (requires --fixed_delta, output format: png)', default=True)
parser.add_argument('--plotname', help='plot the result to a file (requires --fixed_delta, output format: png)', default='rpe.png')
parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', default=True)
args = parser.parse_args()
if args.plot and not args.fixed_delta:
sys.exit("The '--plot' option can only be used in combination with '--fixed_delta'")
traj_gt = read_trajectory(args.groundtruth_file)
traj_est = read_trajectory(args.estimated_file)
result = evaluate_trajectory(traj_gt,
traj_est,
int(args.max_pairs),
args.fixed_delta,
float(args.delta),
args.delta_unit,
float(args.offset),
float(args.scale),
args.interpolate)
stamps = numpy.array(result)[:,0]
trans_error = numpy.array(result)[:,4]
rot_error = numpy.array(result)[:,5]
results = dict()
results['trans_rmse'] = float(numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)))
results['trans_mean'] = float(numpy.mean(trans_error))
results['trans_median'] = float(numpy.median(trans_error))
results['trans_std'] = float(numpy.std(trans_error))
results['trans_min'] = float(numpy.min(trans_error))
results['trans_max'] = float(numpy.max(trans_error))
results['rot_rmse'] = float((numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi))
results['rot_mean'] = float((numpy.mean(rot_error) * 180.0 / numpy.pi))
results['rot_median'] = float(numpy.median(rot_error) * 180.0 / numpy.pi)
results['rot_std'] = float((numpy.std(rot_error) * 180.0 / numpy.pi))
results['rot_min'] = float((numpy.min(rot_error) * 180.0 / numpy.pi))
results['rot_max'] = float((numpy.max(rot_error) * 180.0 / numpy.pi))
# dump experiment results to file:
results_file = params['trace_dir']+'/rpe_results.yaml'
with open(results_file,'w') as outfile:
outfile.write( yaml.dump(results, default_flow_style=False) )
if args.save:
f = open(args.save,"w")
f.write("\n".join([" ".join(["%f"%v for v in line]) for line in result]))
f.close()
if args.verbose:
print "compared_pose_pairs %d pairs"%(len(trans_error))
print "translational_error.rmse %f m"%results['trans_rmse']
print "translational_error.mean %f m"%results['trans_mean']
print "translational_error.median %f m"%results['trans_median']
print "translational_error.std %f m"%results['trans_std']
print "translational_error.min %f m"%results['trans_min']
print "translational_error.max %f m"%results['trans_max']
print "rotational_error.rmse %f deg"%results['rot_rmse']
print "rotational_error.mean %f deg"%results['rot_mean']
print "rotational_error.median %f deg"%results['rot_median']
print "rotational_error.std %f deg"%results['rot_std']
print "rotational_error.min %f deg"%results['rot_min']
print "rotational_error.max %f deg"%results['rot_max']
else:
print numpy.mean(trans_error)
if args.plot:
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
#matplotlib.use('Agg') # no window should appear?
fig = plt.figure(figsize=(8, 3))
ax = fig.add_subplot(111)
ax.plot(stamps - stamps[0],trans_error,'-',color="blue")
#ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red")
ax.set_xlabel('time [s]')
ax.set_ylabel('translational error [m]')
plt.tight_layout()
plt.savefig(dataset_dir+'/rpe_trans_error.pdf')
plt.show()
fig = plt.figure(figsize=(8, 3))
ax = fig.add_subplot(111)
ax.plot(stamps - stamps[0],rot_error* 180.0 / numpy.pi,'-',color="blue")
#ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red")
ax.set_xlabel('time [s]')
ax.set_ylabel('rot error [deg]')
plt.tight_layout()
plt.savefig(dataset_dir+'/rpe_rot_error.pdf')
plt.show()
svo_analysis/scripts/filter_groundtruth_smooth.py
+60
View File
@@ -0,0 +1,60 @@
#!/usr/bin/python
import numpy as np
import matplotlib.pyplot as plt
import transformations
from scipy import signal
save = True
data_filename = '/home/cforster/Datasets/SlamBenchmark/asl_vicon_d2/groundtruth.txt'
filtered_data_filename = '/home/cforster/Datasets/SlamBenchmark/asl_vicon_d2/groundtruth_filtered.txt'
file = open(data_filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
D = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
n = np.shape(D)[0]
rpy = np.empty([n,3])
for i in range(n):
quat = D[i,4:8]
rpy[i,:] = transformations.euler_from_quaternion(quat, axes='sxyz')
# filter rpy
f_sensor = 200.0; # sampling frequency in hz
f_cut = 15.0; # cutoff frequency in hz
b,a = signal.butter(5,f_cut/(f_sensor/2));
print a
print b
rpy_filt = np.empty([n,3])
rpy_filt[:,0] = signal.filtfilt(b, a, rpy[:,0])
rpy_filt[:,1] = signal.filtfilt(b, a, rpy[:,1])
rpy_filt[:,2] = signal.filtfilt(b, a, rpy[:,2])
fig = plt.figure()
ax = fig.add_subplot(111, title='orientation filtered')
ax.plot(rpy[:,0], 'r-')
ax.plot(rpy[:,1], 'g-')
ax.plot(rpy[:,2], 'b-')
ax.plot(rpy_filt[:,0], 'k-', linewidth=2)
ax.plot(rpy_filt[:,1], 'k-', linewidth=2)
ax.plot(rpy_filt[:,2], 'k-', linewidth=2)
fig = plt.figure()
ax = fig.add_subplot(111, title='position')
ax.plot(D[:,1], 'r')
ax.plot(D[:,2], 'g')
ax.plot(D[:,3], 'b')
fig = plt.figure()
ax = fig.add_subplot(111, title='trajectory from top')
ax.plot(D[:,1], D[:,2])
if save:
f = open(filtered_data_filename,'w')
for i in range(np.shape(D)[0]):
quat = transformations.quaternion_from_euler(rpy_filt[i,0], rpy_filt[i,1], rpy_filt[i,2], axes='sxyz')
f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (D[i,0], D[i,1], D[i,2], D[i,3], quat[0], quat[1], quat[2], quat[3]))
f.close()
svo_analysis/scripts/hand_eye_calib.py
+287
View File
@@ -0,0 +1,287 @@
# -*- coding: utf-8 -*-
"""
Created on Sun Aug 4 15:47:55 2013
@author: cforster
"""
import yaml
import numpy as np
import associate
import rotation_utils as ru
import matplotlib.pyplot as plt
def alignSim3(model, data, n):
# select the first n datapoints and remove its mean so their center is zero
M = model[0:n,:]
D = data[0:n,:]
# substract mean
mu_M = M.mean(0)
mu_D = D.mean(0)
M_zerocentered = M - mu_M
D_zerocentered = D - mu_D
# correlation
C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
D_svd = np.diag(D_svd)
V_svd = np.transpose(V_svd)
S = np.eye(3)
if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
S[2,2] = -1
R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
t = mu_M-s*np.dot(R,mu_D)
return s, R, t
def matrixLog(A):
theta = np.arccos((np.trace(A)-1.0)/2.0)
log_theta = 0.5*theta/np.sin(theta) * (A - A.transpose())
x = np.array([log_theta[2,1], log_theta[0,2], log_theta[1,0]])
return x
def handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
''' Implementation of the least squares solution described in the paper:
Robot Sensor Calibration: Solving AX=XB on the Euclidean Group
by Frank C. Park and Bryan J. Martin
'''
n = np.shape(I)[0]
M = np.zeros([3,3])
C = np.zeros([3*n, 3])
b_A = np.zeros([3*n,1])
b_B = np.zeros([3*n,1])
for ix, i in enumerate(I):
A1 = ru.quat2dcm(q_es[i,:])
A2 = ru.quat2dcm(q_es[i+delta,:])
A = np.dot(A1.transpose(), A2)
B1 = ru.quat2dcm(q_gt[i,:])
B2 = ru.quat2dcm(q_gt[i+delta,:])
B = np.dot(B1.transpose(), B2)
alpha = matrixLog(A)
beta = matrixLog(B)
M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha))
C[3*ix:3*ix+3,:] = np.eye(3) - A
b_A[3*ix:3*ix+3,0] = np.dot(np.transpose(A1), p_es[i+delta,:]-p_es[i,:])
b_B[3*ix:3*ix+3,0] = np.dot(np.transpose(B1), p_gt[i+delta,:]-p_gt[i,:])
# compute rotation
D,V = np.linalg.linalg.eig(np.dot(M.transpose(), M))
Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])])
Vinv = np.linalg.linalg.inv(V)
X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose())))
# compute translation
d = np.zeros([3*n,1])
for i in range(n):
d[3*i:3*i+3,:] = b_A[3*i:3*i+3,:] - np.dot(X, b_B[3*i:3*i+3,:])
b = np.dot(np.linalg.inv(np.dot(np.transpose(C),C)), np.dot(np.transpose(C),d))
return np.array(X),b
# user config
display = True
dataset = '20130906_2149_ptam_i7_asl2'
n_measurements = 1000
delta = 20
# load dataset parameters
dataset_dir = '../results/'+dataset
params_stream = open(dataset_dir+'/params.yaml')
params = yaml.load(params_stream)
# set trajectory groundtruth and estimate file
traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
traj_estimate = dataset_dir+'/traj_estimate.txt'
# load dataset params
dataset_param_file_stream = open(params['dataset_directory']+'/dataset_params.yaml','r')
dataset_param = yaml.load(dataset_param_file_stream)
# load data
data_gt = associate.read_file_list(traj_groundtruth)
data_es = associate.read_file_list(traj_estimate)
# select matches
#offset = -dataset_param['cam_delay']
offset = -0.03
matches = associate.associate(data_gt, data_es, offset, 0.02)
#matches = matches[0:470]
p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches])
q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches])
p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches])
# --------------------------------------------------------------------------------
# align Sim3 to get scale
scale,rot,trans = alignSim3(p_gt, p_es, np.shape(matches)[0])
p_es = scale*p_es
#p_es = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans
print 's='+str(scale)
#print 't='+str(trans)
#print 'R='+str(rot)
# plot sim3 aligned trajectory
if display:
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal')
ax.plot(p_es[:,0], p_es[:,1], 'r-', label='estimate')
ax.plot(p_gt[:,0], p_gt[:,1], 'b-', label='groundtruth')
ax.legend()
# --------------------------------------------------------------------------------
# hand-eye-calib
# select random measurements
I = np.array(rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0]
R,b = handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta, True)
print 'quat = ' + str(ru.dcm2quat(R))
print 'b = ' + str(b)
rpy_es = np.zeros([q_es.shape[0]-1, 3])
rpy_gt = np.zeros([q_gt.shape[0]-1, 3])
t_gt = np.zeros([q_es.shape[0]-1,3])
t_es = np.zeros([q_es.shape[0]-1,3])
for i in range(delta,np.shape(q_es)[0]):
A1 = ru.quat2dcm(q_es[i-delta,:])
A2 = ru.quat2dcm(q_es[i,:])
A = np.dot(A1.transpose(), A2)
B1 = ru.quat2dcm(q_gt[i-delta,:])
B2 = ru.quat2dcm(q_gt[i,:])
B = np.dot(B1.transpose(), B2)
B_es = np.dot(np.transpose(R), np.dot(A, R))
rpy_gt[i-delta,:] = ru.dcm2rpy(B)
rpy_es[i-delta,:] = ru.dcm2rpy(B_es)
t_B = np.dot(np.transpose(B1),(p_gt[i,:]-p_gt[i-delta,:]))
t_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:]))
t_gt[i-delta,:] = t_B
t_es[i-delta,:] = np.dot(np.transpose(R), np.dot(A,b[:,0]) + t_A - b[:,0])
alignment_error = (t_gt-t_es)
error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),1))
I_accurate = np.argwhere(error < np.percentile(error, 90))[:,0]
if display:
plt.figure()
plt.plot(rpy_es[:,0], 'r-', label='es roll')
plt.plot(rpy_es[:,1], 'g-', label='es pitch')
plt.plot(rpy_es[:,2], 'b-', label='es yaw')
plt.plot(rpy_gt[:,0], 'r--', label='gt roll')
plt.plot(rpy_gt[:,1], 'g--', label='gt pitch')
plt.plot(rpy_gt[:,2], 'b--', label='gt yaw')
plt.legend()
plt.figure()
plt.plot(t_gt[:,0], 'r-', label='gt x')
plt.plot(t_gt[:,1], 'g-', label='gt y')
plt.plot(t_gt[:,2], 'b-', label='gt z')
plt.plot(t_es[:,0], 'r--', label='es x')
plt.plot(t_es[:,1], 'g--', label='es y')
plt.plot(t_es[:,2], 'b--', label='es z')
plt.legend()
plt.figure()
plt.plot(error,'-g')
print 'e_rms = ' + str(np.sqrt(np.dot(error,error) / len(error)))
print 'e_mean = ' + str(np.mean(error))
print 'e_median = ' + str(np.median(error))
print 'e_std = ' + str(np.std(error))
# now sample again from the filtered list:
N = 10
display = False
n_acc_meas = I_accurate.size
n_measurements = 500
#for i in range(5):
# print '-------------------------------------'
# i0 = np.array(rand(n_measurements,1)*np.shape(I_accurate)[0], dtype=int)[:,0]
# i1 = np.minimum(i0+N, n_acc_meas-1)
# I = np.empty(i0.size*2, dtype=int)
# I[0::2] = I_accurate[i0]
# I[1::2] = I_accurate[i1]
# R,b = handEyeCalib(q_gt[I,:], q_es[I,:], p_gt[I,:], p_es[I,:], True)
# print 'quat = ' + str(ru.dcm2quat(R))
# print 'b = ' + str(b)
# rpy_es = np.zeros([q_es.shape[0]-1, 3])
# rpy_gt = np.zeros([q_gt.shape[0]-1, 3])
# t_gt = np.zeros([q_es.shape[0]-1,3])
# t_es = np.zeros([q_es.shape[0]-1,3])
#
# delta = 10
# for i in range(delta,np.shape(q_es)[0]):
# A1 = ru.quat2dcm(q_es[i-delta,:])
# A2 = ru.quat2dcm(q_es[i,:])
# A = np.dot(A1.transpose(), A2)
# B1 = ru.quat2dcm(q_gt[i-delta,:])
# B2 = ru.quat2dcm(q_gt[i,:])
# B = np.dot(B1.transpose(), B2)
# B_es = np.dot(np.transpose(R), np.dot(A, R))
# rpy_gt[i-delta,:] = ru.dcm2rpy(B)
# rpy_es[i-delta,:] = ru.dcm2rpy(B_es)
# t_B = np.dot(np.transpose(B1),(p_gt[i,:]-p_gt[i-delta,:]))
# t_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:]))
# t_gt[i-delta,:] = t_B
# t_es[i-delta,:] = np.dot(np.transpose(R), np.dot(A,b[:,0]) + t_A - b[:,0])
# alignment_error = (t_gt-t_es)
# error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),1))
#
# if display:
# plt.figure()
# plt.plot(rpy_es[:,0], 'r-', label='es roll')
# plt.plot(rpy_es[:,1], 'g-', label='es pitch')
# plt.plot(rpy_es[:,2], 'b-', label='es yaw')
# plt.plot(rpy_gt[:,0], 'r--', label='gt roll')
# plt.plot(rpy_gt[:,1], 'g--', label='gt pitch')
# plt.plot(rpy_gt[:,2], 'b--', label='gt yaw')
# plt.legend()
# plt.figure()
# plt.plot(t_gt[:,0], 'r-', label='es x')
# plt.plot(t_gt[:,1], 'g-', label='es y')
# plt.plot(t_gt[:,2], 'b-', label='es z')
# plt.plot(t_es[:,0], 'r--', label='gt x')
# plt.plot(t_es[:,1], 'g--', label='gt y')
# plt.plot(t_es[:,2], 'b--', label='gt z')
# plt.legend()
# plt.figure()
# plt.plot(error,'-g')
#
# print 'e_rms = ' + str(np.sqrt(np.dot(error,error) / len(error)))
# print 'e_mean = ' + str(np.mean(error))
# print 'e_median = ' + str(np.median(error))
# print 'e_std = ' + str(np.std(error))
svo_analysis/scripts/rotation_utils.py
+143
View File
@@ -0,0 +1,143 @@
# -*- coding: utf-8 -*-
"""
Created on Fri Jun 28 10:42:37 2013
@author: cforster
"""
import math
import numpy as np
# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0
# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
def quat2dcm(quaternion):
"""Convert Quaternion [x y z w] to Direct Cosine Matrix representation """
q = np.array(quaternion[:4], dtype=np.float64, copy=True)
nq = np.dot(q, q)
if nq < _EPS:
return np.identity(4)
q *= math.sqrt(2.0 / nq)
q = np.outer(q, q)
return np.array([ [1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3]],
[ q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3]],
[ q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1]]],
dtype=np.float64)
def dcm2quat(matrix):
"""Convert Direct Cosine Matrix to Quaternion
This algorithm comes from "Quaternion Calculus and Fast Animation",
Ken Shoemake, 1987 SIGGRAPH course notes
"""
q = np.empty((4, ), dtype=np.float64)
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
t = np.trace(M)
if (t > 0):
t = math.sqrt(t + 1.0);
q[3] = 0.5*t;
t = 0.5/t;
q[0] = (M[2,1] - M[1,2]) * t;
q[1] = (M[0,2] - M[2,0]) * t;
q[2] = (M[1,0] - M[0,1]) * t;
else:
i = 0;
if (M[1,1] > M[0,0]):
i = 1;
if (M[2,2] > M[i,i]):
i = 2;
j = (i+1)%3;
k = (j+1)%3;
t = math.sqrt(M[i,i]-M[j,j]-M[k,k] + 1.0);
q[i] = 0.5 * t;
t = 0.5/t;
q[3] = (M[k,j]-M[j,k])*t;
q[j] = (M[j,i]+M[i,j])*t;
q[k] = (M[k,i]+M[i,k])*t;
return q
def dcm2rpy(matrix, axes='sxyz'):
"""Return Euler angles from rotation matrix for specified axis sequence.
axes : One of 24 axis sequences as string or encoded tuple
Note that many Euler angle triplets can describe one matrix.
>>> R0 = euler_matrix(1, 2, 3, 'syxz')
>>> al, be, ga = euler_from_matrix(R0, 'syxz')
>>> R1 = euler_matrix(al, be, ga, 'syxz')
>>> numpy.allclose(R0, R1)
True
>>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
... R0 = euler_matrix(axes=axes, *angles)
... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
... if not numpy.allclose(R0, R1): print axes, "failed"
"""
try:
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
except (AttributeError, KeyError):
_ = _TUPLE2AXES[axes]
firstaxis, parity, repetition, frame = axes
i = firstaxis
j = _NEXT_AXIS[i+parity]
k = _NEXT_AXIS[i-parity+1]
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
if repetition:
sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
if sy > _EPS:
ax = math.atan2( M[i, j], M[i, k])
ay = math.atan2( sy, M[i, i])
az = math.atan2( M[j, i], -M[k, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2( sy, M[i, i])
az = 0.0
else:
cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
if cy > _EPS:
ax = math.atan2( M[k, j], M[k, k])
ay = math.atan2(-M[k, i], cy)
az = math.atan2( M[j, i], M[i, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2(-M[k, i], cy)
az = 0.0
if parity:
ax, ay, az = -ax, -ay, -az
if frame:
ax, az = az, ax
return ax, ay, az
def quat2rpy(quaternion, axes='sxyz'):
"""Return Euler angles from quaternion for specified axis sequence.
>>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
>>> numpy.allclose(angles, [0.123, 0, 0])
True
"""
return dcm2rpy(quat2dcm(quaternion), axes)
svo_analysis/scripts/trajectory_comparison.py
+166
View File
@@ -0,0 +1,166 @@
# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt
import yaml
from matplotlib import rc
from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes
from mpl_toolkits.axes_grid1.inset_locator import mark_inset
def plotTrajectory(ax, filename, label, color, linewidth):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
trajectory = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
ax.plot(trajectory[:,1], trajectory[:,2], label=label, color=color, linewidth=linewidth)
if __name__ == '__main__':
results_dir = '../results'
experiments = ['20130911_2113_nslam_i7_asl2_acc',
'20130911_2229_nslam_i7_asl2_fast',
'20130906_2149_ptam_i7_asl2']
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
# ------------------------------------------------------------------------------
# position error
fig_poserr = plt.figure(figsize=(8,6))
ax_poserr_x = fig_poserr.add_subplot(311, ylabel='x-error [m]')
ax_poserr_y = fig_poserr.add_subplot(312, ylabel='y-error [m]')
ax_poserr_z = fig_poserr.add_subplot(313, ylabel='z-error [m]', xlabel='time [s]')
for exp in experiments:
# load dataset parameters
params_stream = open(results_dir+'/'+exp+'/params.yaml')
params = yaml.load(params_stream)
# plot translation error
trans_error_filename = results_dir+'/'+exp+'/translation_error.txt'
file = open(trans_error_filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
trans_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
ax_poserr_x.plot(trans_error[:,0], trans_error[:,1], label=params['experiment_label'])
ax_poserr_y.plot(trans_error[:,0], trans_error[:,2])
ax_poserr_z.plot(trans_error[:,0], trans_error[:,3])
ax_poserr_x.set_xlim([0, trans_error[-1,0]+4])
ax_poserr_y.set_xlim([0, trans_error[-1,0]+4])
ax_poserr_z.set_xlim([0, trans_error[-1,0]+4])
ax_poserr_x.legend(bbox_to_anchor=[0, 0], loc='lower left', ncol=3)
ax_poserr_x.grid()
ax_poserr_y.grid()
ax_poserr_z.grid()
fig_poserr.tight_layout()
fig_poserr.savefig('../results/translation_error.pdf')
# ------------------------------------------------------------------------------
# orientation error
fig_roterr = plt.figure(figsize=(8,6))
ax_roterr_r = fig_roterr.add_subplot(311, ylabel='roll-error [rad]')
ax_roterr_p = fig_roterr.add_subplot(312, ylabel='pitch-error [rad]')
ax_roterr_y = fig_roterr.add_subplot(313, ylabel='yaw-error [rad]', xlabel='time [s]')
for exp in experiments:
# load dataset parameters
params_stream = open(results_dir+'/'+exp+'/params.yaml')
params = yaml.load(params_stream)
# plot translation error
rot_error_filename = results_dir+'/'+exp+'/orientation_error.txt'
file = open(rot_error_filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
rot_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
ax_roterr_r.plot(rot_error[:,0], rot_error[:,3], label=params['experiment_label'])
ax_roterr_p.plot(rot_error[:,0], rot_error[:,2])
ax_roterr_y.plot(rot_error[:,0], rot_error[:,1])
ax_roterr_r.set_xlim([0, rot_error[-1,0]+4])
ax_roterr_p.set_xlim([0, rot_error[-1,0]+4])
ax_roterr_y.set_xlim([0, rot_error[-1,0]+4])
ax_roterr_r.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
ax_roterr_r.grid()
ax_roterr_p.grid()
ax_roterr_y.grid()
fig_roterr.tight_layout()
fig_roterr.savefig('../results/orientation_error.pdf')
# ------------------------------------------------------------------------------
# scale error
fig_scale = plt.figure(figsize=(8,2.5))
ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]')
for exp in experiments:
# load dataset parameters
params_stream = open(results_dir+'/'+exp+'/params.yaml')
params = yaml.load(params_stream)
# plot translation error
scale_drift_filename = results_dir+'/'+exp+'/scale_drift.txt'
file = open(scale_drift_filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
scale_drift = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label'])
ax_scale.set_xlim([0, rot_error[-1,0]+4])
ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
ax_scale.grid()
fig_scale.tight_layout()
fig_scale.savefig('../results/scale_drift.pdf')
# ------------------------------------------------------------------------------
# trajectory
fig_traj = plt.figure(figsize=(8,4.8))
ax_traj = fig_traj.add_subplot(111, xlabel='x [m]', ylabel='y [m]', aspect='equal', xlim=[-3.1, 4], ylim=[-1.5, 2.6])
plotTrajectory(ax_traj, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
plotTrajectory(ax_traj, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
plotTrajectory(ax_traj, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
# show detail:
axins = zoomed_inset_axes(ax_traj, 5.0, loc=1) # zoom = 6
plotTrajectory(axins, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
plotTrajectory(axins, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
plotTrajectory(axins, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
axins.set_xlim(1.2, 1.70)
axins.set_ylim(-0.8, -0.3)
axins.patch.set_facecolor('0.9')
plt.xticks(visible=False)
plt.yticks(visible=False)
mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b')
plt.draw()
plt.show()
ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3)
ax_traj.grid()
fig_traj.tight_layout()
fig_traj.savefig('../results/trajectory_asl.pdf')
# ------------------------------------------------------------------------------
# number of matches
# plot number of klt tracked and matched points
# fig = plt.figure(figsize=(8,2))
# ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features', ylim=[0,220])
# ax.plot(np.arange(len(n_fts_1))/25.0, n_fts_1, 'b-', label='Accurate')
# ax.plot(np.arange(len(n_fts_2))/25.0, n_fts_2, 'g-', label='Fast')
# ax.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=2)
# fig.tight_layout()
# fig.savefig('../results/number_of_features2.pdf', bbox_inches="tight")
#
svo_analysis/scripts/transformations.py
+1705
View File
File diff suppressed because it is too large Load Diff
svo_msgs/CMakeLists.txt
+35
View File
@@ -0,0 +1,35 @@
cmake_minimum_required(VERSION 2.8.3)
project(svo_msgs)
# search for everything we need to build the messages, dont forget the message_generation
find_package(catkin REQUIRED COMPONENTS
message_generation
geometry_msgs
sensor_msgs
)
# search for all msg files
FILE(GLOB messages_to_build RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/msg"
"${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg")
# notify catkin to look at the previously found msg files
add_message_files(
FILES
${messages_to_build}
)
# build the header files from the msg files, and notify catkin about the dependencies
generate_messages(
DEPENDENCIES
geometry_msgs
sensor_msgs
)
# export the dependencis of this package for who ever depends on us
catkin_package(
CATKIN_DEPENDS message_runtime geometry_msgs
)
include_directories(
${catkin_INCLUDE_DIRS}
)
svo_msgs/msg/DenseInput.msg
+6
View File
@@ -0,0 +1,6 @@
Header header
uint32 frame_id
geometry_msgs/Pose pose
sensor_msgs/Image image
float32 min_depth
float32 max_depth
svo_msgs/msg/Feature.msg
+5
View File
@@ -0,0 +1,5 @@
float32 u
float32 v
float32 x
float32 y
float32 z
svo_msgs/msg/Info.msg
+6
View File
@@ -0,0 +1,6 @@
Header header
float32 processing_time
int32[] keyframes
int32 num_matches
int32 tracking_quality
int32 stage
svo_msgs/msg/NbvTrajectory.msg
+2
View File
@@ -0,0 +1,2 @@
Header header
geometry_msgs/Pose[] trajectory
svo_msgs/package.xml
+19
View File
@@ -0,0 +1,19 @@
<?xml version="1.0"?>
<package>
<name>svo_msgs</name>
<version>0.0.0</version>
<description>Custom messages for svo</description>
<maintainer email="[email protected]">Christian Forster</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>message_runtime</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<export></export>
</package>
svo_ros/CMakeLists.txt
+62
View File
@@ -0,0 +1,62 @@
SET(PROJECT_NAME svo_ros)
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/")
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()
# Add catkin and required ROS packages
FIND_PACKAGE(catkin REQUIRED COMPONENTS
roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo svo_msgs vikit_common vikit_ros
)
# Add plain cmake packages
FIND_PACKAGE(OpenCV REQUIRED)
FIND_PACKAGE(Eigen REQUIRED)
FIND_PACKAGE(Sophus REQUIRED)
FIND_PACKAGE(Boost REQUIRED COMPONENTS thread signals)
# 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
INCLUDE_DIRS include
LIBRARIES svo_visualizer
)
# Include dirs
INCLUDE_DIRECTORIES(
include
${Eigen_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# Set link libraries
LIST(APPEND LINK_LIBS
${OpenCV_LIBRARIES}
${Sophus_LIBRARIES}
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
# Create visualizer library
ADD_LIBRARY(svo_visualizer src/visualizer.cpp)
TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS})
# Create Executables
ADD_EXECUTABLE(vo src/vo_node.cpp)
TARGET_LINK_LIBRARIES(vo svo_visualizer)
ADD_EXECUTABLE(benchmark src/benchmark_node.cpp)
TARGET_LINK_LIBRARIES(benchmark svo_visualizer)
ADD_EXECUTABLE(remote src/remote_node.cpp)
TARGET_LINK_LIBRARIES(remote svo_visualizer)
svo_ros/include/svo_ros/dataset_img.h
+62
View File
@@ -0,0 +1,62 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef DATASET_IMG_H
#define DATASET_IMG_H
namespace FileType {
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
using ::Eigen::Quaterniond;
using ::Eigen::Vector3d;
/// Specification of Dataset which contains: Camera Image, Gyroscope Rotation
class DatasetImg
{
public:
DatasetImg() {}
virtual ~DatasetImg() {}
double stamp_;
std::string imgname_;
friend std::ostream& operator <<(std::ostream& out, const DatasetImg& pair);
friend std::istream& operator >>(std::istream& in, DatasetImg& pair);
};
std::ostream& operator <<(std::ostream& out, const DatasetImg& gt)
{
out
<< gt.stamp_ << " "
<< gt.imgname_ << " "
<< std::endl;
return out;
}
std::istream& operator >>(std::istream& in, DatasetImg& gt)
{
in >> gt.stamp_;
in >> gt.imgname_;
return in;
}
} // end namespace FileType
#endif /* DATASET_IMG_H */
svo_ros/include/svo_ros/dataset_imu.h
+70
View File
@@ -0,0 +1,70 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef DATASET_IMU_H_
#define DATASET_IMU_H_
namespace FileType {
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
using ::Eigen::Quaterniond;
using ::Eigen::Vector3d;
/// Specification of Dataset which contains: Camera Image, Gyroscope Rotation
class DatasetImu
{
public:
DatasetImu() {}
virtual ~DatasetImu() {}
double stamp_;
Quaterniond q_imu_;
friend std::ostream& operator <<(std::ostream& out, const DatasetImu& pair);
friend std::istream& operator >>(std::istream& in, DatasetImu& pair);
};
std::ostream& operator <<(std::ostream& out, const DatasetImu& gt)
{
out
<< gt.stamp_ << " "
<< gt.q_imu_.x() << " "
<< gt.q_imu_.y() << " "
<< gt.q_imu_.z() << " "
<< gt.q_imu_.w() << " "
<< std::endl;
return out;
}
std::istream& operator >>(std::istream& in, DatasetImu& gt)
{
in >> gt.stamp_;
double x,y,z,w;
in >> x;
in >> y;
in >> z;
in >> w;
gt.q_imu_ = Quaterniond(w, x, y, z);
return in;
}
} // end namespace FileType
#endif /* DATASET_IMU_H_ */
svo_ros/include/svo_ros/visualizer.h
+75
View File
@@ -0,0 +1,75 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_VISUALIZER_H_
#define SVO_VISUALIZER_H_
#include <queue>
#include <ros/ros.h>
#include <svo/global.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/ColorRGBA.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <boost/lexical_cast.hpp>
#include <tf/transform_broadcaster.h>
#include <image_transport/image_transport.h>
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <iostream>
namespace svo {
class Frame;
class Point;
class Map;
class FrameHandlerMono;
typedef boost::shared_ptr<Frame> FramePtr;
/// This class bundles all functions to publish visualisation messages,
/// trace data and output debugging information.
class Visualizer
{
public:
ros::NodeHandle pnh_;
size_t trace_id_;
size_t img_pub_level_;
size_t img_pub_nth_;
size_t dense_pub_nth_;
ros::Publisher pub_frames_;
ros::Publisher pub_points_;
ros::Publisher pub_pose_;
ros::Publisher pub_info_;
ros::Publisher pub_dense_;
image_transport::Publisher pub_images_;
tf::TransformBroadcaster br_;
Visualizer();
~Visualizer() {};
void visualizeMarkers(const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map);
void publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp);
void publishMapRegion(set<FramePtr> frames);
void removeDeletedPts(const Map& map);
void displayKeyframeWithMps(const FramePtr& frame, int ts);
void exportToDense(const FramePtr& frame);
};
} // end namespace svo
#endif /* SVO_VISUALIZER_H_ */
svo_ros/launch/live.launch
+25
View File
@@ -0,0 +1,25 @@
<launch>
<node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
<param name="cam_topic" value="/camera/image_raw" type="str" />
<rosparam file="$(find rpg_calib)/cameras/bluefox_847_cfo_atan.yaml" />
<rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
<param name="map_scale" value="0.5" />
<param name="init_rx" value="3.14" />
<param name="image_publishing_level" value="0" />
<param name="publish_every_nth_img" value="1" />
<param name="accept_console_user_input" value="True" />
<param name="publish_dense_input" value="True" />
<param name="publish_every_nth_dense_input" value="5" />
</node>
<!--
<node pkg="image_view" type="image_view" name="image_svo">
<remap from="image" to="/svo/image" />
</node>
-->
</launch>
svo_ros/launch/test_rig3.launch
+16
View File
@@ -0,0 +1,16 @@
<launch>
<node pkg="svo_ros" type="vo" name="svo" clear_params="true" launch-prefix="gdb -ex run --args" output="screen">
<param name="cam_topic" value="/camera/image_raw" type="str" />
<param name="cam_model" value="ATAN" type="str" />
<param name="cam_width" value="752" />
<param name="cam_height" value="480" />
<param name="cam_fx" value="0.509326" />
<param name="cam_fy" value="0.796651" />
<param name="cam_cx" value="0.459051" />
<param name="cam_cy" value="0.510056" />
<param name="cam_d0" value="0.93207" />
<param name="init_rx" value="3.14" />
<rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
</node>
<node name="svo_ros_rviz" pkg="rviz" type="rviz" args=" -d $(find svo_ros)/rviz_config.rviz" />
</launch>
svo_ros/package.xml
+42
View File
@@ -0,0 +1,42 @@
<?xml version="1.0"?>
<package>
<name>svo_ros</name>
<version>0.1.0</version>
<description>
Monocular Visual Odometry - ROS Nodes
</description>
<author>Christian Forster</author>
<maintainer email="[email protected]">Christian Forster</maintainer>
<license>GPLv3</license>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<build_depend>roscpp</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>tf</build_depend>
<build_depend>svo</build_depend>
<build_depend>svo_msgs</build_depend>
<build_depend>vikit_common</build_depend>
<build_depend>vikit_ros</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>tf</run_depend>
<run_depend>svo</run_depend>
<run_depend>svo_msgs</run_depend>
<run_depend>vikit_common</run_depend>
<run_depend>vikit_ros</run_depend>
</package>
svo_ros/param/vo_accurate.yaml
+4
View File
@@ -0,0 +1,4 @@
grid_size: 20
max_n_kfs: 50
loba_num_iter: 10
max_fts: 200
svo_ros/param/vo_fast.yaml
+3
View File
@@ -0,0 +1,3 @@
grid_size: 30
max_n_kfs: 10
loba_num_iter: 0
svo_ros/rqt.perspective
+293
View File
@@ -0,0 +1,293 @@
{
"keys": {},
"groups": {
"pluginmanager": {
"keys": {
"running-plugins": {
"type": "repr",
"repr": "{u'rqt_rviz/RViz': [1], u'rqt_reconfigure/Param': [1], u'rqt_image_view/ImageView': [1], u'svo_rqt/Svo': [1]}"
}
},
"groups": {
"plugin__rqt_rviz__RViz__1": {
"keys": {},
"groups": {
"dock_widget__": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
}
}
},
"plugin__rqt_robot_steering__RobotSteering__1": {
"keys": {},
"groups": {
"dock_widget__RobotSteeringUi": {
"keys": {
"dockable": {
"type": "repr",
"repr": "u'true'"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
},
"plugin": {
"keys": {
"topic": {
"type": "repr",
"repr": "u'/cmd_vel'"
},
"vx_min": {
"type": "repr",
"repr": "u'-1'"
},
"vw_min": {
"type": "repr",
"repr": "u'-3'"
},
"vw_max": {
"type": "repr",
"repr": "u'3'"
},
"vx_max": {
"type": "repr",
"repr": "u'1'"
}
},
"groups": {}
}
}
},
"plugin__rqt_reconfigure__Param__1": {
"keys": {},
"groups": {
"dock_widget___plugincontainer_top_widget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
},
"plugin": {
"keys": {
"splitter": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000c00000006401000000060100000001')",
"pretty-print": " d "
},
"_splitter": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000013e0000006401000000090100000002')",
"pretty-print": " > d "
}
},
"groups": {}
}
}
},
"plugin__rqt_image_view__ImageView__1": {
"keys": {},
"groups": {
"dock_widget__ImageViewWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
},
"plugin": {
"keys": {
"dynamic_range": {
"type": "repr",
"repr": "False"
},
"max_range": {
"type": "repr",
"repr": "10.0"
},
"topic": {
"type": "repr",
"repr": "u'/svo/image'"
},
"zoom1": {
"type": "repr",
"repr": "False"
}
},
"groups": {}
}
}
},
"plugin__svo_rqt__Svo__1": {
"keys": {},
"groups": {
"dock_widget__SvoWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
},
"dock_widget__SvoUi": {
"keys": {
"dockable": {
"type": "repr",
"repr": "u'true'"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
}
}
},
"plugin__rqt_shell__Shell__1": {
"keys": {},
"groups": {
"plugin": {
"keys": {
"shell_type": {
"type": "repr",
"repr": "u'0'"
}
},
"groups": {}
}
}
},
"plugin__rqt_console__Console__1": {
"keys": {},
"groups": {
"plugin": {
"keys": {
"show_highlighted_only": {
"type": "repr",
"repr": "u'false'"
},
"message_limit": {
"type": "repr",
"repr": "u'20000'"
},
"exclude_filters": {
"type": "repr",
"repr": "u'severity'"
},
"table_splitter": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000004e000000ed01000000090100000002')",
"pretty-print": " N "
},
"highlight_filters": {
"type": "repr",
"repr": "u'message'"
},
"paused": {
"type": "repr",
"repr": "u'false'"
},
"settings_exist": {
"type": "repr",
"repr": "u'true'"
},
"filter_splitter": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000720000007201000000090100000002')",
"pretty-print": " r r "
}
},
"groups": {
"exclude_filter_0": {
"keys": {
"enabled": {
"type": "repr",
"repr": "u'true'"
},
"itemlist": {
"type": "repr",
"repr": "u''"
}
},
"groups": {}
},
"highlight_filter_0": {
"keys": {
"regex": {
"type": "repr",
"repr": "u'false'"
},
"text": {
"type": "repr",
"repr": "u''"
},
"enabled": {
"type": "repr",
"repr": "u'true'"
}
},
"groups": {}
}
}
}
}
}
}
},
"mainwindow": {
"keys": {
"geometry": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00010000000007a9fffffffc00001187000005a7000007b1000000180000117f0000059f000000000000')",
"pretty-print": " "
},
"state": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000009cf00000559fc0100000005fb0000002c00730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f005500690100000000000003890000000000000000fb0000006a007200710074005f0072006f0062006f0074005f007300740065006500720069006e0067005f005f0052006f0062006f0074005300740065006500720069006e0067005f005f0031005f005f0052006f0062006f0074005300740065006500720069006e00670055006901000000000000020b0000000000000000fc00000000000003530000022e00fffffffc0200000004fb0000003400730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f0057006900640067006500740100000019000000a30000009900fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c006500570069006400670065007401000000c3000003b50000000000000000fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000c2000002700000005100fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f00770069006400670065007401000003380000023a0000010f00fffffffb00000026007200710074005f007200760069007a005f005f005200560069007a005f005f0031005f005f010000035900000676000000a700fffffffb00000040007200710074005f007300680065006c006c005f005f005300680065006c006c005f005f0031005f005f0058005400650072006d005700690064006700650074010000091c000000b30000000000000000000009cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
"pretty-print": " ,svo_rqt__Svo__1__SvoUi jrqt_robot_steering__RobotSteering__1__RobotSteeringUi S . p Q 8 : Y v "
}
},
"groups": {
"toolbar_areas": {
"keys": {
"MinimizedDockWidgetsToolbar": {
"type": "repr",
"repr": "8"
}
},
"groups": {}
}
}
}
}
}
svo_ros/rviz_config.rviz
+153
View File
@@ -0,0 +1,153 @@
Panels:
- Class: rviz/Displays
Help Height: 108
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /TF1
- /Marker1
- /Marker2
- /Marker2/Namespaces1
- /Image1
Splitter Ratio: 0.5
Tree Height: 680
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Image
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Class: rviz/Marker
Enabled: true
Marker Topic: /svo/keyframes
Name: Marker
Namespaces:
cams: true
kfs: true
Queue Size: 100
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /svo/points
Name: Marker
Namespaces:
pts: true
trajectory: true
Queue Size: 100
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /svo/image
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Value: true
Enabled: true
Global Options:
Background Color: 255; 255; 255
Fixed Frame: /world
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 5.48911
Focal Point:
X: 0.777511
Y: 0.689927
Z: -0.445833
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.125204
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.45215
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1416
Hide Left Dock: false
Hide Right Dock: true
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000002cc00000542fc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000355000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000383000001e70000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000006fd0000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 2511
X: 1962
Y: 24
svo_ros/src/benchmark_node.cpp
+155
View File
@@ -0,0 +1,155 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <vector>
#include <string>
#include <iostream>
#include <sophus/se3.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <opencv2/opencv.hpp>
#include <vikit/file_reader.h>
#include <vikit/params_helper.h>
#include <vikit/camera_loader.h>
#include <vikit/abstract_camera.h>
#include <vikit/user_input_thread.h>
#include <svo/config.h>
#include <svo/frame.h>
#include <svo/map.h>
#include <svo/frame_handler_mono.h>
#include <svo_ros/visualizer.h>
#include <svo_ros/dataset_img.h>
namespace svo {
class BenchmarkNode
{
svo::FrameHandlerMono* vo_;
svo::Visualizer visualizer_;
vk::UserInputThread user_input_thread_;
int frame_count_;
std::ofstream trace_est_pose_;
vk::AbstractCamera* cam_;
bool quit_;
public:
BenchmarkNode(ros::NodeHandle& nh);
~BenchmarkNode();
void runCamImuBenchmark();
void processUserActions();
};
BenchmarkNode::BenchmarkNode(ros::NodeHandle& nh) :
vo_(NULL),
frame_count_(0),
quit_(false)
{
// Create Camera
if(!vk::camera_loader::loadFromRosNs("svo", cam_))
throw std::runtime_error("Camera model not correctly specified.");
// Initialize VO
vo_ = new svo::FrameHandlerMono(cam_);
vo_->start();
}
BenchmarkNode::~BenchmarkNode()
{
delete vo_;
delete cam_;
}
void BenchmarkNode::runCamImuBenchmark()
{
// create image reader and load dataset
std::string benchmark_dir(vk::getParam<std::string>("svo/dataset_directory"));
std::string filename_benchmark(benchmark_dir + "/images.txt");
vk::FileReader<FileType::DatasetImg> dataset_reader(filename_benchmark);
dataset_reader.skipComments();
if(!dataset_reader.next())
std::runtime_error("Failed to open images file");
std::vector<FileType::DatasetImg> dataset;
dataset_reader.readAllEntries(dataset);
// create pose tracefile
std::string trace_est_name(Config::traceDir() + "/traj_estimate.txt");
trace_est_pose_.open(trace_est_name.c_str());
if(trace_est_pose_.fail())
throw std::runtime_error("ERROR:frame_count_ could not create tracefile. probably the specified folder does not exist.");
// process dataset
for(vk::vector<FileType::DatasetImg>::iterator it = dataset.begin();
it != dataset.end(); ++it, ++frame_count_)
{
// Read image
std::string img_filename(benchmark_dir + "/" + it->imgname_);
cv::Mat img(cv::imread(img_filename, 0));
if(frame_count_ == 0)
SVO_INFO_STREAM("Reading image "<<img_filename);
if(img.empty())
throw std::runtime_error("Image could not be loaded.");
// Add image to VO
processUserActions();
vo_->addImage(img, it->stamp_);
// Visualize
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->stamp_);
if(vo_->stage() != svo::FrameHandlerMono::PAUSED)
visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
// write pose to tracefile
if(vo_->stage() == svo::FrameHandlerMono::DEFAULT_FRAME)
{
Quaterniond q(vo_->lastFrame()->T_f_w_.inverse().unit_quaternion());
Vector3d p(vo_->lastFrame()->T_f_w_.inverse().translation());
trace_est_pose_.precision(15);
trace_est_pose_.setf(std::ios::fixed, std::ios::floatfield );
trace_est_pose_ << it->stamp_<< " ";
trace_est_pose_.precision(6);
trace_est_pose_ << p.x() << " " << p.y() << " " << p.z() << " "
<< q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
}
if (!ros::ok() || quit_)
break;
}
}
void BenchmarkNode::processUserActions()
{
char input = user_input_thread_.getInput();
switch(input)
{
case 'q': quit_ = true; break;
case 'r': vo_->reset(); break;
case 's': vo_->start(); break;
default: ;
}
}
} // namespace svo
int main(int argc, char** argv)
{
ros::init(argc, argv, "svo");
ros::NodeHandle nh;
{
svo::BenchmarkNode benchmark(nh);
benchmark.runCamImuBenchmark();
}
printf("BenchmarkNode finished.\n");
return 0;
}
svo_ros/src/remote_node.cpp
+169
View File
@@ -0,0 +1,169 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <map>
#include <set>
#include <ros/ros.h>
#include <vikit/user_input_thread.h>
#include <vikit/ringbuffer.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <svo_msgs/Info.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <vikit/output_helper.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
class RemoteHandler
{
public:
ros::NodeHandle& nh_;
vk::UserInputThread input_thread_;
ros::Publisher pub_key_;
ros::Publisher pub_frames_;
tf::TransformBroadcaster br_;
vk::RingBuffer<double> acc_timing_;
vk::RingBuffer<int> acc_obs_;
typedef std::map<size_t, std::pair<Eigen::Vector3d, Eigen::Matrix3d> > pose_map_t;
pose_map_t poses_;
std::set<int> published_kfs_;
std::set<int> current_kfs_;
RemoteHandler(ros::NodeHandle& nh);
~RemoteHandler();
void handleUserInput();
void poseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr msg);
void infoCb(const svo_msgs::InfoConstPtr msg);
};
RemoteHandler::RemoteHandler(ros::NodeHandle& nh) :
nh_(nh),
pub_key_(nh_.advertise<std_msgs::String> ("/svo/remote_key", 10)),
pub_frames_(nh_.advertise<visualization_msgs::Marker>("/svo/vis/frame", 100)),
acc_timing_(50),
acc_obs_(50)
{}
RemoteHandler::~RemoteHandler()
{}
void RemoteHandler::handleUserInput()
{
char key = input_thread_.getInput();
std_msgs::StringPtr msg(new std_msgs::String);
switch(key)
{
case 'q': msg->data = 'q'; pub_key_.publish(msg); break; // quit
case 'r': msg->data = 'r'; pub_key_.publish(msg); break; // reset
case 'p': msg->data = 'p'; pub_key_.publish(msg); break; // pause
case 's': msg->data = 's'; pub_key_.publish(msg); break; // start
default: ;
}
}
void RemoteHandler::poseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr msg)
{
Eigen::Vector3d p(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
Eigen::Quaterniond q(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
Eigen::Matrix3d R_world_from_frame(q.inverse());
Eigen::Vector3d p_frame_in_world(-R_world_from_frame*p);
poses_.insert(std::pair<size_t, std::pair<Eigen::Vector3d, Eigen::Matrix3d> >(
msg->header.seq,
std::pair<Eigen::Vector3d, Eigen::Matrix3d>(p_frame_in_world, R_world_from_frame)));
// publish trajectory
vk::output_helper::publishPointMarker(pub_frames_, p_frame_in_world, "trajectory_points",
msg->header.stamp, msg->header.seq, 0, 0.006, Eigen::Vector3d(0.,0.,0.5));
// publish camera pose tf
tf::StampedTransform transform_msg;
transform_msg.frame_id_ = "cam";
transform_msg.child_frame_id_ = "world";
transform_msg.stamp_ = msg->header.stamp;
transform_msg.setOrigin(tf::Vector3(p.x(), p.y(), p.z()));
transform_msg.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w()));
br_.sendTransform(transform_msg);
// publish keyframes
std::set<int> deleted_kfs;
std::set_difference(published_kfs_.begin(), published_kfs_.end(), current_kfs_.begin(), current_kfs_.end(), std::inserter(deleted_kfs, deleted_kfs.end()));
for(std::set<int>::iterator it=deleted_kfs.begin(); it!=deleted_kfs.end(); ++it)
{
pose_map_t::iterator kf = poses_.find(*it);
if(kf != poses_.end())
vk::output_helper::publishFrameMarker(pub_frames_, kf->second.second, kf->second.first, "kfs", ros::Time::now(), (*it)*10, 2, 0.015);
}
std::set<int> unpublished_kfs;
std::set_difference(current_kfs_.begin(), current_kfs_.end(), published_kfs_.begin(), published_kfs_.end(), std::inserter(unpublished_kfs, unpublished_kfs.end()));
for(std::set<int>::iterator it=unpublished_kfs.begin(); it!=unpublished_kfs.end(); ++it)
{
pose_map_t::iterator kf = poses_.find(*it);
if(kf != poses_.end())
vk::output_helper::publishFrameMarker(pub_frames_, kf->second.second, kf->second.first, "kfs", ros::Time::now(), (*it)*10, 0, 0.015);
}
published_kfs_.swap(current_kfs_);
// publish hexacopter
vk::output_helper::publishHexacopterMarker(pub_frames_, "cam", "hexacopter", msg->header.stamp, 1, 0, 0.3, Eigen::Vector3d(0.,0.,1.));
}
void RemoteHandler::infoCb(const svo_msgs::InfoConstPtr msg)
{
std::string kfmsg("");
current_kfs_.clear();
for(std::vector<int32_t>::const_iterator it=msg->keyframes.begin(); it!=msg->keyframes.end(); ++it) {
current_kfs_.insert(static_cast<int>(*it));
if(*it == static_cast<int32_t>(msg->header.seq)) {
kfmsg = "Keyframe!";
break;
}
}
acc_timing_.push_back(msg->processing_time);
acc_obs_.push_back(msg->num_matches);
printf("[%i] t=%.2fhz (mean=%.2f) \t n_obs=%i (mean=%i) \t %s\n",
msg->header.seq, 1./msg->processing_time, 1./acc_timing_.getMean(),
msg->num_matches, acc_obs_.getMean(), kfmsg.c_str());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "svo_remote");
ros::NodeHandle nh;
RemoteHandler node(nh);
printf("Listening to User-Input...\n");
ros::Subscriber sub_pose = nh.subscribe("/svo/pose", 10, &RemoteHandler::poseCb, &node);
ros::Subscriber sub_info = nh.subscribe("/svo/info", 10, &RemoteHandler::infoCb, &node);
ros::Rate r(100);
while (ros::ok())
{
node.handleUserInput();
ros::spinOnce();
r.sleep();
}
return 0;
}
svo_ros/src/visualizer.cpp
+268
View File
@@ -0,0 +1,268 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <svo_ros/visualizer.h>
#include <svo/frame_handler_mono.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/map.h>
#include <svo/feature.h>
#include <svo_msgs/Info.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/tf.h>
#include <iostream>
#include <fstream>
#include <ros/package.h>
#include <vikit/timer.h>
#include <vikit/output_helper.h>
#include <vikit/params_helper.h>
#include <deque>
#include <algorithm>
#include <svo_msgs/DenseInput.h>
namespace svo {
Visualizer::
Visualizer() :
pnh_("~"),
trace_id_(0),
img_pub_level_(vk::getParam<int>("svo/image_publishing_level", 0)),
img_pub_nth_(vk::getParam<int>("svo/publish_every_nth_img", 1)),
dense_pub_nth_(vk::getParam<int>("svo/publish_every_nth_dense_input", 1))
{
// Init ROS Marker Publishers
pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
pub_info_ = pnh_.advertise<svo_msgs::Info>("info", 10);
pub_dense_ = pnh_.advertise<svo_msgs::DenseInput>("dense_input",10);
// create video publisher
image_transport::ImageTransport it(pnh_);
pub_images_ = it.advertise("image", 10);
}
void Visualizer::
visualizeMarkers(const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map)
{
if(frame != NULL)
{
// publish camera pose tf
vk::output_helper::publishTfTransform(frame->T_f_w_, ros::Time(frame->timestamp_), "cam_pos", "world", br_);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
{
vk::output_helper::publishHexacopterMarker(pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_), 1, 0, 0.3, Vector3d(0.,0.,1.));
vk::output_helper::publishPointMarker(pub_points_, frame->pos(), "trajectory", ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
publishMapRegion(core_kfs);
removeDeletedPts(map);
}
}
}
void Visualizer::
publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp)
{
++trace_id_;
std_msgs::Header header_msg;
header_msg.frame_id = "/cam";
header_msg.seq = trace_id_;
header_msg.stamp = ros::Time(timestamp);
// publish svo info msgs
if(pub_info_.getNumSubscribers() > 0)
{
svo_msgs::Info msg_info;
msg_info.header = header_msg;
msg_info.processing_time = slam.lastProcessingTime();
msg_info.keyframes.resize(slam.map().keyframes_.size());
for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
msg_info.keyframes.push_back((*it)->id_);
msg_info.stage = static_cast<int>(slam.stage());
msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
if(frame != NULL)
msg_info.num_matches = slam.lastNumObservations();
else
msg_info.num_matches = 0;
pub_info_.publish(msg_info);
}
// publish pyramid-image every nth-frame
if(frame != NULL && img_pub_nth_ > 0
&& trace_id_%img_pub_nth_ == 0 && pub_images_.getNumSubscribers() > 0)
{
const int scale = (1<<img_pub_level_);
cv::Mat img_rgb(frame->img_pyr_[img_pub_level_].size(), CV_8UC3);
cv::cvtColor(frame->img_pyr_[img_pub_level_], img_rgb, CV_GRAY2RGB);
// during initialization, draw lines
if(slam.stage() == FrameHandlerBase::SECOND_FRAME)
{
const vector<cv::Point2f>& px_ref(slam.initFeatureTrackRefPx());
const vector<cv::Point2f>& px_cur(slam.initFeatureTrackCurPx());
for(vector<cv::Point2f>::const_iterator it_ref=px_ref.begin(), it_cur=px_cur.begin();
it_ref != px_ref.end(); ++it_ref, ++it_cur)
cv::line(img_rgb,
cv::Point2f(it_cur->x/scale, it_cur->y/scale),
cv::Point2f(it_ref->x/scale, it_ref->y/scale), cv::Scalar(0,255,0), 2);
}
// draw features on frame
if(img_pub_level_ == 0)
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]-2, (*it)->px[1]-2),
cv::Point2f((*it)->px[0]+2, (*it)->px[1]+2),
cv::Scalar(0,255,0), CV_FILLED);
if(img_pub_level_ == 1)
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]/scale-1, (*it)->px[1]/scale-1),
cv::Point2f((*it)->px[0]/scale+1, (*it)->px[1]/scale+1),
cv::Scalar(0,255,0), CV_FILLED);
else
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
cv::Scalar(0,255,0), CV_FILLED);
// publish image
cv_bridge::CvImage img_msg;
img_msg.header = header_msg;
img_msg.image = img_rgb;
img_msg.encoding = sensor_msgs::image_encodings::BGR8;
pub_images_.publish(img_msg.toImageMsg());
}
else if(frame == NULL && pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::PAUSED)
{
// display image when slam is not running
cv_bridge::CvImage img_msg;
img_msg.header.stamp = ros::Time::now();
img_msg.header.frame_id = "/image";
img_msg.image = img;
img_msg.encoding = sensor_msgs::image_encodings::MONO8;
pub_images_.publish(img_msg.toImageMsg());
}
// publish pose (world in camera frame)
if(pub_pose_.getNumSubscribers() > 0 && frame != NULL)
{
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
Quaterniond q(frame->T_f_w_.rotation_matrix());
Vector3d p(frame->T_f_w_.translation());
msg_pose->header = header_msg;
msg_pose->pose.pose.position.x = p[0];
msg_pose->pose.pose.position.y = p[1];
msg_pose->pose.pose.position.z = p[2];
msg_pose->pose.pose.orientation.x = q.x();
msg_pose->pose.pose.orientation.y = q.y();
msg_pose->pose.pose.orientation.z = q.z();
msg_pose->pose.pose.orientation.w = q.w();
for(size_t i=0; i<36; ++i)
msg_pose->pose.covariance[i] = frame->Cov_(i%6, i/6);
pub_pose_.publish(msg_pose);
}
}
void Visualizer::
publishMapRegion(set<FramePtr> frames)
{
if(pub_points_.getNumSubscribers() > 0)
{
int ts = vk::Timer::getCurrentTime();
for(set<FramePtr>::iterator it=frames.begin(); it!=frames.end(); ++it)
displayKeyframeWithMps(*it, ts);
}
}
void Visualizer::
removeDeletedPts(const Map& map)
{
if(pub_points_.getNumSubscribers() > 0)
{
for(list<Point*>::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it)
vk::output_helper::publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d());
}
}
void Visualizer::
displayKeyframeWithMps(const FramePtr& frame, int ts)
{
// publish keyframe
vk::output_helper::publishFrameMarker(pub_frames_, frame->T_f_w_.inverse().rotation_matrix(), frame->pos(), "kfs", ros::Time::now(), frame->id_*10, 0, 0.015);
// publish point cloud and links
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
if((*it)->point->last_published_ts_ == ts)
continue;
vk::output_helper::publishPointMarker(pub_points_, (*it)->point->pos_, "pts", ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0));
(*it)->point->last_published_ts_ = ts;
}
}
void Visualizer::
exportToDense(const FramePtr& frame)
{
// publish air_ground_msgs
if(frame != NULL && dense_pub_nth_ > 0
&& trace_id_%dense_pub_nth_ == 0 && pub_dense_.getNumSubscribers() > 0)
{
svo_msgs::DenseInput msg;
msg.header.stamp = ros::Time(frame->timestamp_);
msg.header.frame_id = "/world";
msg.frame_id = frame->id_;
cv_bridge::CvImage img_msg;
img_msg.header.stamp = msg.header.stamp;
img_msg.header.frame_id = "camera";
img_msg.image = frame->img();
img_msg.encoding = sensor_msgs::image_encodings::MONO8;
msg.image = *img_msg.toImageMsg();
double min_z = std::numeric_limits<double>::max();
double max_z = std::numeric_limits<double>::min();
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point==NULL)
continue;
Vector3d pos = frame->T_f_w_*(*it)->point->pos_;
min_z = min(pos[2], min_z);
max_z = max(pos[2], max_z);
}
msg.min_depth = (float) min_z;
msg.max_depth = (float) max_z;
Quaterniond q(frame->T_f_w_.inverse().rotation_matrix());
Vector3d p(frame->T_f_w_.inverse().translation());
msg.pose.position.x = p[0];
msg.pose.position.y = p[1];
msg.pose.position.z = p[2];
msg.pose.orientation.w = q.w();
msg.pose.orientation.x = q.x();
msg.pose.orientation.y = q.y();
msg.pose.orientation.z = q.z();
pub_dense_.publish(msg);
}
}
} // end namespace svo
svo_ros/src/vo_node.cpp
+181
View File
@@ -0,0 +1,181 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <ros/package.h>
#include <string>
#include <svo/frame_handler_mono.h>
#include <svo/map.h>
#include <svo/config.h>
#include <svo_ros/visualizer.h>
#include <vikit/params_helper.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <boost/thread.hpp>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Core>
#include <vikit/abstract_camera.h>
#include <vikit/camera_loader.h>
#include <vikit/user_input_thread.h>
namespace svo {
using namespace std;
using namespace vk;
/// SVO Interface
class VoNode
{
public:
svo::FrameHandlerMono* vo_;
svo::Visualizer visualizer_;
bool publish_markers_; //!< publish only the minimal amount of info (choice for embedded devices)
bool publish_dense_input_;
vk::UserInputThread* user_input_thread_;
ros::Subscriber sub_remote_key_;
string remote_input_;
vk::AbstractCamera* cam_;
bool quit_;
VoNode();
~VoNode();
void imgCb(const sensor_msgs::ImageConstPtr& msg);
void processUserActions();
void remoteKeyCb(const std_msgs::StringConstPtr& key_input);
};
VoNode::
VoNode() :
vo_(NULL),
publish_markers_(vk::getParam<bool>("svo/publish_markers", true)),
publish_dense_input_(vk::getParam<bool>("svo/publish_dense_input", false)),
user_input_thread_(new vk::UserInputThread()),
remote_input_(""),
cam_(NULL),
quit_(false)
{
// Create Camera
if(!vk::camera_loader::loadFromRosNs("svo", cam_))
throw std::runtime_error("Camera model not correctly specified.");
// Init camera
vo_ = new svo::FrameHandlerMono(cam_);
// Set initial position and orientation
Sophus::SE3 T_cam_from_world(
vk::rpy2dcm(Vector3d(vk::getParam<double>("svo/init_rx", 0.0),
vk::getParam<double>("svo/init_ry", 0.0),
vk::getParam<double>("svo/init_rz", 0.0))),
Eigen::Vector3d(vk::getParam<double>("svo/init_tx", 0.0),
vk::getParam<double>("svo/init_ty", 0.0),
vk::getParam<double>("svo/init_tz", 0.0)));
vo_->setInitialPose(T_cam_from_world);
vo_->start();
}
VoNode::
~VoNode()
{
delete vo_;
delete user_input_thread_;
delete cam_;
}
void VoNode::
imgCb(const sensor_msgs::ImageConstPtr& msg)
{
cv::Mat img;
try {
img = cv_bridge::toCvShare(msg, "mono8")->image;
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
}
processUserActions();
vo_->addImage(img, msg->header.stamp.toSec());
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
if(publish_markers_ && vo_->stage() != FrameHandlerBase::PAUSED)
visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
if(publish_dense_input_)
visualizer_.exportToDense(vo_->lastFrame());
if(vo_->stage() == FrameHandlerMono::PAUSED)
usleep(100000); // avoid busy loop when paused
}
void VoNode::processUserActions()
{
char input = remote_input_.c_str()[0];
remote_input_ = "";
char console_input = user_input_thread_->getInput();
if(console_input != 0)
input = console_input;
switch(input)
{
case 'q':
quit_ = true;
printf("Svo User Input: QUIT\n");
break;
case 'r':
vo_->reset();
printf("Svo User Input: RESET\n");
break;
case 's':
vo_->start();
printf("Svo User Input: START\n");
break;
default: ;
}
}
void VoNode::remoteKeyCb(const std_msgs::StringConstPtr& key_input)
{
remote_input_ = key_input->data;
}
} // namespace svo
int main(int argc, char **argv)
{
ros::init(argc, argv, "svo");
ros::NodeHandle nh;
std::cout << "create vo_node" << std::endl;
svo::VoNode vo_node;
// subscribe to cam msgs
std::string cam_topic(vk::getParam<std::string>("svo/cam_topic", "camera/image_raw"));
image_transport::ImageTransport it(nh);
image_transport::Subscriber it_sub = it.subscribe(cam_topic, 5, &svo::VoNode::imgCb, &vo_node);
// subscribe to remote input
vo_node.sub_remote_key_ = nh.subscribe("svo/remote_key", 5, &svo::VoNode::remoteKeyCb, &vo_node);
// start processing callbacks
while(ros::ok() && !vo_node.quit_)
{
ros::spinOnce();
// TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
}
printf("Svo terminated.\n");
return 0;
}